// -*- C++ -*-
/*!
 * @file  LRFCapture_URG.cpp
 * @brief LRF capture module for URG
 * @date $Date$
 *
 * $Id$
 */

#include <cstdlib>
#include <iostream>
#include "LRFCapture_URG.h"

using namespace std;

#define DEG2RAD(x) ((x)*M_PI/180.0)

// Module specification
// <rtc-template block="module_spec">
static const char* lrfcapture_urg_spec[] =
  {
    "implementation_id", "LRFCapture_URG",
    "type_name",         "LRFCapture_URG",
    "description",       "LRF capture module for URG",
    "version",           "1.0.3",
    "vendor",            "Tsuyoshi Suenaga, NAIST",
    "category",          "sensor",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.device_port", "auto",
    "conf.default.start_deg", "-90",
    "conf.default.end_deg", "270",

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

LRFCapture_URG::LRFCapture_URG(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_RangeDataOut("RangeData", m_RangeData),
    m_LRFInfoPort("LRFInfo"),
    // </rtc-template>
    m_LRF(&info),
    dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  registerOutPort("RangeData", m_RangeDataOut);
  
  // Set service provider to Ports
  m_LRFInfoPort.registerProvider("LRF", "LRFInfo", m_LRF);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  registerPort(m_LRFInfoPort);
  
  // </rtc-template>

}

LRFCapture_URG::~LRFCapture_URG()
{
}



RTC::ReturnCode_t LRFCapture_URG::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("device_port", m_device_port, "auto");
  bindParameter("start_deg", m_start_deg, "-90");
  bindParameter("end_deg", m_end_deg, "270");

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

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

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

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


RTC::ReturnCode_t LRFCapture_URG::onActivated(RTC::UniqueId ec_id)
{
  vector<string> ports;

  // 接続ポート指定
  if(m_device_port=="auto"){
    findUrgPorts(ports);
    if (ports.empty()) {
      cerr << "no ports." << endl;
      std::exit(1);
    }
  }else{
    ports.push_back(m_device_port);
  }

  // URGに接続
  if (! urg.connect(ports[0].c_str())) {
    cerr << "Unable to connect to " << urg.what() << endl;
    std::exit(1);
  }else{
    cout << "Connected to " << ports[0] << endl;
  }

  // パラメータ情報の取得
  parameters = urg.parameter();

//   std::cerr << "TotalPoints: " << parameters.total_points << std::endl;
//   std::cerr << "Resolution : " << parameters.resolution << std::endl;
//   std::cerr << "OffsetFOV  : " << parameters.offset_fov << std::endl;
//   std::cerr << "FieldOfView: " << parameters.fov << std::endl;

  // データ取得範囲の設定
  double start_rad = DEG2RAD(m_start_deg);
  double end_rad   = DEG2RAD(m_end_deg);
  int start_index = ((urg.index2rad(parameters.area_min)+M_PI/2.0) > start_rad) ? parameters.area_min : urg.rad2index(start_rad-M_PI/2.0);
  int end_index   = ((urg.index2rad(parameters.area_max)+M_PI/2.0) < end_rad  ) ? parameters.area_max : urg.rad2index(end_rad-M_PI/2.0);
  std::cout << start_index << "," << end_index << std::endl;
  urg.setCaptureRange(start_index, end_index);

  // LRFInformationにパラメータをセット
  info.total_points = end_index - start_index + 1;
  info.resolution = M_PI*2.0/parameters.area_total;
  info.offset_fov = urg.index2rad(start_index) + M_PI/2.0;
  info.field_of_view = (info.total_points - 1)*info.resolution;
  info.start_index = start_index;
  info.end_index = end_index;
  info.max_distance = parameters.distance_max/1000.0; // [mm]->[m]
  info.min_distance = parameters.distance_min/1000.0; // [mm]->[m]
  
  return RTC::RTC_OK;
}



RTC::ReturnCode_t LRFCapture_URG::onDeactivated(RTC::UniqueId ec_id)
{
  // URGから切断
  if(urg.isConnected()){
    urg.disconnect();
  }
  return RTC::RTC_OK;
}



RTC::ReturnCode_t LRFCapture_URG::onExecute(RTC::UniqueId ec_id)
{
  // 距離データ取得
  int n = urg.capture(data);

  if (n <= 0) {
    return RTC::RTC_OK;
  }

  std::cout << "[0/" << info.total_points << "] ";
  // データ書き込み
  m_RangeData.data.length(info.total_points);
  for(int i=0; i<info.total_points; i++)
    m_RangeData.data[i] = data[info.start_index+i];
  std::cout << m_RangeData.data[0] << std::endl;

  // データ送信
  m_RangeDataOut.write();

  return RTC::RTC_OK;
}


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

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

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

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

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



extern "C"
{
 
  void LRFCapture_URGInit(RTC::Manager* manager)
  {
    RTC::Properties profile(lrfcapture_urg_spec);
    manager->registerFactory(profile,
                             RTC::Create<LRFCapture_URG>,
                             RTC::Delete<LRFCapture_URG>);
  }
  
};


