// -*- C++ -*-
/*!
 * @file  CameraEye.cpp
 * @brief CameraEye
 * @date $Date$
 *
 * $Id$
 */

#include "CameraEye.h"

// Module specification
// <rtc-template block="module_spec">
static const char* cameraeye_spec[] =
  {
    "implementation_id", "CameraEye",
    "type_name",         "CameraEye",
    "description",       "CameraEye",
    "version",           "1.0.0",
    "vendor",            "FSI",
    "category",          "Camera",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    "conf.default.Flip", "2",
    "conf.default.CaribFile", "carib.yml",
	"conf.default.Magnification","1.00",
    ""
  };
// </rtc-template>

double CameraEye::compute_reprojection_error( const CvMat* object_points,
        const CvMat* rot_vects, const CvMat* trans_vects,
        const CvMat* camera_matrix, const CvMat* dist_coeffs,
        const CvMat* image_points, const CvMat* point_counts,
        CvMat* per_view_errors )
{
    CvMat* image_points2 = cvCreateMat( image_points->rows,
        image_points->cols, image_points->type );
    int i, image_count = rot_vects->rows, points_so_far = 0;
    double total_err = 0, err;

    for( i = 0; i < image_count; i++ )
    {
        CvMat object_points_i, image_points_i, image_points2_i;
        int point_count = point_counts->data.i[i];
        CvMat rot_vect, trans_vect;

        cvGetCols( object_points, &object_points_i,
            points_so_far, points_so_far + point_count );
        cvGetCols( image_points, &image_points_i,
            points_so_far, points_so_far + point_count );
        cvGetCols( image_points2, &image_points2_i,
            points_so_far, points_so_far + point_count );
        points_so_far += point_count;

        cvGetRow( rot_vects, &rot_vect, i );
        cvGetRow( trans_vects, &trans_vect, i );

        cvProjectPoints2( &object_points_i, &rot_vect, &trans_vect,
                          camera_matrix, dist_coeffs, &image_points2_i,
                          0, 0, 0, 0, 0 );
        err = cvNorm( &image_points_i, &image_points2_i, CV_L1 );
        if( per_view_errors ){
            per_view_errors->data.db[i] = err/point_count;
        }
        total_err += err;
    }
    
    cvReleaseMat( &image_points2 );

    return total_err/points_so_far;
}


int CameraEye::load_calibration(/* CvSeq* image_points_seq, CvSize img_size, CvSize board_size,
                     float square_size, float aspect_ratio, int flags,
                     CvMat* camera_matrix, CvMat* dist_coeffs, CvMat** extr_params,
					 CvMat** reproj_errs, double* avg_reproj_err */std::string caribFile)
{
	CvFileStorage *fs;

	// キャリブレーションパラメタファイルを読み込む
	CvFileNode	*param;
	fs = cvOpenFileStorage (caribFile.c_str(), 0, CV_STORAGE_READ);

//    param = cvGetFileNodeByName( fs, NULL, "image_count");
//    image_count = cvReadInt(param, 0);
	param = cvGetFileNodeByName( fs, NULL, "image_width");
    m_img_size.width = cvReadInt(param, 0);
	param = cvGetFileNodeByName( fs, NULL, "image_height");
    m_img_size.height = cvReadInt(param, 0);
    param = cvGetFileNodeByName( fs, NULL, "board_width");
    m_board_size.width = cvReadInt(param, 0);
    param = cvGetFileNodeByName( fs, NULL, "board_height");
    m_board_size.height = cvReadInt(param, 0);
	param = cvGetFileNodeByName( fs, NULL, "square_size");
    m_square_size = cvReadReal(param, 0);

	param = cvGetFileNodeByName( fs, NULL, "aspect_ratio");
    m_aspect_ratio = cvReadReal(param, 0);

	param = cvGetFileNodeByName( fs, NULL, "flags");
    m_flags = cvReadInt(param, 0);

	param = cvGetFileNodeByName( fs, NULL, "camera_matrix");
	m_camera_matrix = (CvMat*)cvRead(fs, param, NULL);
    param = cvGetFileNodeByName( fs, NULL, "distortion_coefficients");
	m_dist_coeffs = (CvMat*)cvRead(fs, param, NULL);
	param = cvGetFileNodeByName( fs, NULL, "image_points");
//	image_points_mat = (CvMat*)cvRead(fs, param, NULL);
	m_image_points_seq = (CvSeq*)cvRead(fs, param, NULL);
	return 0;
}

