// -*- C++ -*-
/*!
 * @file  CaptureTimedCameraImage.cpp
 * @brief Camera Capture Module using OpenCV
 * @date $Date$
 *
 * $Id$
 */

#include <rtm/SystemLogger.h>
#include "CaptureTimedCameraImage.h"

// Module specification
// <rtc-template block="module_spec">
static const char* capturetimedcameraimage_spec[] =
  {
    "implementation_id", "CaptureTimedCameraImage",
    "type_name",         "CaptureTimedCameraImage",
    "description",       "Camera Capture Module using OpenCV",
    "version",           "0.2.1",
    "vendor",            "Tsuyoshi Suenaga, NAIST",
    "category",          "camera",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.camera_id", "0",
    "conf.default.output_color_format", "RGB",
    "conf.default.camera_param_filename", "camera.yml",
    "conf.default.undistortion_flag", "0",
    "conf.default.cap_continuous_flag", "1",
    // Widget
    "conf.__widget__.camera_id", "text",
    "conf.__widget__.output_color_format", "text",
    "conf.__widget__.camera_param_filename", "text",
    "conf.__widget__.undistortion_flag", "radio",
    "conf.__widget__.cap_continuous_flag", "radio",
    // Constraints
    "conf.__constraints__.output_color_format", "(GRAY,RGB)",
    "conf.__constraints__.undistortion_flag", "(0,1)",
    "conf.__constraints__.cap_continuous_flag", "(0,1)",
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
CaptureTimedCameraImage::CaptureTimedCameraImage(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_imageOut("Image", m_image),
    m_CameraCaptureServicePortPort("CameraCaptureServicePort")

    // </rtc-template>
{
}

/*!
 * @brief destructor
 */
CaptureTimedCameraImage::~CaptureTimedCameraImage()
{
}



RTC::ReturnCode_t CaptureTimedCameraImage::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  addOutPort("Image", m_imageOut);
  
  // Set service provider to Ports
  m_CameraCaptureServicePortPort.registerProvider("CameraCaptureService", "Img::CameraCaptureService", m_CameraCaptureService);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  addPort(m_CameraCaptureServicePortPort);
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("camera_id", m_camera_id, "0");
  bindParameter("output_color_format", m_output_color_format, "RGB");
  bindParameter("camera_param_filename", m_camera_param_filename, "camera.yml");
  bindParameter("undistortion_flag", m_undistortion_flag, "0");
  bindParameter("cap_continuous_flag", m_cap_continuous_flag, "1");
  
  // </rtc-template>
  return RTC::RTC_OK;
}


RTC::ReturnCode_t CaptureTimedCameraImage::onFinalize()
{
  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t CaptureTimedCameraImage::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

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

/*!
 * カメラデバイスのオープン、カメラパラメータの読み込み
 * を実行する．
 */

RTC::ReturnCode_t CaptureTimedCameraImage::onActivated(RTC::UniqueId ec_id)
{
  std::cout << "Capture init" << std::endl;

  // setup camera device
  if(!cap.open(m_camera_id)) {
    RTC_ERROR(("Unable to open video device. [%d]", m_camera_id));
    return RTC::RTC_ERROR;
  }

  // get width and height
  cap >> cap_img;
  width = cap_img.cols;
  height = cap_img.rows;
  depth = cap_img.depth();

  std::cout << "Capture image size: " << width << "x" << height << std::endl;
  std::cout << "DEPTH: " << depth << std::endl;
  std::cout << "CHANNEL: " << cap_img.channels() << std::endl;
  RTC_TRACE(("Capture image size %d x %d", width, height));
  RTC_TRACE(("Depth %d, Channels %d", depth, cap_img.channels()));

  // check channels
  channels = (m_output_color_format == "GRAY") ? 1 : 3;
  if(channels > cap_img.channels()){
    if(m_output_color_format == "RGB") {
      std::cout << "CONVERT: GRAY -> RGB" << std::endl;
    }
  }else if(channels < cap_img.channels()){
    std::cout << "CONVERT: BGR -> GRAY" << std::endl;
  }else{
    if(m_output_color_format == "RGB")
      std::cout << "CONVERT: BGR -> RGB" << std::endl;
    if(m_output_color_format == "GRAY")
      std::cout << "OUTPUT: GRAY" << std::endl;
  }

#ifdef _WITH_GUI
  // create debug window
  cv::namedWindow("capture", CV_WINDOW_AUTOSIZE);
#endif
  
  // load camera parameter file
  std::cout << "Load camera parameter from " << m_camera_param_filename << std::endl;
  cv::FileStorage fs(m_camera_param_filename, cv::FileStorage::READ);
  if (!fs.isOpened()) {
    RTC_ERROR(("Unable to open camera parameter file. [%s]",
               m_camera_param_filename.c_str()));
    std::cerr << "Could not open camera parameter." << std::endl;
    return RTC::RTC_ERROR;
  }

#if CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >= 2
  fs["image_width"] >> cam_param.imageSize.width;
  fs["image_height"] >> cam_param.imageSize.height;
  fs["camera_matrix"] >> cam_param.cameraMatrix;
  fs["distortion_coefficients"] >> cam_param.distCoeffs;
#else  // CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >= 2
  cv::FileNode node(fs.fs, NULL);
  cam_param.imageSize.width = node["image_width"];
  cam_param.imageSize.height = node["image_height"];
  cv::FileNode fn = node["camera_matrix"];
  cv::read(fn, cam_param.cameraMatrix);
  fn = node["distortion_coefficients"];
  cv::read(fn, cam_param.distCoeffs);
#endif  // CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >= 2

  std::cout << "image size = " << cam_param.imageSize.width << " x " << cam_param.imageSize.height << std::endl;
#if CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >= 2
  std::cout << "camera matrix = " << cam_param.cameraMatrix << std::endl;
  std::cout << "distortion coefficients = " << cam_param.distCoeffs << std::endl;
#endif
  std::cout << std::endl;

  // set undistort rectify map
  CameraParam *cp = &cam_param;
#if CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >= 2
  cv::initUndistortRectifyMap(cp->cameraMatrix, cp->distCoeffs, cv::Mat(),
                              getOptimalNewCameraMatrix(cp->cameraMatrix, cp->distCoeffs, cp->imageSize, 1, cp->imageSize, 0),
                              cp->imageSize, CV_16SC2, cp->map1, cp->map2);
#else
  cv::initUndistortRectifyMap(cp->cameraMatrix, cp->distCoeffs, cv::Mat(),
                              cp->cameraMatrix,
                              cp->imageSize, CV_16SC2, cp->map1, cp->map2);
#endif

  // set default continuous_flag
  m_CameraCaptureService.cap_continuous = m_cap_continuous_flag;

  std::cout << "Capture start..." << std::endl;

  return RTC::RTC_OK;
}

/*!
 * カメラデバイスのクローズを実行する．
 */

RTC::ReturnCode_t CaptureTimedCameraImage::onDeactivated(RTC::UniqueId ec_id)
{
  std::cout << "Capture stop" << std::endl;

  // release camera device
  cap.release();
#ifdef _WITH_GUI
  // destroy debug window
#if CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >= 2
  cv::destroyWindow("capture");
#endif // CV_MAJOR_VERSION, CV_MINOR_VERSION
#endif // _WITH_GUI
  return RTC::RTC_OK;
}

/*!
 * 画像のキャプチャを実行する．コンフィギュレーションパ
 * ラメータにより，レンズ歪補正を合わせて実行する．
 */

RTC::ReturnCode_t CaptureTimedCameraImage::onExecute(RTC::UniqueId ec_id)
{
  int i;
  cv::Mat image, rimage;

  if(m_CameraCaptureService.cap_continuous == true) {
    {;}  // do capture process
  } else if(m_CameraCaptureService.cap_count > 0) {
    --m_CameraCaptureService.cap_count;
  } else {
    RTC_DEBUG(("Waiting capture command from continuous_flag or Service Port."))
    return RTC::RTC_OK;
  }

  // image capture
  cap >> cap_img;
  if(cap_img.empty()){
    RTC_WARN(("Image data is empty."))
    return RTC::RTC_OK;
  }

  setTimestamp(m_image);
  m_image.data.captured_time = m_image.tm;

  // color conversion
  if(channels > cap_img.channels()){
    if(m_output_color_format == "RGB")
      cv::cvtColor(cap_img, nChannels_img, CV_GRAY2RGB);
    image = nChannels_img;
  }else if(channels < cap_img.channels()){
    cv::cvtColor(cap_img, nChannels_img, CV_BGR2GRAY);
    image = nChannels_img;
  }else{
    if(m_output_color_format == "RGB"){
      cv::cvtColor(cap_img, nChannels_img, CV_BGR2RGB);
      image = nChannels_img;
    }else{
      image = cap_img;
    }
  }

  // copy camera intrinsic matrix to Data Port
  m_image.data.intrinsic.matrix_element[0] = cam_param.cameraMatrix.at<double>(0, 0);
  m_image.data.intrinsic.matrix_element[1] = cam_param.cameraMatrix.at<double>(0, 1);
  m_image.data.intrinsic.matrix_element[2] = cam_param.cameraMatrix.at<double>(1, 1);
  m_image.data.intrinsic.matrix_element[3] = cam_param.cameraMatrix.at<double>(0, 2);
  m_image.data.intrinsic.matrix_element[4] = cam_param.cameraMatrix.at<double>(1, 2);
  m_image.data.intrinsic.distortion_coefficient.length(cam_param.distCoeffs.rows);

  // undistortion
  if(m_undistortion_flag == 1) {
    //cv::remap(image, rimage, cam_param.map1, cam_param.map2, cv::INTER_LINEAR);
    cv::undistort(image, rimage, cam_param.cameraMatrix, cam_param.distCoeffs, cam_param.cameraMatrix);

    // copy distortion coefficient to Data Port
    for(int j = 0; j < 5; ++j){
      m_image.data.intrinsic.distortion_coefficient[j] = 0.0;
    }
    image = rimage;
  }else{
    // copy distortion coefficient to Data Port
    for(int j = 0; j < 5; ++j){
      m_image.data.intrinsic.distortion_coefficient[j] = cam_param.distCoeffs.at<double>(0, j);
    }
  }

  // copy TimedCameraImage data to Data Port
  m_image.data.image.width = width;
  m_image.data.image.height = height;
  m_image.data.image.format
    = (m_output_color_format == "RGB") ? Img::CF_RGB :
      (m_output_color_format == "GRAY") ? Img::CF_GRAY : Img::CF_UNKNOWN;
  m_image.data.image.raw_data.length(width * height * channels);
  for(i=0; i<height; ++i){
    memcpy(&m_image.data.image.raw_data[i*width*channels],
           &image.data[i*image.step],
           sizeof(unsigned char)*width*channels);
  }

  // send TimedCameraImage data
  m_imageOut.write();

#ifdef _WITH_GUI
  // show image
  if(m_output_color_format == "RGB")
    cv::cvtColor(image, image, CV_RGB2BGR);
  cv::imshow("capture", image);
  cv::waitKey(5);
#endif

  return RTC::RTC_OK;
}

/*!
 * カメラデバイスのクローズ、パラメータのリセットを実行
 * する．
 */

RTC::ReturnCode_t CaptureTimedCameraImage::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}

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

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

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

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



extern "C"
{
 
  void CaptureTimedCameraImageInit(RTC::Manager* manager)
  {
    coil::Properties profile(capturetimedcameraimage_spec);
    manager->registerFactory(profile,
                             RTC::Create<CaptureTimedCameraImage>,
                             RTC::Delete<CaptureTimedCameraImage>);
  }
  
};


