// -*- C++ -*-
 /*!
  * @file  ObjectTracking.cpp
  * @brief Object tracking component
  * @date $Date$
  *
  * $Id$
  */
 
 #include "ObjectTracking.h"
 
 #define HIST_RANGE_MAX 180.0
 #define HIST_RANGE_MIN 0.0
 
 // 입력 화상용 IplImage
 IplImage* g_image;
 
 // CamShift 트랙킹용 변수
 CvPoint g_origin;
 CvRect g_selection;  // 
 
 // 초기 추적 영역의 설정 판별 플래그치 (0: 설정 이루어, 1: 설정 있어)
 int g_select_object;
 
 // 트랙킹의 개시/정지용 플래그치 (0: 정지, -1: 개시, 1: 트랙킹중)
 int g_track_object;
 
 // 오브젝트 선택 판정용 플래그(0: 1이외 1: 선택된 직후)
 int g_selected_flag;
 
 // 선택된 범위의 중심 좌표
 float g_selected_x;
 float g_selected_y;
 
 
 // Module specification
 static const char* objecttracking_spec[] =
   {
     "implementation_id", "ObjectTracking",
     "type_name",         "ObjectTracking",
     "description",       "Object tracking component",
     "version",           "1.0.0",
     "vendor",            "AIST",
     "category",          "Category",
     "activity_type",     "PERIODIC",
     "kind",              "DataFlowComponent",
     "max_instance",      "1",
     "language",          "C++",
     "lang_type",         "compile",
     "exec_cxt.periodic.rate", "1.0",
     // Configuration variables
     "conf.default.brightness_max", "256",
     "conf.default.brightness_min", "10",
     "conf.default.saturation_min", "30",
     "conf.default.image_height", "240",
     "conf.default.image_width", "320",
     ""
   };
 
 /*!
  * @brief constructor
  * @param manager Maneger Object
  */
 ObjectTracking::ObjectTracking(RTC::Manager* manager)
   : RTC::DataFlowComponentBase(manager),
     m_in_imageIn("in_image", m_in_image),
     m_tracking_imageOut("tracing_image", m_tracking_image),
     m_displacementOut("displacement", m_displacement),
     dummy(0),
     m_hsv(0), m_hue(0), m_mask(0), m_backproject(0),
     m_hist(0), m_backproject_mode(0),m_hdims(16),
     m_init_flag(0)
 {
   // Registration: InPort/OutPort/Service
   // Set InPort buffers
   registerInPort("in_image", m_in_imageIn);
   
   // Set OutPort buffer
   registerOutPort("tracing_image", m_tracking_imageOut);
   registerOutPort("displacement", m_displacementOut);
 
   m_hranges_arr[0] = HIST_RANGE_MIN;
   m_hranges_arr[1] = HIST_RANGE_MAX;
   m_hranges = m_hranges_arr;
 }
 
 /*!
  * @brief destructor
  */
 ObjectTracking::~ObjectTracking()
 {
 }
 
 
 
 RTC::ReturnCode_t ObjectTracking::onInitialize()
 {
   // Bind variables and configuration variable
   bindParameter("brightness_max", m_b_max, "256");
   bindParameter("brightness_min", m_b_min, "10");
   bindParameter("saturation_min", m_s_min, "30");
   bindParameter("image_height", m_img_height, "240");
   bindParameter("image_width", m_img_width, "320");
   m_displacement.data.length(2);
   return RTC::RTC_OK;
 }
 
 
 RTC::ReturnCode_t ObjectTracking::onActivated(RTC::UniqueId ec_id)
 {
   m_init_flag = 0;
   // 윈도우를 생성한다
   cvNamedWindow( "ObjectTracking", 1 );
   // 마우스 조작시의 콜백 처리의 등록
   cvSetMouseCallback( "ObjectTracking", on_mouse, 0 );
   g_selected_flag = 0;
   g_selected_x = 0.0;;
   g_selected_y = 0.0;;
 
   return RTC::RTC_OK;
 }
 
 
 RTC::ReturnCode_t ObjectTracking::onDeactivated(RTC::UniqueId ec_id)
 {
   if (m_init_flag) {
     // 메모리를 해방한다
     cvReleaseImage(&g_image);
     cvReleaseImage(&m_hsv);
     cvReleaseImage(&m_hue);
     cvReleaseImage(&m_mask);
     cvReleaseImage(&m_backproject);
   }
   // 윈도우를 파기한다
   cvDestroyWindow("ObjectTracking");
   m_init_flag = 0;
   return RTC::RTC_OK;
 }
 
 
 RTC::ReturnCode_t ObjectTracking::onExecute(RTC::UniqueId ec_id)
 {
   if (m_in_imageIn.isNew()) {
     m_in_imageIn.read();
 
     if(!m_init_flag) allocateBuffers();
 
     memcpy(g_image->imageData,(void *)&(m_in_image.data[0]),m_in_image.data.length());
     cvShowImage("ObjectTracking", g_image);
 
     // 캡쳐 된 화상을 HSV표색계로 변환해 hsv에 격납
     cvCvtColor( g_image, m_hsv, CV_BGR2HSV );
 
     // g_track_object 플래그가 0 이하라면, 이하의 처리를 실시한다
     if( g_track_object )
       {
     cvInRangeS( m_hsv, cvScalar(HIST_RANGE_MIN,m_s_min,MIN(m_b_min,m_b_max),0),
             cvScalar(HIST_RANGE_MAX,256,MAX(m_b_min,m_b_max),0), m_mask );
     cvSplit( m_hsv, m_hue, 0, 0, 0 );
 
     if( g_track_object < 0 ) calcHistogram();
 
     // 백 프로젝션을 계산한다
     cvCalcBackProject( &m_hue, m_backproject, m_hist );
     // backProjection 가운데, 마스크가 1이라고 된 부분만 남긴다
     cvAnd( m_backproject, m_mask, m_backproject, 0 );
     // CamShift법에 따르는 영역 추적을 실행한다
     cvCamShift( m_backproject, m_track_window,
             cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
             &m_track_comp, &m_track_box );
     m_track_window = m_track_comp.rect;
     
     if( m_backproject_mode )
       cvCvtColor( m_backproject, g_image, CV_GRAY2BGR );
     if( g_image->origin )
       m_track_box.angle = -m_track_box.angle;
     cvEllipseBox( g_image, m_track_box, CV_RGB(255,0,0), 3, CV_AA, 0 );
 
     // 마우스로 선택된 영역의 중심 좌표를 보존
     if (g_selected_flag) {
       g_selected_x = m_track_box.center.x;
       g_selected_y = m_track_box.center.y;
       g_selected_flag = 0;
     }
 
     // 마우스로 선택된 위치로부터의 이동량을 OutPort로부터 출력
     m_displacement.data[0] = m_track_box.center.x - g_selected_x;
     m_displacement.data[1] = -(m_track_box.center.y - g_selected_y);
     m_displacementOut.write();
       }
         
     // 마우스로 선택중의 초기 추적 영역의 색을 반전시킨다
     if( g_select_object && g_selection.width > 0 && g_selection.height > 0 )
       {
     cvSetImageROI( g_image, g_selection );
     cvXorS( g_image, cvScalarAll(255), g_image, 0 );
     cvResetImageROI( g_image );
       }
 
     // 화상을 표시한다
     cvShowImage( "ObjectTracking", g_image );
 
     // 화상을 OutPort로부터 출력한다
     int len = g_image->nChannels * g_image->width * g_image->height;
     m_tracking_image.data.length(len);
     memcpy((void *)&(m_tracking_image.data[0]),g_image->imageData,len);
     m_tracking_imageOut.write();
 
     // 키 입력을 기다려, 밀린 키에 의해서 처리를 분기 시킨다
     int c = cvWaitKey(10);
     // while 엔들레스 루프로부터 탈출(프로그램을 종료)
     if( (char) c == 27 ) {
       this->exit();
     }
   }
   return RTC::RTC_OK;
 }
 
 /*!
  * 모든 이미지용 메모리의 확보
  */
 void ObjectTracking::allocateBuffers() {
   g_image = cvCreateImage( cvSize(m_img_width,m_img_height),8, 3 );
   m_hsv = cvCreateImage( cvSize(m_img_width,m_img_height),8, 3 );
   m_hue = cvCreateImage( cvSize(m_img_width,m_img_height),8, 1 );
   m_mask = cvCreateImage( cvSize(m_img_width,m_img_height),8, 1 );
   m_backproject = cvCreateImage( cvSize(m_img_width,m_img_height),8, 1 );
   m_hist = cvCreateHist( 1, &m_hdims, CV_HIST_ARRAY, &m_hranges, 1 );
   m_init_flag = 1;
 }
 
 /*!
  * 히스토그램의 계산
  */
 void ObjectTracking::calcHistogram() {
   float max_val = 0.f;
   cvSetImageROI( m_hue, g_selection );
   cvSetImageROI( m_mask, g_selection );
   // 히스토그램을 계산해, 최대치를 요구한다
   cvCalcHist( &m_hue, m_hist, 0, m_mask );
   cvGetMinMaxHistValue( m_hist, 0, &max_val, 0, 0 );
   // 히스토그램의 세로축(빈도)을0-255의 다이나믹 레인지에 정규화
   cvConvertScale( m_hist->bins, m_hist->bins, max_val ? 255. / max_val : 0., 0 );
   // hue,mask 화상으로 설정된 ROI를 리셋트
   cvResetImageROI( m_hue );
   cvResetImageROI( m_mask );
   m_track_window = g_selection;
   // track_object를 트랙킹중으로 한다
   g_track_object = 1;
 }
 
 
 extern "C"
 {
  
   void ObjectTrackingInit(RTC::Manager* manager)
   {
     RTC::Properties profile(objecttracking_spec);
     manager->registerFactory(profile,
                              RTC::Create<ObjectTracking>,
                              RTC::Delete<ObjectTracking>);
   }
   
 
   //
   //    마우스 드러그에 의해서 초기 추적 영역을 지정한다
   //
   //    인수:
   //        event    : 마우스왼쪽 버튼 상태
   //        x        : 마우스가 현재 포인트 하고 있는 x좌표
   //        y        : 마우스가 현재 포인트 하고 있는 y좌표
   //        flags    : 본프로그램에서는 미사용
   //        param    : 본프로그램에서는 미사용
   //
   void on_mouse( int event, int x, int y, int flags, void* param )
   {
     // 화상이 취득되어 있지 않으면, 처리를 실시하지 않는다
     if( !g_image )
       return;
 
     // 원점의 위치에 따라 y의 값을 반전(화상의 반전은 아니다)
     if( g_image->origin )
       y = g_image->height - y;
 
     // 마우스의 왼쪽 버튼이 밀리고 있으면 이하의 처리를 실시한다
     if( g_select_object )
       {
     g_selection.x = MIN(x,g_origin.x);
     g_selection.y = MIN(y,g_origin.y);
     g_selection.width = g_selection.x + CV_IABS(x - g_origin.x);
     g_selection.height = g_selection.y + CV_IABS(y - g_origin.y);
         
     g_selection.x = MAX( g_selection.x, 0 );
     g_selection.y = MAX( g_selection.y, 0 );
     g_selection.width = MIN( g_selection.width, g_image->width );
     g_selection.height = MIN( g_selection.height, g_image->height );
     g_selection.width -= g_selection.x;
     g_selection.height -= g_selection.y;
       }
 
     // 마우스의 왼쪽 버튼 상태에 의해서 처리를 분기
     switch( event )
       {
       case CV_EVENT_LBUTTONDOWN:
     // 마우스의 왼쪽 버튼이 밀린 것이면,
     // 원점 및 선택된 영역을 설정
     g_origin = cvPoint(x,y);
     g_selection = cvRect(x,y,0,0);
     g_select_object = 1;
     break;
       case CV_EVENT_LBUTTONUP:
     // 마우스의 왼쪽 버튼이 떼어 놓아졌을 때, width와 height가 어느쪽이나 정이면,
     // g_track_object 플래그를 개시 플래그로 한다
     g_select_object = 0;
     if( g_selection.width > 0 && g_selection.height > 0 ) {
       g_track_object = -1;
       g_selected_flag = 1;
     }
     break;
       }
   }
 
 };

 // -*- C++ -*-
 /*!
  * @file  ObjectTracking.h
  * @brief Object tracking component
  * @date  $Date$
  *
  * $Id$
  */
 
 #ifndef OBJECTTRACKING_H
 #define OBJECTTRACKING_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 <cv.h>
 #include <highgui.h>
 #include <stdio.h>
 #include <ctype.h>
 
 
 using namespace RTC;
 
 /*!
  * @class ObjectTracking
  * @brief Object tracking component
  *
  */
 class ObjectTracking
   : public RTC::DataFlowComponentBase
 {
  public:
   /*!
    * @brief constructor
    * @param manager Maneger Object
    */
   ObjectTracking(RTC::Manager* manager);
 
   /*!
    * @brief destructor
    */
   ~ObjectTracking();
 
   /*!
    *
    * The initialize action (on CREATED->ALIVE transition)
    * formaer rtc_init_entry() 
    *
    * @return RTC::ReturnCode_t
    * 
    * 
    */
    virtual RTC::ReturnCode_t onInitialize();
 
 
   /***
    *
    * 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);
 
 
   /*!
    *    입력된 1개의 색상치를 RGB로 변환한다
    *
    *    인수:
    *        hue        : HSV표색계에 있어서의 색상치 H
    *    반환값：
    *        CvScalar: RGB의 색정보가 BGR의 순서로 격납된 컨테이너
    */
   CvScalar hsv2rgb( float hue ) {
     int rgb[3], p, sector;
     static const int sector_data[][3]=
       {{0,2,1}, {1,2,0}, {1,0,2}, {2,0,1}, {2,1,0}, {0,1,2}};
     hue *= 0.033333333333333333333333333333333f;
     sector = cvFloor(hue);
     p = cvRound(255*(hue - sector));
     p ^= sector & 1 ? 255 : 0;
 
     rgb[sector_data[sector][0]] = 255;
     rgb[sector_data[sector][1]] = 0;
     rgb[sector_data[sector][2]] = p;
     
     return cvScalar(rgb[2], rgb[1], rgb[0],0);
   }
   
 
   /*!
    * 모든 이미지용 버퍼의 확보
    */
   void allocateBuffers();
 
   /*!
    * 히스토그램의 계산
    */
   void calcHistogram();
 
  protected:
   // Configuration variable declaration
   /*!
    * 
    * - Name:  b_max
    * - DefaultValue: 256
    * - 명도의 최대치
    */
   int m_b_max;
   /*!
    * 
    * - Name:  b_min
    * - DefaultValue: 10
    * - 명도의 최소치 
    */
   int m_b_min;
   /*!
    * 
    * - Name:  s_min
    * - DefaultValue: 30
    * - 채도의 최소치
    */
   int m_s_min;
   /*!
    * 
    * - Name:  m_img_height
    * - DefaultValue: 240
    * - 화상의 높이
    */
   int m_img_height;
   /*!
    * 
    * - Name:  m_img_width
    * - DefaultValue: 320
    * - 화상의 폭
    */
   int m_img_width;
 
   // DataInPort declaration
   TimedOctetSeq m_in_image;
   InPort<TimedOctetSeq> m_in_imageIn;
   
   // DataOutPort declaration
   TimedOctetSeq m_tracking_image;
   OutPort<TimedOctetSeq> m_tracking_imageOut;
 
   TimedFloatSeq m_displacement;
   OutPort<TimedFloatSeq> m_displacementOut;
 
  private:
   int dummy;
   IplImage* m_hsv;         // HSV표색계용 IplImage
   IplImage* m_hue;         // HSV표색계의 H채널용 IplImage
   IplImage* m_mask;        // 마스크 화상용 IplImage

 
   CvHistogram * m_hist; // 히스토그램 처리용 구조체
 
   // 처리 모드 선택용 플래그
   int m_backproject_mode; // 백 프로젝션 화상의 표시/비표시용 플래그치 (0: 비표시, 1: 표시)
 
   // CamShift 트랙킹용 변수
   CvRect m_track_window;
   CvBox2D m_track_box;
   CvConnectedComp m_track_comp;
 
   // 히스토그램용 변수
   int m_hdims;                // 히스토그램의 차원수
   float m_hranges_arr[2]; // 히스토그램의 레인지
   float* m_hranges;
 
   // 초기화 판정 플래그
   int m_init_flag;
 };
 
 
 
 extern "C"
 {
   void ObjectTrackingInit(RTC::Manager* manager);
   void on_mouse( int event, int x, int y, int flags, void* param );
 };
 
 #endif // OBJECTTRACKING_H