/************************************************************************************
URG 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 <math.h>
#include <string.h>
#include "Urg.h"
#include "scip2.h"
#include "x11win.h"
#include "misc.h"

//#define DEBUG
#ifdef DEBUG
#define DB(x) x
#else
#define DB(x)
#endif



// Module specification
// <rtc-template block="module_spec">
static const char* urg_spec[] =
  {
    "implementation_id", "Urg",
    "type_name",         "Urg",
    "description",       "Urg Component",
    "version",           "1.0.0",
    "vendor",            "SegwayJapan",
    "category",          "Generic",
    "activity_type",     "SPORADIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.str_port", "/dev/ttyACM0",
    "conf.default.start", "0",
    "conf.default.end", "1080",
    "conf.default.skip", "1",
    "conf.default.win", "1",
    "conf.default.timesynccount", "10",
    "conf.default.timesyncperiod", "60.0",

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

Urg::Urg(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_ScanOut("Scan", m_Scan),
    m_ScanTSOut("ScanTS", m_ScanTS),
    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  registerOutPort("Scan", m_ScanOut);
  registerOutPort("ScanTS", m_ScanTSOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>


}

Urg::~Urg()
{
}


RTC::ReturnCode_t Urg::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("str_port", m_str_port, "/dev/ttyACM0");
  bindParameter("start", m_start, "0");
  bindParameter("end", m_end, "1080");
  bindParameter("skip", m_skip, "1");
  bindParameter("win", m_win, "1");
  bindParameter("timesynccount", m_timesynccount, "10");
  bindParameter("timesyncperiod", m_timesyncperiod, "60.0");
  
  // </rtc-template>
  return RTC::RTC_OK;
}



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

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

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


RTC::ReturnCode_t Urg::onActivated(RTC::UniqueId ec_id)
{
  memset(scan, 0, sizeof(long)*SCIP2_TOPURG_SAMPLES);

  dev = Scip2_Start((char*)m_str_port.c_str());

  if (m_win) {
    win.start(0, NULL, 640,480,limagew,limageh, "RTC Urg");
  }

  if (m_timesynccount && !strstr(m_str_port.c_str(), "/dev/ttySIM")) {
    Scip2_TimeSync(dev, 100);

    timesyncnext = coil::gettimeofday();
  }

  return RTC::RTC_OK;
}



RTC::ReturnCode_t Urg::onDeactivated(RTC::UniqueId ec_id)
{
  Scip2_End(dev);
  dev = NULL;

  if (m_win) {
    win.end();
  }

  return RTC::RTC_OK;
}



void Urg::drawlaser()
{
  //draw scan image
  memset(scanimage, 0xFF, sizeof(unsigned long) * limagew * limageh);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   0, limageh/2-PIXEL_PER_METER*2, limagew, limageh/2-PIXEL_PER_METER*2, 
	   0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   0, limageh/2-PIXEL_PER_METER, limagew, limageh/2-PIXEL_PER_METER, 
	   0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   0, limageh/2,                 limagew, limageh/2, 
	   0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   0, limageh/2+PIXEL_PER_METER, limagew, limageh/2+PIXEL_PER_METER, 
	   0x80, 0x80, 0x80);
  drawline((unsigned char*)scanimage, limagew, limageh,
	   0, limageh/2+PIXEL_PER_METER*2, limagew, limageh/2+PIXEL_PER_METER*2, 
	   0x80, 0x80, 0x80);


  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2, 0, limagew/2, limageh, 0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2-PIXEL_PER_METER, 0, limagew/2-PIXEL_PER_METER, limageh, 0x80, 0x80, 0x80);
  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2-PIXEL_PER_METER*2, 0, limagew/2-PIXEL_PER_METER*2, limageh, 0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2+PIXEL_PER_METER, 0, limagew/2+PIXEL_PER_METER, limageh, 0x80, 0x80, 0x80);
  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2+PIXEL_PER_METER*2, 0, limagew/2+PIXEL_PER_METER*2, limageh, 0x80, 0x80, 0x80);

  //naname
  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2, limageh/2,                 0,       limageh, 
	   0x80, 0x80, 0x80);
  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2, limageh/2,                 limagew, limageh, 
	   0x80, 0x80, 0x80);


  float pi = 3.141592f;
  float oneang = (pi / 180.0f) * SCIP2_TOPURG_ONEANG;
  float starttheta = -(pi/4.0f);


  int sample = 0;
  for (int i = m_start; i <= m_end-m_skip; i += m_skip) {

    float laser0;
    float laser1;

    laser0 = (float)scan[sample]   / 1000.0;
    laser1 = (float)scan[sample+1] / 1000.0;

    float x0, y0, x1, y1;
    x0 = laser0 * cosf(starttheta + oneang*(float)i) * PIXEL_PER_METER;
    y0 = laser0 * sinf(starttheta + oneang*(float)i) * PIXEL_PER_METER;
    x1 = laser1 * cosf(starttheta + oneang*(float)(i+m_skip)) * PIXEL_PER_METER;
    y1 = laser1 * sinf(starttheta + oneang*(float)(i+m_skip)) * PIXEL_PER_METER;

    int ix0, iy0, ix1, iy1;
    ix0 = (int)x0 + limagew/2;
    iy0 = -(int)y0 + limageh/2;
    ix1 = (int)x1 + limagew/2;
    iy1 = -(int)y1 + limageh/2;

    drawline((unsigned char*)scanimage, limagew, limageh, ix0, iy0, ix1, iy1, 0, 0, 0xFF);

    sample++;
  }
  //printf("%d", sample);
}



RTC::ReturnCode_t Urg::onExecute(RTC::UniqueId ec_id)
{
  coil::TimeValue timestamp;

  if (m_timesynccount && !strstr(m_str_port.c_str(), "/dev/ttySIM")) {

    //-------------------------------------
    //update time syncronization paramter
    //-------------------------------------
    double time = coil::gettimeofday();
    if (time > timesyncnext) {
      Scip2_TimeSync(dev, m_timesynccount);
      timesyncnext = time + m_timesyncperiod;
    }

#ifdef DEBUG
    double pre = coil::gettimeofday();
#endif

    int timestamp_urg = 
    Scip2_GD(dev,m_start,m_end,m_skip, scan);

    //-------------------------------------
    //estimate URG timestamp as PC time
    //-------------------------------------
    timestamp = Scip2_TimeEstimate(timestamp_urg);

#ifdef DEBUG
    double urg = (double)timestamp;
    double post = (double)coil::gettimeofday();
    printf("pre %f urg %f post %f   urg-pre %f post-urg %f\n",pre, urg, post, urg-pre, post-urg);
#endif
  }
  else {
    Scip2_GD(dev,m_start,m_end,m_skip, scan);

    //----------------------------
    //use PC time directly
    //----------------------------
    timestamp = coil::gettimeofday();
  }


  if (m_win) {
    drawlaser();
    win.drawlock();
    win.drawimage((char*)scanimage, 0, 0, limagew, limageh);
    win.drawunlock();
  }


  //面倒だからカウントしちゃえ・・
  int cnt = 0;
  for (int i = m_start; i <= m_end; i += m_skip) {
    cnt++;
  }
  //printf(" %d\n", cnt);
  m_Scan.data.length(cnt);  
  m_ScanTS.scan.data.length(cnt);

  for (int i = 0; i < cnt; i++) {
    m_Scan.data[i] = scan[i];
    m_ScanTS.scan.data[i] = scan[i];
  }

  //time stamp
  RTC::Time tm;
  tm.sec = timestamp.sec();
  tm.nsec = timestamp.usec() * 1000;
  m_Scan.tm = tm;
  m_ScanTS.tm = tm;

  m_ScanOut.write();
  m_ScanTSOut.write();

  usleep(10000);

  return RTC::RTC_OK;
}


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

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

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

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

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



extern "C"
{

  void UrgInit(RTC::Manager* manager)
  {
    RTC::Properties profile(urg_spec);
    manager->registerFactory(profile,
                             RTC::Create<Urg>,
                             RTC::Delete<Urg>);
  }

};


