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

#ifndef ObstacleMonitor_H
#define ObstacleMonitor_H

#include <sys/time.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>

// Service implementation headers
// <rtc-template block="service_impl_h">

// </rtc-template>

// Service Consumer stub headers
// <rtc-template block="consumer_stub_h">
#include "intellirobotStub.h"

// </rtc-template>

using namespace RTC;

class ObstacleMonitor
  : public RTC::DataFlowComponentBase
{
 public:
  ObstacleMonitor(RTC::Manager* manager);
  ~ObstacleMonitor();
  virtual RTC::ReturnCode_t onInitialize();
  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

 protected:
  // Configuration variable declaration
  // <rtc-template block="config_declare">
  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;
	double m_pthre_1;
	double m_pthre_2;
	double m_waittime;

  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  IIS::TimedVelocity2D m_in_vel;
  InPort<IIS::TimedVelocity2D> m_in_velIn;
  TimedObstacles m_obstacles;
  InPort<TimedObstacles> m_obstaclesIn;
 
  // 一時停止側への出力
  IIS::TimedVelocity2D m_out_vel_stp;
  OutPort<IIS::TimedVelocity2D> m_out_vel_stpOut;
  TimedObstacles m_obstacles_stp;
  OutPort<TimedObstacles> m_obstacles_stpOut;
 
  // 回避側への出力
  IIS::TimedVelocity2D m_out_vel_avd;
  OutPort<IIS::TimedVelocity2D> m_out_vel_avdOut;
  TimedObstacles m_obstacles_avd;
  OutPort<TimedObstacles> m_obstacles_avdOut;

	IIS::TimedState m_state;
	OutPort<IIS::TimedState> m_stateOut;
  
 private:
  int dummy;
  double lastSpeed, lastJog;
	//待ち時間計測用
  struct timeval tv0, tv1;
  unsigned long dsec, dusec;

	//状態遷移監視用
	double point_current, pthre_1, pthre_2;
	int state;
	int frontState[3];
	enum {
		INITIAL = 0,
		NO_OBSTACLE,
		STOP,
		WAITING,
		AVOID,
		WARNING,
		RUNNING
	};
};


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

#endif // ObstacleMonitor_H
