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

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

using namespace std;

// Module specification
// <rtc-template block="module_spec">
static const char* obstacleavoidance_spec[] =
  {
    "implementation_id", "CollisionDetection",
    "type_name",         "CollisionDetection",
    "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.acceleration", "0.1",
    "conf.default.space", "0.5",
    "conf.default.interval_length", "0.3",

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

CollisionDetection::CollisionDetection(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_velOut("out_vel", m_out_vel),
    
    // </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", m_out_velOut);

}

CollisionDetection::~CollisionDetection()
{
}


RTC::ReturnCode_t CollisionDetection::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("body_radius", m_body_radius, "0.5");
  bindParameter("acceleration", m_acceleration, "0.1");
  bindParameter("space", m_space, "0.5");
  bindParameter("interval_length", m_interval_length, "0.3");

  printf("Collieion Detection Component ver.0.1\nInitialized\n\n");
  
  // </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;
}

RTC::ReturnCode_t CollisionDetection::onExecute(RTC::UniqueId ec_id)
{
  //UnsignedLongSeq* scan = new UnsignedLongSeq();
  // double timestamp;
  double target_v,target_w;
  //Point points[1200];	  double x = 0;
#if PLOT
  static FILE *gnuplot;
  FILE *path_file,*target_file;
#endif 
  static int count;
  double run_length,distance;

  //障害物及び、速度情報が入力されているか
  if(!m_obstaclesIn.isEmpty() && !m_in_velIn.isEmpty()){
    printf("hoge\n");
    count++;


    //速度情報の読み込み
    while(!m_in_velIn.isEmpty())m_in_velIn.read();
    //障害物情報の読み込み
    while(!m_obstaclesIn.isEmpty())m_obstaclesIn.read();
   
    //空っぽなら帰る
    if(m_obstacles.obstacles.length()==0)return RTC::RTC_OK;
    
    //目標値の設定
    target_v =   (double)m_in_vel.data.vx;
    target_w =   (double)m_in_vel.data.va;
#ifdef PLOT
    //デバッグ用：gnuplotの起動
    if(!gnuplot){
      gnuplot=popen("/usr/bin/gnuplot","w");
      fprintf(gnuplot,"set term x11\n");
      fprintf(gnuplot,"set xrange [-4:4]\n");
      fprintf(gnuplot,"set yrange [-3:3]\n");
    }
#endif 
    //テーブルの初期化
    init_table();

    //障害物情報の登録と、一番近い点の探索
    double shortestReading = DBL_MAX;
    for (size_t i = 0;i<m_obstacles.obstacles.length(); i++){ 
      double reading = 
	m_obstacles.obstacles[i].x*m_obstacles.obstacles[i].x+ 
	m_obstacles.obstacles[i].y*m_obstacles.obstacles[i].y;
      
      add_point( m_obstacles.obstacles[i].x,m_obstacles.obstacles[i].y);
      
      //一番近い点
      if (reading < shortestReading) shortestReading = reading;
    }

    //障害物情報のテーブルへの登録
    regist_points();    
    
    double bestSpeed = 0., bestJog = 0.;
    double dspeed = 0.4/10;
    
    //曲率の計算
    double target_rot_ir,last_rot_ir;
    //目標とする軌跡の曲率
    if(fabs(target_v)>0.001)target_rot_ir = target_w/target_v;
    else target_rot_ir=10000;//その場回転
    //前回の軌跡の曲率
    if(fabs(lastSpeed)>0.001)last_rot_ir = lastJog/lastSpeed;
    else last_rot_ir=10000;//その場回転
    
    
    if (shortestReading < 0.20*0.20||
	(fabs(target_v)<0.05 && fabs(target_w)<0.001) ){//近すぎなら速度・角速度は０ 
      bestSpeed = 0.; bestJog = 0.;

    }else if(fabs(target_v)<0.05 ){//遅いときは回転だけ 
      bestSpeed = 0.; bestJog = target_w;

    }else{//そうでなければ軌跡を計算 
      double pathCost;
            
      //経路のコストの計算
      pathCost = traj_check(target_v, target_w, m_body_radius, 0.05,
			    &run_length, &distance);
      
      //最大安全速度の計算
      double max_speed;
      if(run_length>m_space)
	max_speed = sqrt((run_length-m_space)*2*m_acceleration);
      else 
	max_speed = 0;
      
      //length = v*v/(a*2) 
      //v=sqrt(length*2*a)
      printf("%f %f %f\n",run_length, distance, max_speed);
      
      //速度の決定
      if(max_speed > target_v){
	//目標速度で衝突してないならそのまま出力
	printf("tracking...");
	
	bestSpeed = target_v;
	bestJog   = target_w;	
      }else{ 
	/*減速すれば大丈夫そうなら減速して済ます。*/
	printf("slow...%f ",max_speed);
	
	if(fabs(target_v)>0.01)
	  bestJog=target_rot_ir*max_speed;
	else
	  bestJog=target_w;
	
	bestSpeed=max_speed;
      }
    }
    
    //加速度制限
    if(lastSpeed>0){
      if( bestSpeed > lastSpeed+dspeed/5.){
	bestSpeed= lastSpeed + dspeed/5.;
      }
    }else{
      if( bestSpeed < lastSpeed-dspeed/5. ){
	bestSpeed=lastSpeed - dspeed/5.;
      }
    }
    
    //角加速度制限
    /*   
	 if( bestJog > lastJog+djog/4 ){
	 bestJog = lastJog + djog/4;
	 }else if(bestJog < lastJog-djog/4 ){
	 bestJog = lastJog-djog/4; 
	 }
    */
    
    //目標速度を出力
    m_out_vel.data.vx = bestSpeed;
    m_out_vel.data.vy = 0;
    m_out_vel.data.va = bestJog;
    m_out_velOut.write();    
    
    //デバッグ
    printf(" %f %f %f %f \n", target_v, bestSpeed, target_w,  bestJog);
    
    //前回値を保存
    lastSpeed = bestSpeed;
    lastJog   = bestJog; 
  }
  //  else printf("Cannot read from URG service.\n");
  return RTC::RTC_OK;
}

extern "C"
{
 
  void CollisionDetectionInit(RTC::Manager* manager)
  {
    RTC::Properties profile(obstacleavoidance_spec);
    manager->registerFactory(profile,
                             RTC::Create<CollisionDetection>,
                             RTC::Delete<CollisionDetection>);
  }
  
};


