/************************************************************************************
Camera RT-Component
Copyright (c) 2010, Segway Japan, Ltd.
All rights reserved.

Contact us if you use this software for sell.
If you use this software not for sell, you can use this software under BSD lisence.
See the files LICENSE.TXT and LICENSE-BSD.TXT for more details.                     
************************************************************************************/
#include "Cam_v4l2.h"


#define USE_HSV  (0)

// Module specification
// <rtc-template block="module_spec">
static const char* cam_v4l2_spec[] =
  {
    "implementation_id", "Cam_v4l2",
    "type_name",         "Cam_v4l2",
    "description",       "Camera video4linux2 component",
    "version",           "1.0",
    "vendor",            "SGIJ",
    "category",          "Generic",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.width", "640",
    "conf.default.height", "480",
    "conf.default.str_port", "/dev/video0",

    ""
  };
// </rtc-template>

Cam_v4l2::Cam_v4l2(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_ImageOut("Image", m_Image),
    m_ImageYUYVOut("ImageYUYV", m_ImageYUYV),
    
    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  registerOutPort("Image", m_ImageOut);
  registerOutPort("ImageYUYV", m_ImageYUYVOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  Hbuf = NULL;
  Sbuf = NULL;
  Vbuf = NULL;
}

Cam_v4l2::~Cam_v4l2()
{
}


RTC::ReturnCode_t Cam_v4l2::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("width", m_width, "640");
  bindParameter("height", m_height, "480");
  bindParameter("str_port", m_str_port, "/dev/video0");
  
  // </rtc-template>


  return RTC::RTC_OK;
}



/*
RTC::ReturnCode_t Cam_v4l2::onFinalize()
{
  return RTC::RTC_OK;
}
*/

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

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


RTC::ReturnCode_t Cam_v4l2::onActivated(RTC::UniqueId ec_id)
{
#if USE_HSV
  if (Hbuf == NULL) {
    Hbuf = (float*)malloc(sizeof(float) * m_width * m_height);
  }
  if (Sbuf == NULL) {
    Sbuf = (float*)malloc(sizeof(float) * m_width * m_height);
  }
  if (Vbuf == NULL) {
    Vbuf = (float*)malloc(sizeof(float) * m_width * m_height);
  }
#endif
  m_ImageYUYV.data.length(m_width * m_height);
  m_Image.data.length(m_width * m_height);

#if USE_HSV
  m_H.data.length(m_width * m_height);
  m_S.data.length(m_width * m_height);
  m_V.data.length(m_width * m_height);
#endif

  cam.Init(m_width, m_height, (char*)m_str_port.c_str());

  win.start(0, NULL, 100,100,m_width,m_height, "RTC Cam_v4l2");

  return RTC::RTC_OK;
}



RTC::ReturnCode_t Cam_v4l2::onDeactivated(RTC::UniqueId ec_id)
{
  cam.End();

  win.end();

  if (Hbuf) {
    free(Hbuf);
    Hbuf = NULL;
  }
  if (Sbuf) {
    free(Sbuf);
    Sbuf = NULL;
  }
  if (Vbuf) {
    free(Vbuf);
    Vbuf = NULL;
  }

  return RTC::RTC_OK;
}



RTC::ReturnCode_t Cam_v4l2::onExecute(RTC::UniqueId ec_id)
{
  cam.Capture();

  unsigned char* yuyvbuf = cam.GetYUYVbufptr();

  for (int i = 0; i < m_width * m_height; i++) {
    unsigned short color =
      (yuyvbuf[i*2]   << 0)  |
      (yuyvbuf[i*2+1] << 8);

    m_ImageYUYV.data[i] = color;
  }

#if USE_HSV
  cam.GetHSV(Hbuf, Sbuf, Vbuf);
#endif
  unsigned char* ibuf = cam.GetXRGBbufptr();

  for (int i = 0; i < m_width * m_height; i++) {
    unsigned long color =
      (ibuf[i*4]   << 0)  |
      (ibuf[i*4+1] << 8)  |
      (ibuf[i*4+2] << 16) |
      (ibuf[i*4+3] << 24);

    m_Image.data[i] = color;
#if USE_HSV
    m_H.data[i] = Hbuf[i];
    m_S.data[i] = Sbuf[i];
    m_V.data[i] = Vbuf[i];
#endif
  }

  m_ImageYUYVOut.write();
  m_ImageOut.write();
#if USE_HSV
  m_HOut.write();
  m_SOut.write();
  m_VOut.write();
#endif
  win.drawlock();
  win.drawimage((char*)ibuf, 0,0, m_width, m_height);
  win.drawunlock();
  //  win.eventloop();

  usleep(10000);

  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t Cam_v4l2::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

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

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

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

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



extern "C"
{

  void Cam_v4l2Init(RTC::Manager* manager)
  {
    RTC::Properties profile(cam_v4l2_spec);
    manager->registerFactory(profile,
                             RTC::Create<Cam_v4l2>,
                             RTC::Delete<Cam_v4l2>);
  }

};


