// -*- C++ -*-
/*!
 * @file  LRFViewer.cpp
 * @brief LRF Viewer
 * @date $Date$
 *
 * $Id$
 */
#include <cmath>
#include "LRFViewer.h"

#include "interface.h"
#include "support.h"

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

// Module specification
// <rtc-template block="module_spec">
static const char* lrfviewer_spec[] =
  {
    "implementation_id", "LRFViewer",
    "type_name",         "LRFViewer",
    "description",       "LRF Viewer",
    "version",           "1.0.0",
    "vendor",            "Tsuyoshi Suenaga, NAIST",
    "category",          "viewer",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.m_per_pixel", "0.01",
    "conf.default.screen_offset_x", "0",
    "conf.default.screen_offset_y", "0",
    "conf.default.position_x", "0.0",
    "conf.default.position_y", "0.0",
    "conf.default.position_theta_deg", "0.0",


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

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

  initialize_for_gtk();
}

LRFViewer::~LRFViewer()
{
}



RTC::ReturnCode_t LRFViewer::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("m_per_pixel", m_m_per_pixel, "0.01");
  bindParameter("screen_offset_x", m_screen_offset_x, "0");
  bindParameter("screen_offset_y", m_screen_offset_y, "0");
  bindParameter("position_x", m_position_x, "0.0");
  bindParameter("position_y", m_position_y, "0.0");
  bindParameter("position_theta_deg", m_position_theta_deg, "0.0");
  
  // </rtc-template>
  return RTC::RTC_OK;
}

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

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

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


RTC::ReturnCode_t LRFViewer::onActivated(RTC::UniqueId ec_id)
{
  // ウィンドウの作成
  //window1 = create_window1 ();
  gui = create_window1 ();
  // gtkスタート
  g_thread_create((GThreadFunc)gtk_main_thread, NULL, FALSE, NULL);

  // サービスポートからパラメータを取得
  laser.total_points = m_LRF->getTotalScanPoints();
  laser.resolution = m_LRF->getResolution();
  laser.offset_fov = m_LRF->getOffsetFOV();

  laser.fov = (laser.total_points - 1) * laser.resolution;
  laser.range.resize(laser.total_points);

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

  return RTC::RTC_OK;
}



RTC::ReturnCode_t LRFViewer::onDeactivated(RTC::UniqueId ec_id)
{
  laser.range.clear();
  // ウィンドウ終了
  destroy_window(gui);
  // gtk終了
  gtk_main_quit();
  return RTC::RTC_OK;
}



RTC::ReturnCode_t LRFViewer::onExecute(RTC::UniqueId ec_id)
{
  int i;
  int width, height;
  Point pose;

  pose.x = m_position_x;
  pose.y = m_position_y;
  pose.theta = DEG2RAD(m_position_theta_deg);

  if(m_RangeDataIn.isNew()){
    // データの取得
    m_RangeDataIn.read();
    for(i=0; i<laser.total_points; i++){
      laser.range.at(i) = m_RangeData.data[i]/1000.0;
      //laser.range.push_back(m_RangeData.data[i]);
    }
    std::cout << laser.range[0] << std::endl;

    gdk_threads_enter();
    // 表示のオフセットを設定する
    set_screen_offset(m_screen_offset_x, m_screen_offset_y);
    // 表示の解像度を設定する
    set_screen_resolution(m_m_per_pixel);

    // pixmapを初期化する
    draw_area_initialize(gui);
    // LRFデータをpixmapに描画
    draw_laser_range(gui, &laser, &pose);
    // pixmapにセンサ中心を描画
    draw_point(gui, &pose, 10, make_color(0x9999, 0x9999, 0x9999));
    // pixmapを描画
    show_draw_area(gui);
    gdk_threads_leave();
  }

  // ウィンドウサイズを取得する
  gdk_threads_enter();
  get_draw_size(gui, &width, &height);
  gdk_threads_leave();
//   fprintf(stderr, "(%4d,%4d)\n",
// 	  width, height);

  return RTC::RTC_OK;
}


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

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

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

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

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



extern "C"
{
 
  void LRFViewerInit(RTC::Manager* manager)
  {
    RTC::Properties profile(lrfviewer_spec);
    manager->registerFactory(profile,
                             RTC::Create<LRFViewer>,
                             RTC::Delete<LRFViewer>);
  }
  
};


