// -*- C++ -*-
/*!
 * @file  Urg_to_Obstacles.cpp * @brief compornent * $Date$ 
 *
 * $Id$ 
 */
#include "Urg_to_Obstacles.h"
#include <math.h>

// Module specification
// <rtc-template block="module_spec">
static const char* urg_to_obstacles_spec[] =
  {
    "implementation_id", "Urg_to_Obstacles",
    "type_name",         "Urg_to_Obstacles",
    "description",       "compornent",
    "version",           "1.0",
    "vendor",            "MyName",
    "category",          "example",
    "activity_type",     "SPORADIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.obstacle_max_radius", "0.05",
    "conf.default.max_dist", "3.0",
    "conf.default.urg_res", "0.25",
    "conf.default.urg_anglefront", "540",
    "conf.default.urg_anglemin", "0",
    "conf.default.urg_pos_x", "0.15",
    "conf.default.urg_pos_y", "0.0",
    "conf.default.urg_pos_theta", "0.0",
    ""
  };
// </rtc-template>

Urg_to_Obstacles::Urg_to_Obstacles(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_obstacleOut("obstacle", m_obstacle),
    m_urgIn("urg", m_urg)

    // </rtc-template>
{

  registerInPort("urg", m_urgIn);

  registerOutPort("obstacle", m_obstacleOut);
}

Urg_to_Obstacles::~Urg_to_Obstacles()
{
}


RTC::ReturnCode_t Urg_to_Obstacles::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("obstacle_max_radius", m_obstacle_max_radius, "0.05");
  bindParameter("max_dist", m_max_dist, "3.0");
  bindParameter("urg_res", m_urg_res, "0.25");
  bindParameter("urg_anglefront", m_urg_anglefront, "540");
  bindParameter("urg_anglemin", m_urg_anglemin, "0");
  bindParameter("urg_pos_x", m_urg_pos_x, "0.0");
  bindParameter("urg_pos_y", m_urg_pos_y, "0.0");
  bindParameter("urg_pos_theta", m_urg_pos_theta, "0.0");

  printf("URG to Obstacle Component ver.0.1 \n Initialized\n\n");
  // </rtc-template>
  return RTC::RTC_OK;
}



RTC::ReturnCode_t Urg_to_Obstacles::onExecute(RTC::UniqueId ec_id)
{
  int angleMin = m_urg_anglemin;
  int angleFront = m_urg_anglefront;
  double res = 135. / (double) angleFront;
  double startAngle = floor(res * angleMin - 135.);

  double x,y;
#ifdef LOGGING
  FILE *range;
#endif

  if (!m_urgIn.isEmpty()) {
    double angle = startAngle * M_PI / 180.;
    double stepSize = res * M_PI / 180.;
    m_urgIn.read();

    int obst_num = 0;
    printf("urg_get\n");

#ifdef LOGGING
    range = fopen("range","w");
#endif 
    m_obstacle.obstacles.length(m_urg.data.length());
    for (size_t i = 0; i < m_urg.data.length(); i++) {
      angle += stepSize;
//      if (angle < -M_PI/2 || angle > M_PI/2) continue;
      if (angle < -3*M_PI/8 || angle > 3*M_PI/8) continue;
      double reading = (double) m_urg.data[i] * 0.001;	// URG output is in millimeters, we want meters
      if(reading < 0.05)continue;
      x = reading * cos(angle);
      y = reading * sin(angle);
#ifdef LOGGING 
      fprintf(range,"%f %f\n",x,y);
#endif  
      m_obstacle.obstacles[obst_num].x = 
	x*cos(m_urg_pos_theta)-y*sin(m_urg_pos_theta)+m_urg_pos_x;
      m_obstacle.obstacles[obst_num].y = 
	x*sin(m_urg_pos_theta)+y*cos(m_urg_pos_theta)+m_urg_pos_y;
      m_obstacle.obstacles[obst_num].r = 0.05;
      obst_num++;
    }

printf("ObstNum= %d\n",obst_num);

    m_obstacle.obstacles.length(obst_num);
    m_obstacleOut.write();
#ifdef LOGGING
    fclose(range);  
#endif 
  }


  return RTC::RTC_OK;
}


extern "C"
{
 
  void Urg_to_ObstaclesInit(RTC::Manager* manager)
  {
    RTC::Properties profile(urg_to_obstacles_spec);
    manager->registerFactory(profile,
                             RTC::Create<Urg_to_Obstacles>,
                             RTC::Delete<Urg_to_Obstacles>);
  }
  
};



