// -*- C++ -*-
/*!
 * @file  ObstacleAvoidance.h
 * @brief compornent
 * @date  $Date$
 *
 * $Id$
 */

#ifndef OBSTACLEAVOIDANCE_H
#define OBSTACLEAVOIDANCE_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include "intellirobotStub.h"


using namespace RTC;

class ObstacleAvoidance
  : public RTC::DataFlowComponentBase
{
 public:
  ObstacleAvoidance(RTC::Manager* manager);
  ~ObstacleAvoidance();

  virtual RTC::ReturnCode_t onInitialize();
  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

 protected:
  double m_body_radius;
  double m_speed_weight;
  double m_turn_weight;
  double m_length_weight;
  double m_interval_length;
  double m_space;
  double m_acceleration;

  IIS::TimedVelocity2D m_in_vel;
  InPort<IIS::TimedVelocity2D> m_in_velIn;
  TimedObstacles m_obstacles;
  InPort<TimedObstacles> m_obstaclesIn;
  
  IIS::TimedVelocity2D m_out_vel;
  OutPort<IIS::TimedVelocity2D> m_out_velOut;
  

 private:
  int dummy;
  double lastSpeed, lastJog;
};


extern "C"
{
  void ObstacleAvoidanceInit(RTC::Manager* manager);
};

#endif // OBSTACLEAVOIDANCE_H
