// -*- C++ -*-
/*!
 * @file  ARMarkerDetectSimple.cpp
 * @brief AR Marker Detection Module using ARToolkit
 * @date $Date$
 *
 * @author Tsuyoshi Suenaga, NAIST
 * tsuyo-s@is.naist.jp
 *
 * GNU General Public License
 *
 * $Id$
 */

#include "ARMarkerDetectSimple.h"

// Module specification
// <rtc-template block="module_spec">
static const char* armarkerdetectsimple_spec[] =
  {
    "implementation_id", "ARMarkerDetectSimple",
    "type_name",         "ARMarkerDetectSimple",
    "description",       "AR Marker Detection Module using ARToolkit",
    "version",           "0.2.0",
    "vendor",            "Tsuyoshi Suenaga, NAIST",
    "category",          "marker",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "0",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.marker_info_filename", "Data/markers_info.dat",
    "conf.default.default_detect_marker_id", "0",
    "conf.default.camera_parameter_filename", "Data/camera_para.dat",
    "conf.default.continuous_flag", "1",
    // Widget
    "conf.__widget__.marker_info_filename", "text",
    "conf.__widget__.default_detect_marker_id", "text",
    "conf.__widget__.camera_parameter_filename", "text",
    "conf.__widget__.continuous_flag", "radio",
    // Constraints
    "conf.__constraints__.continuous_flag", "(0,1)",
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
ARMarkerDetectSimple::ARMarkerDetectSimple(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_imageIn("Image", m_image),
    m_imageAROut("ImageAR", m_imageAR),
    m_markerPoseAllOut("MarkerPoseAll", m_markerPoseAll),
    m_markerPoseOut("MarkerPose", m_markerPose),
    m_CameraCaptureServicePortPort("CameraCaptureServicePort"),
    m_ObjectRecognitionPortPort("ObjectRecognitionPort")

    // </rtc-template>
{
}

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

void ARMarkerDetectSimple::logCameraParameter(void) {
  RTC_TRACE(("ARToolKit perspective matrix"));
  for (int i = 0; i < 3; ++i)
    RTC_TRACE(("  %f %f %f %f",
               detector->camera_param.mat[i][0],
               detector->camera_param.mat[i][1],
               detector->camera_param.mat[i][2],
               detector->camera_param.mat[i][3]));
  RTC_TRACE(("ARToolKit distortion factor"));
  RTC_TRACE(("  %f %f %f %f",
             detector->camera_param.dist_factor[0],
             detector->camera_param.dist_factor[1],
             detector->camera_param.dist_factor[2],
             detector->camera_param.dist_factor[3]));
}

void ARMarkerDetectSimple::cleanup(void) {
  // ARマーカのリセット
  detector->ResetMarkersConfig();
  RTC_INFO(("AR marker data is released"));
  detector->GlDrawFinal();

#ifdef _WITH_GUI
#if CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >= 2
  cv::destroyWindow("ARMarkerDetectSimple");
#endif // CV_MAJOR_VERSION, CV_MINOR_VERSION
#endif // _WITH_GUI
}


RTC::ReturnCode_t ARMarkerDetectSimple::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("Image", m_imageIn);
  
  // Set OutPort buffer
  addOutPort("ImageAR", m_imageAROut);
  addOutPort("MarkerPoseAll", m_markerPoseAllOut);
  addOutPort("MarkerPose", m_markerPoseOut);
  
  // Set service provider to Ports
  m_ObjectRecognitionPortPort.registerProvider("RecognitionService", "RecognitionService", m_RecognitionService);
  
  // Set service consumers to Ports
  m_CameraCaptureServicePortPort.registerConsumer("CameraCaptureService", "Img::CameraCaptureService", m_CameraCaptureService);
  
  // Set CORBA Service Ports
  addPort(m_CameraCaptureServicePortPort);
  addPort(m_ObjectRecognitionPortPort);
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("marker_info_filename", m_marker_info_filename, "Data/markers_info.dat");
  bindParameter("default_detect_marker_id", m_default_detect_marker_id, "0");
  bindParameter("camera_parameter_filename", m_camera_parameter_filename, "Data/camera_para.dat");
  bindParameter("continuous_flag", m_continuous_flag, "1");
  
  // MarkerDetectクラスのインスタンス作成
  if ((detector = new MarkerDetect()) == NULL) {
    RTC_ERROR(("Unable to create an instance of MarkerDetect Class"));
    return RTC::RTC_ERROR;
  }
  RTC_INFO(("Create an instance of MarkerDetect Class"));

  int argc = 1;
  char *argv[1] = {"ARMarkerDetectSimple"};
  glutInit(&argc, argv);
  RTC_INFO(("Initialize glut"));

  // </rtc-template>
  return RTC::RTC_OK;
}


RTC::ReturnCode_t ARMarkerDetectSimple::onFinalize()
{
  // MarkerDetectクラスのインスタンスメモリ開放
  delete detector;
  RTC_INFO(("Destroy an instance of MarkerDetect Class"));
  
  return RTC::RTC_OK;
}

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

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


RTC::ReturnCode_t ARMarkerDetectSimple::onActivated(RTC::UniqueId ec_id)
{
  // カメラパラメータの読み込み
  if (detector->LoadCameraParameter(m_camera_parameter_filename.c_str(), 1) != 0) {
    RTC_ERROR(("Could not load camera parameter [%s]",
               m_camera_parameter_filename.c_str()));
    return RTC::RTC_ERROR;
  }
  logCameraParameter();
  detector->PrintCameraParameter("loadCameraParameter");

  // ARマーカの読み込み
  if (detector->LoadMarkersConfig(m_marker_info_filename.c_str()) != 0) {
    RTC_ERROR(("Could not load marker config [%s]",
               m_marker_info_filename.c_str()));
    return RTC::RTC_ERROR;
  }
  RTC_INFO(("Marker config including %d markers was loaded",
            detector->markers.size()));
  detector->PrintMarkersConfig();

  // デフォルトマーカIDの設定
  m_RecognitionService.marker_id = m_default_detect_marker_id;

  // GlDrawInit()はカメラパラメータの設定まで呼び出せなく、
  // カメラパラメータはデータポート経由でも送信される可能性があるため
  // onExecute()内に移動
  // detector->GlDrawInit();

#ifdef _WITH_GUI
  // GUI初期化
  cv::namedWindow("ARMarkerDetectSimple", CV_WINDOW_AUTOSIZE);
#endif

  // サービスポート接続状態の取得
  PortServiceList* portlist = this->get_ports();
  port_connect_check = new bool [portlist->length()];
  for(unsigned int i=0; i<portlist->length(); ++i){
    PortService_ptr port;
    port = (*portlist)[i];
    if(port->get_port_profile()->connector_profiles.length()!=0){
      port_connect_check[i]=true;
    }else{
      port_connect_check[i]=false;
    }
  }

  // サービスポートCameraCaptureServiceの接続状態チェック
  if(port_connect_check[4]==true){
    // 相手がActivatedか？もチェックすべき
    if(m_continuous_flag == 1){
      std::cout << "start continuous." << std::endl;
      RTC_INFO(("Send command of \"start continuous\" via CameraCaptureService."));
      m_CameraCaptureService->start_continuous();
    }
  }

  return RTC::RTC_OK;
}


RTC::ReturnCode_t ARMarkerDetectSimple::onDeactivated(RTC::UniqueId ec_id)
{
  // サービスポートCameraCaptureServiceの接続状態チェック
  if(port_connect_check[4]==true){
    // 相手がActivatedか？もチェックすべき
    if(m_continuous_flag == 1){
      std::cout << "stop continuous." << std::endl;
      RTC_INFO(("Send command of \"stop continuous\" via CameraCaptureService."));
      m_CameraCaptureService->stop_continuous();
    }
  }
  delete port_connect_check;
  cleanup();
  return RTC::RTC_OK;
}


RTC::ReturnCode_t ARMarkerDetectSimple::onExecute(RTC::UniqueId ec_id)
{
  // サービスポートCameraCaptureServiceおよび
  // サービスポートRecognitionServiceの接続状態チェック
  if(port_connect_check[4]==true && port_connect_check[5]==true){
    // 相手がActivatedか？もチェックすべき
    if(m_continuous_flag == 0 &&
       m_RecognitionService.command_receive_flag == true){
      std::cout << "take one frame." << std::endl;
      RTC_DEBUG(("Send command of \"take one frame\" via CameraCaptureService."));
      m_CameraCaptureService->take_one_frame();
      m_RecognitionService.command_receive_flag = false;
    }
  }

  if (m_imageIn.isNew()) {
    // 画像の読み込み
    m_imageIn.read();
    RTC_DEBUG(("Get & read image."));

    // 画像の幅、高さ、チャンネル数のチェック
    width = m_image.data.image.width;
    height = m_image.data.image.height;
    channels = (m_image.data.image.format == Img::CF_GRAY) ? 1 :
      (m_image.data.image.format == Img::CF_RGB) ? 3 :
      (m_image.data.image.raw_data.length()/width/height);
    RTC_TRACE(("Width %d, Height %d, Channels %d", width, height, channels));

    // 画像メモリの確保
    if(channels == 3)
      image.create(height, width, CV_8UC3);
    else
      image.create(height, width, CV_8UC1);

    // 画像のコピー
    for(int i=0; i<height; ++i){
      memcpy(&(image.data[i*image.step]),
             &m_image.data.image.raw_data[i*width*channels],
             sizeof(unsigned char)*width*channels);
    }

    // カメラパラメータの更新
    // if (m_image.data.intrinsic.matrix_element[0] != 0.0) {  // 内部パラメータではデータの有無を判別できない
    if (m_image.data.intrinsic.distortion_coefficient.length() > 0) {
      if (detector->SetCameraParameter(width, height,
                                       m_image.data.intrinsic.matrix_element[0],
                                       m_image.data.intrinsic.matrix_element[2],
                                       m_image.data.intrinsic.matrix_element[3],
                                       m_image.data.intrinsic.matrix_element[4],
                                       m_image.data.intrinsic.matrix_element[3],
                                       m_image.data.intrinsic.matrix_element[4],
                                       0.0, 1.0) < 0) {
        RTC_ERROR(("Unable to set camera parameter."));
        return RTC::RTC_ERROR;
      }
      logCameraParameter();
      detector->PrintCameraParameter("setCameraParameter");
    }
    // カメラパラメータが設定された直後に描画関数の初期化
    detector->GlDrawInit();

    // ARToolKitはRGBを利用するので、グレースケールはRGBに変換
    if (channels == 1) {
      cv::cvtColor(image, image, CV_GRAY2RGB);
      RTC_DEBUG(("Color conversion GRAY -> RGB"));
    }

    // ARマーカ検出
    if (detector->Detect(&(image.data[0]), 100) < 0) {
      RTC_WARN(("Unable to finish detection process normally."));
      return RTC::RTC_OK;
    }
    detector->PrintMarkersConfig();

    // ARマーカ姿勢の書き込み
    marker_id = m_RecognitionService.marker_id;
    if (detector->QueryDetectState(marker_id)) {
      RTC_DEBUG(("Specified Marker ID [%ld] is detected",
                 marker_id));
      m_markerPose.data.length(12);
      RTC_TRACE(("Pose of specified marker ID [%ld]",
                 marker_id));
      for (unsigned int i = 0; i < 3; ++i) {
        std::string comment = " ";
        for (unsigned int j = 0; j < 4; ++j) {
          m_markerPose.data[i*4 + j] = 
            detector->markers[marker_id].config->trans[i][j];
          // RTC::Loggerでの位置姿勢ログ用
          char s[32];
          snprintf(s, 32, " %12.6lf",
                   m_markerPose.data[i*4 + j]);
          comment = comment + " " + s;
        }
        RTC_TRACE(("%s", comment.c_str()));
      }
      m_markerPoseOut.write();
    }
    std::vector<int> ids;
    detector->GetDetectedMarkerIDs(&ids);
    m_markerPoseAll.data.length(ids.size());
    RTC_DEBUG(("Total of detected marker is %d",
               ids.size()));
    for (unsigned int n = 0; n < ids.size(); ++n) {
      long id = ids[n];
      RTC_DEBUG(("Marker ID [%ld] is detected",
                 id));
      m_markerPoseAll.data[n].id = id;
      m_markerPoseAll.data[n].pose.length(12);
      RTC_TRACE(("Pose of marker ID [%ld]",
                 id));
      for (unsigned int i = 0; i < 3; ++i) {
        std::string comment = " ";
        for (unsigned int j = 0; j < 4; ++j) {
          m_markerPoseAll.data[n].pose[i*4 + j] = 
            detector->markers[id].config->trans[i][j];
          // RTC::Loggerでの位置姿勢ログ用
          char s[32];
          snprintf(s, 32, " %12.6lf",
                   m_markerPoseAll.data[n].pose[i*4 + j]);
          comment = comment + " " + s;
        }
        RTC_TRACE(("%s", comment.c_str()));
      }
    }
    m_markerPoseAllOut.write();

    // ARマーカの描画
    detector->GlDraw3D(&(image.data[0]));
    RTC_DEBUG(("Overlay CG image"));

    // 出力用のカメラパラメータの書き込み
    if (m_imageAR.data.intrinsic.matrix_element[0] != 0.0) {
      for (int i = 0; i < 5; ++i) {
        m_imageAR.data.intrinsic.matrix_element[i] =
          m_image.data.intrinsic.matrix_element[i];
      }
      int len = m_image.data.intrinsic.distortion_coefficient.length();
      m_imageAR.data.intrinsic.distortion_coefficient.length(len);

      for (int i = 0; i < len; ++i) {
        m_imageAR.data.intrinsic.distortion_coefficient[i] =
          m_image.data.intrinsic.distortion_coefficient[i];
      }
    }

    // 出力用の画像の画像幅、高さ、フォーマットの書き込み
    m_imageAR.data.image.width = width;
    m_imageAR.data.image.height = height;
    m_imageAR.data.image.format = Img::CF_RGB;

    // 出力用の画像の書き込み
    m_imageAR.data.image.raw_data.length(width * height * 3);
    for (int i = 0; i < height; ++i){
      memcpy(&m_imageAR.data.image.raw_data[i*width*3],
             &(image.data[i*image.step]),
             sizeof(unsigned char)*width*3);
    }
    m_imageAROut.write();

#ifdef _WITH_GUI
    // 画像の表示
    cv::cvtColor(image, image, CV_RGB2BGR);
    if(!image.empty()) {
      cv::imshow("ARMarkerDetectSimple", image);
      RTC_DEBUG(("AR image displayed"));
    } else {
      RTC_WARN(("AR image is empty"));
    }
    cv::cvtColor(image, image, CV_BGR2RGB);
    cv::waitKey(5);
#endif
  }

  return RTC::RTC_OK;
}


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

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

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

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

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



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


