// -*- 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", "1",
    "conf.default.CaribFile", "ncm_cameraparam.xml",
		"conf.default.Magnification","1.00",
		"conf.default.CaptureWindow","on",
		"conf.default.USBdevID","1",
    ""
  };
// </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 OutPort buffer
  registerOutPort("ImageData", m_ImageDataOut);
}

CameraEye::~CameraEye()
{
}



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

  // <rtc-template block="bind_config">
  bindParameter("Flip", m_Flip, "1");
  bindParameter("CaribFile", m_caribFile, "ncm_cameraparam.xml");
  bindParameter("Magnification", m_magnification, "1.00");
	bindParameter("CaptureWindow", m_captureWindow, "on");
	bindParameter("USBdevID", m_usbID, "1");


	cap = NULL;
	cap_window = NULL;
  // </rtc-template>
  /* カメラの取得 */
  //ここはカメラ種類の検索もできれば自動でやりたいところ

  //cap = cvCreateCameraCapture(CV_CAP_IEEE1394); //IEEE1394カメラ
  //cap = cvCreateCameraCapture(CV_CAP_V4L + m_usbID);//UVC対応のUSBカメラ

  //if(cap == NULL){
  //  printf("Camera Not Found!\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;
	//NM33の場合は歪み補正後の画像を得るのでキャリブレーション関係は省略できる．

  //キャプチャ画像のウィンドウを定義
  //err = cvNamedWindow("OriginalWnd", CV_WINDOW_AUTOSIZE);

  //歪み補正のためのカメラパラメータを生成

  //carib.yml形式に対応バージョン
  //load_calibration(m_caribFile);
  //run_calibration();

  //ncm_cameraparam.xml形式に対応バージョン
  CvFileStorage *fs;
  CvFileNode *param; 
  fs = cvOpenFileStorage ("ncm_cameraparam.xml", 0, CV_STORAGE_READ);
  param = cvGetFileNodeByName (fs, NULL, "intrinsic"); 
  m_camera_matrix = (CvMat *) cvRead (fs, param);//cvUndistort2で使用
  param = cvGetFileNodeByName (fs, NULL, "distortion");
  m_dist_coeffs = (CvMat *) cvRead (fs, param);//cvUndistort2で使用
  cvReleaseFileStorage (&fs);

  return RTC::RTC_OK;
}


RTC::ReturnCode_t CameraEye::onFinalize()
{
	if(cap != NULL)
		cvReleaseCapture(&cap);
	return RTC::RTC_OK;
}


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

RTC::ReturnCode_t CameraEye::onActivated(RTC::UniqueId ec_id)
{
	if(cap == NULL)
		cap = cvCreateCameraCapture(CV_CAP_V4L + m_usbID);

	strcpy(captureWindow,m_captureWindow.c_str());

	if(strcmp("off", captureWindow) == 0)
		window_flag = 0;
	else if(strcmp("on", captureWindow) == 0)
		window_flag = 1;
	else{
		printf("configuration error: CaptureWindow= %s\n", captureWindow);
		fflush(stdout);
		return RTC::RTC_ERROR;
	}
	if(cap_window == NULL && window_flag == 1){
  	cap_window = cvNamedWindow("OriginalWnd", CV_WINDOW_AUTOSIZE);
	}
  return RTC::RTC_OK;
}



RTC::ReturnCode_t CameraEye::onDeactivated(RTC::UniqueId ec_id)
{
	cap_window = NULL;
  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);
	//NM33のカメラを使うときは歪み補正なし
	//cvUndistort2(img, tempImage, m_camera_matrix, m_dist_coeffs);
	//反転
	flipImage = cvCloneImage(tempImage);
	if(m_Flip == -1 || m_Flip == 0 || m_Flip == 1){//NM33のときは1,ncmのときは2とか
		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;
      }
    }
  }
  */

	if(window_flag == 1)
  	cvShowImage("OriginalWnd", flipImage);
  //キー入力待機 - 待機が短いとcvShowImageが効かない
  int key;
  key = cvWaitKey(20);
  #if 1
  //画像キャプチャ
  if(key != -1){
    int		err;
    char	imgFileName[1024];

    //保存ファイル名生成
    memset(imgFileName, 0, sizeof(imgFileName));
    sprintf(imgFileName, "capImage_%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;
}


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


