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

#include "ObstacleMonitor.h"
#include "nearest.h"
#include <math.h>
#include <float.h>

using namespace std;

// Module specification
// <rtc-template block="module_spec">
static const char* ObstacleMonitor_spec[] =
  {
    "implementation_id", "ObstacleMonitor",
    "type_name",         "ObstacleMonitor",
    "description",       "compornent",
    "version",           "1.0",
    "vendor",            "MyName",
    "category",          "example",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.body_radius", "0.5",
    "conf.default.speed_weight", "0.3",
    "conf.default.turn_weight", "0.3",
    "conf.default.length_weight", "0.3",
    "conf.default.interval_length", "0.3",
    "conf.default.space", "0.5",
    "conf.default.acceleration", "0.1",
		"conf.default.pthre_1", "400.0",
		"conf.default.pthre_2", "550.0",
		"conf.default.waittime", "5.0",
    ""
  };
// </rtc-template>

ObstacleMonitor::ObstacleMonitor(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_in_velIn("in_vel", m_in_vel),
    m_obstaclesIn("obstacles", m_obstacles),
    m_out_vel_stpOut("out_vel_stp", m_out_vel_stp),
    m_out_vel_avdOut("out_vel_avd", m_out_vel_avd),
    m_obstacles_stpOut("obstacles_stp", m_obstacles_stp),
    m_obstacles_avdOut("obstacles_avd", m_obstacles_avd),
    m_stateOut("state", m_state),
    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("in_vel", m_in_velIn);
  registerInPort("obstacles", m_obstaclesIn);
  
  // Set OutPort buffer
  registerOutPort("out_vel_stp", m_out_vel_stpOut);
  registerOutPort("out_vel_avd", m_out_vel_avdOut);
  registerOutPort("obstacles_stp", m_obstacles_stpOut);
  registerOutPort("obstacles_avd", m_obstacles_avdOut);
  registerOutPort("state", m_stateOut);
  
}

ObstacleMonitor::~ObstacleMonitor()
{
}


RTC::ReturnCode_t ObstacleMonitor::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("body_radius", m_body_radius, "0.1");
  bindParameter("speed_weight", m_speed_weight, "0.3");
  bindParameter("turn_weight", m_turn_weight, "0.3");
  bindParameter("length_weight", m_length_weight, "0.3");
  bindParameter("interval_length", m_interval_length, "0.3");
  bindParameter("space", m_space, "0.5");
  bindParameter("acceleration", m_acceleration, "0.1");
  bindParameter("point_thread_1", m_pthre_1, "400.0");
  bindParameter("point_thread_2", m_pthre_2, "550.0");
	bindParameter("waittime", m_waittime, "5.0");

  printf("Obstacle Avoidance Component ver.0.1\nInitialized\n\n");

	point_current = 0.0;
	state = INITIAL;

  tv0.tv_sec = 0;
  tv1.tv_sec = 0;
  tv0.tv_usec = 0;
  tv1.tv_usec = 0;

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


double e_dist(double tx,double ty, double x, double y){
  return sqrt((tx-x)*(tx-x)+(ty-y)*(ty-y));
}

double e_heading(double tx,double ty, double x, double y,double theta){
  double angle;

  angle = atan2(ty-y,tx-x)-theta;
  
  if(angle > M_PI)angle -=M_PI/2;
  if(angle <-M_PI)angle +=M_PI/2;

  return fabs(angle);
}

double e_angle(double theta,double ttheta){
  double angle;

  angle = ttheta-theta;
  
  if(angle > M_PI)angle -=M_PI/2;
  if(angle <-M_PI)angle +=M_PI/2;

  return fabs(angle);
}
double e_near(double dist){
  if(dist > 1)return 0;
     return(fabs(1-dist));
}

double traj_check(double v, double w, double radius, double expand,
		  double *run_length, double *distance){
  double x = 0;
  double y = 0;
  double theta = 0;	
  double cost = 0;
  double length = 0,b_length=-1;
  double min_cost=DBL_MAX;
  double min_dist=DBL_MAX;
  double dt = 0.1;
  int i=0;

  for (int i = 0; i < 50; i++) {
    //軌跡を計算
    x += v*dt*cos(theta);
    y += v*dt*sin(theta);
    theta += w*dt;
    length += fabs(dt*v);
    if(b_length +0.1> length)continue;
    b_length = length;

    //最近傍点を計算
    double dist = search_min_dist(x,y);
    
    if(min_dist > dist)min_dist = dist;
    
    cost = 0;//e_dist(tx,ty,x,y) 
      //  +    e_heading(tx,ty,x,y,theta)
      //+    e_angle(ttheta,theta)/20.0,
      //+    e_near(min_dist-radius)*m_distWeight
      //+    (fabs(target_v)*50*dt-length)*m_lengthWeight
      //+    fabs(jog -target_w)*m_angleWeight;
    if(cost < min_cost)min_cost = cost;  
    
    //衝突していないか
    if(dist < radius + length*expand){
      //v*v/(a*2)  
      if(length < v*v/(2*0.1))//衝突していて、かつ止まれないなら最大値
	min_cost = DBL_MAX;

      *run_length = length;
      *distance = min_dist;
      return min_cost;//止まれるなら、現在の最小コスト
    }
  }
  min_cost += pow(0.6 * std::max(2.0 - min_dist, 0.0), 2.);    // low obst distance -> high cost
  min_cost += 0.01 * std::max(10.0 - i * dt, 0.0);
  min_cost += 1.0 * fabs(sin(w));
  min_cost += 1.0 * std::max(10.0 - length, 0.0);


  *run_length = length;
  *distance = min_dist;

  return min_cost;
}