int CameraEye::run_calibration(){
    int code;
    int image_count = m_image_points_seq->total;
    int point_count = m_board_size.width * m_board_size.height;
    CvMat* image_points = cvCreateMat( 1, image_count*point_count, CV_32FC2 );
    CvMat* object_points = cvCreateMat( 1, image_count*point_count, CV_32FC3 );
    CvMat* point_counts = cvCreateMat( 1, image_count, CV_32SC1 );
    CvMat rot_vects, trans_vects;
    int i, j, k;
    CvSeqReader reader;
    cvStartReadSeq( m_image_points_seq, &reader, 0 );

    // initialize arrays of points
    for( i = 0; i < image_count; i++ )
    {
        CvPoint2D32f* src_img_pt = (CvPoint2D32f*)reader.ptr;
        CvPoint2D32f* dst_img_pt = ((CvPoint2D32f*)image_points->data.fl) + i*point_count;
        CvPoint3D32f* obj_pt = ((CvPoint3D32f*)object_points->data.fl) + i*point_count;

        for( j = 0; j < m_board_size.height; j++ )
            for( k = 0; k < m_board_size.width; k++ )
            {
                *obj_pt++ = cvPoint3D32f(j * m_square_size, k * m_square_size, 0);
                *dst_img_pt++ = *src_img_pt++;
            }
        CV_NEXT_SEQ_ELEM( m_image_points_seq->elem_size, reader );
    }

    cvSet( point_counts, cvScalar(point_count) );

    m_extr_params = cvCreateMat( image_count, 6, CV_32FC1 );
    cvGetCols( m_extr_params, &rot_vects, 0, 3 );
    cvGetCols( m_extr_params, &trans_vects, 3, 6 );

    cvZero( m_camera_matrix );
    cvZero( m_dist_coeffs );

    if( m_flags & CV_CALIB_FIX_ASPECT_RATIO )
    {
        m_camera_matrix->data.db[0] = m_aspect_ratio;
        m_camera_matrix->data.db[4] = 1.;
    }

    cvCalibrateCamera2( object_points, image_points, point_counts,
                        m_img_size, m_camera_matrix, m_dist_coeffs,
                        &rot_vects, &trans_vects, m_flags );

    code = cvCheckArr( m_camera_matrix, CV_CHECK_QUIET ) &&
        cvCheckArr( m_dist_coeffs, CV_CHECK_QUIET ) &&
        cvCheckArr( m_extr_params, CV_CHECK_QUIET );

    m_reproj_errs = cvCreateMat( 1, image_count, CV_64FC1 );
#if 1
    m_avg_reproj_err =
        compute_reprojection_error( object_points, &rot_vects, &trans_vects,
            m_camera_matrix, m_dist_coeffs, image_points, point_counts, m_reproj_errs );
#endif

    cvReleaseMat( &object_points );
    cvReleaseMat( &image_points );
    cvReleaseMat( &point_counts );

    return code;
}


