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

#ifndef ARMARKERDETECTSIMPLE_H
#define ARMARKERDETECTSIMPLE_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include <rtm/idl/ExtendedDataTypesSkel.h>
#include <rtm/idl/InterfaceDataTypesSkel.h>
//#include <rtm/idl/Img.h>
//#include <rtm/idl/MyDataType.h>

// Service implementation headers
// <rtc-template block="service_impl_h">
#include "MyDataTypeSVC_impl.h"

// </rtc-template>

// Service Consumer stub headers
// <rtc-template block="consumer_stub_h">
#include "ImgStub.h"

// </rtc-template>
#include <cv.h>
#include <highgui.h>
#include "MarkerDetect.h"

using namespace RTC;
using namespace Img;
using namespace MyDataType;

/*!
 * @class ARMarkerDetectSimple
 * @brief AR Marker Detection Module using ARToolkit
 *
 * ARToolKitを利用したマーカ位置姿勢推定モジュールです。
 *
 * ARToolKit Official Site
 * - http://www.hitl.washington.edu/artoolkit/
 * 工学ナビARToolKitを使った拡張現実感プログラミング
 * - http://kougaku-navi.net/ARToolKit.html
 *
 */
class ARMarkerDetectSimple
  : public RTC::DataFlowComponentBase
{
 public:
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
  ARMarkerDetectSimple(RTC::Manager* manager);

  /*!
   * @brief destructor
   */
  ~ARMarkerDetectSimple();

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

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

  /***
   *
   * The initialize action (on CREATED->ALIVE transition)
   * formaer rtc_init_entry() 
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onInitialize();

  /***
   *
   * The finalize action (on ALIVE->END transition)
   * formaer rtc_exiting_entry()
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onFinalize();

  /***
   *
   * The startup action when ExecutionContext startup
   * former rtc_starting_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);

  /***
   *
   * The shutdown action when ExecutionContext stop
   * former rtc_stopping_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);

  /***
   *
   * The activated action (Active state entry action)
   * former rtc_active_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);

  /***
   *
   * The deactivated action (Active state exit action)
   * former rtc_active_exit()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);

  /***
   *
   * The execution action that is invoked periodically
   * former rtc_active_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

  /***
   *
   * The aborting action when main logic error occurred.
   * former rtc_aborting_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);

  /***
   *
   * The error action in ERROR state
   * former rtc_error_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);

  /***
   *
   * The reset action that is invoked resetting
   * This is same but different the former rtc_init_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
  
  /***
   *
   * The state update action that is invoked after onExecute() action
   * no corresponding operation exists in OpenRTm-aist-0.2.0
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);

  /***
   *
   * The action that is invoked when execution context's rate is changed
   * no corresponding operation exists in OpenRTm-aist-0.2.0
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);


 protected:
  // <rtc-template block="protected_attribute">
  
  // </rtc-template>

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

  // Configuration variable declaration
  // <rtc-template block="config_declare">
  /*!
   * 
   * - Name:  marker_info_filename
   * - DefaultValue: Data/markers_info.dat
   */
  std::string m_marker_info_filename;
  /*!
   * MarkerPose出力ポートから送信するデフォルトのマーカID．
   * サービスポートRecognitionServiceよりIDが指定された場合は、サ
   * ービスポート側が優先される．
   * - Name: デフォルトの検出対象ID default_detect_marker_id
   * - DefaultValue: 0
   */
  long int m_default_detect_marker_id;
  /*!
   * 
   * - Name:  camera_parameter_filename
   * - DefaultValue: Data/camera_para.dat
   */
  std::string m_camera_parameter_filename;
  /*!
   * 1を指定すると連続的にマーカ位置検出処理を実行する．
   * 0を指定するとサービスポートObjectRecognitionServiceよりsetModelID
   * ()が呼び出されたタイミングのみ実行する．
   * - Name: 連続検出有効化のフラグ continuous_flag
   * - DefaultValue: 1
   */
  bool m_continuous_flag;
  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedCameraImage m_image;
  /*!
   * カメラ画像。このデータにはカメラの内部パラメータ、
   * レンズの歪パラメータなども含まれる。
   * - Type: Img::TimedCameraImage
   * - Semantics: カメラ画像
   */
  InPort<TimedCameraImage> m_imageIn;
  
  // </rtc-template>


  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  TimedCameraImage m_imageAR;
  /*!
   * 重畳画像を出力する。
   * - Type: Img::TimedCameraImage
   * - Semantics: 入力ポートで取得したカメラ画像上にCGを重ね
   *              合わせた画像
   */
  OutPort<TimedCameraImage> m_imageAROut;
  TimedObjectPoseMatSeq m_markerPoseAll;
  /*!
   * 検出されたマーカの位置姿勢をID番号と共に全て出力する
   * 。
   */
  OutPort<TimedObjectPoseMatSeq> m_markerPoseAllOut;
  TimedDoubleSeq m_markerPose;
  /*!
   * 指定されたマーカの位置姿勢を出力する。出力対象のマ
   * ーカIDは、始めはConfigurationパラーメータで指定されたIDであ
   * り、サービスポートのRecognitionServiceにより任意のIDも指定可
   * 能である。
   * - Type: double
   * - Number: 12
   * - Semantics: 以下のような行列があった場合、
   *              r1 r2 r3 t1
   *              r4 r5 r6 t2
   *              r7 r8 r9 t3
   *               0  0  0  1
   *              r1, r2, r3, t1, r4, r5, r6, t2, r7, r8, r9, t3
   *              として出力する．
   */
  OutPort<TimedDoubleSeq> m_markerPoseOut;
  
  // </rtc-template>

  // CORBA Port declaration
  // <rtc-template block="corbaport_declare">
  /*!
   */
  RTC::CorbaPort m_CameraCaptureServicePortPort;
  /*!
   */
  RTC::CorbaPort m_ObjectRecognitionPortPort;
  
  // </rtc-template>

  // Service declaration
  // <rtc-template block="service_declare">
  /*!
   */
  RecognitionServiceSVC_impl m_RecognitionService;
  
  // </rtc-template>

  // Consumer declaration
  // <rtc-template block="consumer_declare">
  /*!
   */
  RTC::CorbaConsumer<Img::CameraCaptureService> m_CameraCaptureService;
  
  // </rtc-template>

 private:
  void logCameraParameter(void);
  void cleanup(void);

  MarkerDetect *detector;
  double intrinsic[3][4];
  double dist_factor[4];
  int width, height, channels;
  int marker_id;
  cv::Mat image;
  bool *port_connect_check;
  // <rtc-template block="private_attribute">
  
  // </rtc-template>

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

};


extern "C"
{
  DLL_EXPORT void ARMarkerDetectSimpleInit(RTC::Manager* manager);
};

#endif // ARMARKERDETECTSIMPLE_H