double gradeState(TimedObstacles obsts){
	double points = 0.0;
	double x,y; //ロボット前方がx, ロボット左方向がy
	double dist, angle;//障害物までの距離と方向(正面がゼロ．上から見てCCWが＋).
	for(int i=0;i<obsts.obstacles.length();i++){
			x = obsts.obstacles[i].x;
			y = obsts.obstacles[i].y;
			dist = sqrt(x*x+y*y);
			if(dist<0.5){
				angle = fabs(atan2(y,x));
				//障害物との距離がより短く，より正面に近いほど状態の危険度を示す点数が高くなる関数
				points += 2/dist * 2.57/(angle+1.0);
			}
	}
	return points;
}

void checkFront(TimedObstacles obsts, int *state){//stateは要素数3のint配列
	state[0] = 0;
	state[1] = 0;
	state[2] = 0;
	double x,y; //ロボット前方がx, ロボット左方向がy
	double dist, angle;//障害物までの距離と方向(正面がゼロ．上から見てCCWが＋).
	for(int i=0;i<obsts.obstacles.length();i++){
			x = obsts.obstacles[i].x;
			y = obsts.obstacles[i].y;
			dist = sqrt(x*x+y*y);
			if(dist<0.8){
				angle = atan2(y,x)*180.0/3.14;
				if(angle>=-45.0 && angle<-15.0)
					state[0] = 1;
				else if(angle>=-15.0 && angle<=15.0)
					state[1] = 1;
				else if(angle>15.0 && angle<=45.0)
					state[2] = 1;
			}
	}
	return;
}


RTC::ReturnCode_t ObstacleMonitor::onExecute(RTC::UniqueId ec_id)
{

  //障害物情報が入力されているかチェック
  if(!m_obstaclesIn.isEmpty()){

    //速度情報の読み込み
    while(!m_in_velIn.isEmpty())
			m_in_velIn.read();

    //障害物情報の読み込み
    while(!m_obstaclesIn.isEmpty())
			m_obstaclesIn.read();
   
    //障害物の検知なしなら
    if(m_obstacles.obstacles.length()!=0){
			checkFront(m_obstacles,frontState);
			if(frontState[1]==1){
				switch(state){
				case RUNNING:
					state = STOP;
					gettimeofday(&tv0,0);
					printf("どいてください\n");
					break;
				case STOP:
				case WAITING:
					state = WAITING;
					gettimeofday(&tv1,0);
					dsec = tv1.tv_sec - tv0.tv_sec;
		      dusec = tv1.tv_usec - tv0.tv_usec;
		      dusec = dsec*1000*1000+dusec;
					if(dusec>m_waittime*1000000){
						printf("回避します\n");
						state = AVOID;
						gettimeofday(&tv0,0);
					}
					break;
				case AVOID:
					gettimeofday(&tv1,0);
					dsec = tv1.tv_sec - tv0.tv_sec;
		      dusec = tv1.tv_usec - tv0.tv_usec;
		      dusec = dsec*1000*1000+dusec;
					if(dusec>(m_waittime+3.0)*1000000){
						printf("いい加減どいてください\n");
						state = RUNNING;
					}
				default:
					break;
				}
			}
			else{
				state = RUNNING;
			}
			//printf("Front State: %d %d %d\n",frontState[1],state);
		}
		m_state.id = state;
		m_stateOut.write();

		if(state==RUNNING || state==AVOID){
			m_out_vel_avd = m_in_vel;
			m_out_vel_avdOut.write();
			m_obstacles_avd = m_obstacles;
			m_obstacles_avdOut.write();		
		}
		else{
			m_out_vel_stp = m_in_vel;
			m_out_vel_stpOut.write();
			m_obstacles_stp = m_obstacles;
			m_obstacles_stpOut.write();
		}
  }
  //  else printf("Cannot read from URG service.\n");
  return RTC::RTC_OK;
}


extern "C"
{
 
  void ObstacleMonitorInit(RTC::Manager* manager)
  {
    RTC::Properties profile(ObstacleMonitor_spec);
    manager->registerFactory(profile,
                             RTC::Create<ObstacleMonitor>,
                             RTC::Delete<ObstacleMonitor>);
  }
  
};