CameraEye::CameraEye(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_ImageDataOut("ImageData", m_ImageData),
    
    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  registerOutPort("ImageData", m_ImageDataOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

}

CameraEye::~CameraEye()
{
}



RTC::ReturnCode_t CameraEye::onInitialize()
{
	int		err;

  // <rtc-template block="bind_config">

  bindParameter("Flip", m_Flip, "2");
  bindParameter("CaribFile", m_caribFile, "carib.yml");
  bindParameter("Magnification", m_magnification, "1.00");

  // </rtc-template>
	/* カメラの検索＆取得 */
/*
#if 1
	cap = cvCreateCameraCapture(1);
#else
	cap = cvCaptureFromCAM(0);
#endif
*/
//	if(cap == NULL)
//		cap = cvCreateCameraCapture(CV_CAP_IEEE1394);
		cap = cvCreateCameraCapture(CV_CAP_V4L);
//		cap = cvCreateCameraCapture(0);

	if(cap == NULL){
		printf("Camera Not Fount!\n");
		return RTC::RTC_ERROR;
	}

  hikaku_img = cvLoadImage ("hikaku.png",  CV_LOAD_IMAGE_GRAYSCALE );
  if (!hikaku_img)
    {
      fprintf(stderr, "%s: LOAD ERROR\n", "hikaku.jpg");
     
    }

	/* キャプチャ画像のシリアル番号を初期化 */
	m_CapSerial = 1;

	err = cvNamedWindow("OriginalWnd", CV_WINDOW_AUTOSIZE);
	load_calibration(m_caribFile);
	run_calibration();

  return RTC::RTC_OK;
}


RTC::ReturnCode_t CameraEye::onFinalize()
{
	cvReleaseCapture(&cap);
	return RTC::RTC_OK;
}



RTC::ReturnCode_t CameraEye::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t CameraEye::onShutdown(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CameraEye::onActivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CameraEye::onDeactivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/


RTC::ReturnCode_t CameraEye::onExecute(RTC::UniqueId ec_id)
{
	unsigned char *imgStream;
	IplImage	*flipImage;
	IplImage	*tempImage;


	/* カメラからデータを取得 */
	img = cvQueryFrame(cap);
	if(img == NULL){
		printf("Not Frame\n");
	}

	tempImage = cvCloneImage(img);
	cvUndistort2(img, tempImage, m_camera_matrix, m_dist_coeffs);
	flipImage = cvCloneImage(tempImage);
	if(m_Flip == -1 || m_Flip == 0 || m_Flip == 1){

		cvFlip(tempImage, flipImage, m_Flip);
	}


//ここで画像サイズを調整しちゃうぞ！

 
  roi.width = flipImage->width/m_magnification;
  roi.height = flipImage->height/m_magnification; 
  roi.x = (int)(flipImage->width-roi.width)/2;
  roi.y = (int)(flipImage->height-roi.height)/2;
  cvSetImageROI (flipImage, roi);



	cvResize(flipImage,tempImage,CV_INTER_NN);
	cvReleaseImage(&flipImage);
	flipImage=cvCloneImage(tempImage);




	/* ストリームデータをCORBA領域に出力 */
	imgStream = (unsigned char*)flipImage;
	m_ImageData.data.length(sizeof(IplImage) + flipImage->imageSize);

	memcpy(&(m_ImageData.data[0]), (unsigned char*)flipImage, sizeof(IplImage));
	memcpy(&(m_ImageData.data[sizeof(IplImage)]), flipImage->imageData, flipImage->imageSize);

	m_ImageDataOut.write();


//比較画像をtempImageに貼り付け
  for (int y = 0; y < tempImage->height; y++) {
    for (int x = 0; x < tempImage->width; x++) {

if(cvGetReal2D(hikaku_img,y,x)>250){
      /* 画素値を直接操作する一例 */
      tempImage->imageData[tempImage->widthStep * y + x * 3] = 255;
      tempImage->imageData[tempImage->widthStep * y + x * 3 + 1] = 255;
      tempImage->imageData[tempImage->widthStep * y + x * 3 + 2] = 255;
    }}
  }



	cvShowImage("OriginalWnd", tempImage);
	/* キー入力待機・・・待機が短いと何も表示されない */
	int key;
	key = cvWaitKey(20);
#if 1
	if(key != -1){
		int		err;
		char	imgFileName[1024];
		
		/* 保存ファイル名生成 */
		memset(imgFileName, 0, sizeof(imgFileName));
		sprintf(imgFileName, "RTC-CENTER_%d.jpeg", m_CapSerial);

		/* 画像をキャプチャ */
		err = cvSaveImage(imgFileName, flipImage);
		m_CapSerial++;
		printf("キャプチャ完了 (%s)\n", imgFileName);
	}
#endif
	cvReleaseImage(&flipImage);
	cvReleaseImage(&tempImage);
	return RTC::RTC_OK;
}



RTC::ReturnCode_t CameraEye::onAborting(RTC::UniqueId ec_id)
{
	printf("OnAborting Call!!\n");
  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t CameraEye::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CameraEye::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CameraEye::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CameraEye::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/



extern "C"
{
 
  void CameraEyeInit(RTC::Manager* manager)
  {
    RTC::Properties profile(cameraeye_spec);
    manager->registerFactory(profile,
                             RTC::Create<CameraEye>,
                             RTC::Delete<CameraEye>);
  }
  
};


