//hw3.C
//CS576 final project.  4/15/00,  Zuojun Shen

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h> 
#include <iostream.h>
#include <fstream.h>   
#include<ctype.h> 

// Things that are used from the LEDA library
#include <LEDA/graph.h>
#include <LEDA/list.h>
#include <LEDA/point.h>
#include <LEDA/circle.h>
#include <LEDA/polygon.h>
#include <LEDA/ps_file.h>
#include <LEDA/random_source.h>
#include <LEDA/window.h>
#include <LEDA/vector.h>
#include "robot.h"

#define INFINITY 1.0e40
robot arrow_point;
random_source R;
int max_iter;
double stepsize,tolerance,delta,DeltaT;
vector LowerState(4),UpperState(4),InitState(4),GoalState(4);

list<circle> Obst;  // Obstacle region
list<polygon> Robot;  // Robot with thrust arrow

window W(600,600); 
ps_file Fps(15.0,15.0,"process.ps");
GRAPH<vector,double> G;
list<circle> OBST;

void READIN_PRBLM(char *fname );
vector RandomState();

bool COLLISION_FREE(vector newstate);
node NearestNeighbor(vector random_state);
double Metric(const vector &x1, const vector &x2);

double Angle_with_X(double x,double y);
double SelectBestInput(const vector &x1, const double & u_x1, const vector &x2, 
			    vector &nx_best, bool &success,int itr);
vector Integrate(const vector &x, const double &u, const double &h);
vector StateTransitionEquation(const vector &x, const double &u);
bool Extend(const vector &x, node &nn, node &n_best,int itr);
bool RRT_SEARCH(int max_iter,node &nn,double tolerance);
list<vector> PathToLeaf(const node &n);
void SHOW_PATH(const list<vector> &ar);
void SHOW_ROBOT_ARROW(double x, double y,double alpha);
void SHOW_TRAJ(const list<vector> &ar);
void MakeAnimatedGif(const list<vector> &Path);
double sigma_angle(double att);
double DIST_TO_GOAL(vector x1,vector x2);
main(int argc, char **argv)
{ 
char *inputfile;
int my;
bool success;
node final_nade;
list<vector> ar;
point p;
inputfile = argv[1];

W.init(-20,120,-20); 
Fps.init(-20,120,-20);  
W.display();
READIN_PRBLM(inputfile);

success = RRT_SEARCH(max_iter,final_nade,tolerance);

if(success)
  {
    cout<<"The path has been successfully found"<<endl;
ar = PathToLeaf(final_nade);
cout<<"final node=  "<<G.inf(final_nade)<<endl;
cout<<"length=  "<<ar.length()<<endl;
    SHOW_PATH(ar);
  }
else
    cout<<"The path couldn't been found in "<<max_iter<<" iterations."<<endl;

   my = W.read_mouse(p);
MakeAnimatedGif(ar);

}
  

void READIN_PRBLM(char *fname )
{

point target,Obst_center;
double radius, x_move,y_move;
node n;
ifstream ins;
ins.open(fname);

//Draw the obstacles
if(isalpha(ins.peek())) ins.ignore(100,'\n');
while(!ins.eof() && isdigit(ins.peek())){
ins>>Obst_center>>radius;
     W.draw_disc(Obst_center,radius);
     Fps.draw_disc(Obst_center,radius);

ins.ignore(100,'\n');
OBST.push(circle(Obst_center,radius));
}
//Draw the target
ins.ignore(100,'\n');
ins>>GoalState[0]>>GoalState[1]>>GoalState[2]>>GoalState[3];
ins.ignore(100,'\n');
     W.draw_circle(point(GoalState[0],GoalState[1]),delta);
     Fps.draw_circle(point(GoalState[0],GoalState[1]),delta);
//Draw the robot
ins.ignore(100,'\n');
ins>>arrow_point;
x_move = -arrow_point.mass.xcoord();
y_move = -arrow_point.mass.ycoord();


  arrow_point.mass = arrow_point.mass.translate(x_move,y_move);
     W.draw_disc(arrow_point.mass,arrow_point.center);
     Fps.draw_disc(arrow_point.mass,arrow_point.center);

  arrow_point.arrow = arrow_point.arrow.translate(x_move,y_move);
  arrow_point.arrow = arrow_point.arrow.rotate(arrow_point.mass,-90/57.3);

//SHOW_ROBOT_ARROW(x_move,y_move,-90/57.3);

ins.ignore(100,'\n');
ins>>InitState[0]>>InitState[1]>>InitState[2]>>InitState[3];
n = G.new_node(InitState);

ins.ignore(100,'\n');

ins.ignore(100,'\n');
ins>>LowerState[0]>>LowerState[1]>>LowerState[2]>>LowerState[3];

ins.ignore(100,'\n');
ins>>UpperState[0]>>UpperState[1]>>UpperState[2]>>UpperState[3];
ins.ignore(100,'\n');

ins.ignore(100,'\n');
ins>>max_iter>>tolerance>>delta>>DeltaT;

ins.close();
}


bool COLLISION_FREE(vector newstate)
{
 bool is_outside;
 circle obst;
 forall(obst,OBST)
   {
     if(obst.inside(point(newstate[0],newstate[1])))
        return false;
    }

is_outside =(newstate[0]<=LowerState[0]  || newstate[0]>=UpperState[0] || 
             newstate[1]<=LowerState[1]  || newstate[1]>=UpperState[1] || 
             newstate[2]<=LowerState[2]  || newstate[2]>=UpperState[2] || 
             newstate[3]<=LowerState[3]  || newstate[3]>=UpperState[3]);
if(is_outside)return false;
return true;
}

vector RandomState()
{
  int i;
  double r;
  vector rx;
  rx = LowerState;

  while(!COLLISION_FREE(rx))
    {
    R >> r;
    if(r<0.75)
  
        for (i = 0; i < 4; i++) {
          R >> r; 
          rx[i] = r * (UpperState[i] - LowerState[i]);
          rx[2] = 0;
          rx[3] = 0; }
                      
    else 
      {
      if(0.75 <= r && r <= 0.95){
        for (i = 0; i < 4; i++) {
          R >> r; 
          if(i<2)
          rx[i] = (r-0.5) * (UpperState[i] - LowerState[i])*0.02 + GoalState[i]; 
          if(i>1)
          rx[i] = (r-0.5) * (UpperState[i] - LowerState[i])*0.04 + GoalState[i]; }}

     else
       rx = GoalState; 
      }

   }
  return rx;
}

node NearestNeighbor(vector random_state)
{
  double d,d_min;
  node n,n_best;
  
  n_best = n = G.first_node(); // Keeps the warnings away
  d_min = INFINITY; d = 0.0;

  forall_nodes(n,G) {
    d = Metric(G[n],random_state);
    if (d < d_min) {
      d_min = d; n_best = n; 
    }
  }

//n_best = G.last_node();

  return n_best;
}

double Metric(const vector &x1, const vector &x2)
{
  double rho,rho1,weight,velocity_angle, direction_angle,angle_bias;
  
//  rho = sqrt((x1[0] - x2[0])*(x1[0] - x2[0]) + (x1[1] - x2[1])*(x1[1] - x2[1]));
  velocity_angle = Angle_with_X(x1[2], x1[3]);
  direction_angle = Angle_with_X((x2[0]-x1[0]),(x2[1]-x1[1]));
  angle_bias = velocity_angle - direction_angle;
//  rho = rho * (angle_bias*angle_bias + 1.);

 rho = sqrt((x1[0] - x2[0])*(x1[0] - x2[0]) + (x1[1] - x2[1])*(x1[1] - x2[1]));
 rho1 = sqrt((x1[2] - x2[2])*(x1[2] - x2[2]) + (x1[3] - x2[3])*(x1[3] - x2[3]));
   weight = ((rho + 7.) / (rho + 1.))*((rho + 7.) / (rho + 1.));
 rho = sqrt(rho*rho + weight*rho1*rho1);
 rho = rho * (angle_bias*angle_bias + 1.);

  return rho;
}

double Angle_with_X(double x,double y)
{
 double angle;
 if(y >= 0.)
 angle = acos(x/sqrt(x*x + y*y));
 if(y < 0.)
 angle = -acos(x/sqrt(x*x + y*y));
return angle;
}

double SelectBestInput(const vector &x1, const double & u_x1, const vector &x2, 
			    vector &nx_best, bool &success,int itr)
{
  list<double> il; 
  vector nx;
  int i;
  double d,d_min,u,u_best;
  success = false;
//  d_min = INFINITY;
u_best = u_x1;

  d_min = Metric(x1,x2);
if(x1 == InitState)
   for(i=-15;i<=15;i++)il.push(u_x1 + i*12./57.3);
else
  for(i=-10;i<=10;i++)il.push(u_x1 + i*15/57.3);

/*
u_best=sigma_angle(itr*DeltaT);
il.push(u_best);
*/

  forall(u,il) {
      nx = Integrate(x1,u,DeltaT);
      d  = Metric(nx,x2);

    if ((d < d_min)&&(COLLISION_FREE(nx))&&(x1 != nx)) 
      {
	d_min = d; u_best = u; nx_best = nx; success = true;
      }
  }
if(success){
//cout<<"sel   "<<"Metric1=   "<<Metric(x1,x2)<<endl;
//cout<<"Metric2=   "<<Metric(nx_best,x2)<<endl;
}
  return u_best;
}

vector Integrate(const vector &x, const double &u, 
		  const double &h) 
{
  vector k1,k2,k3,k4;

  k1 = StateTransitionEquation(x,u);
  k2 = StateTransitionEquation(x + (0.5*h)*k1,u);
  k3 = StateTransitionEquation(x + (0.5*h)*k2,u);
  k4 = StateTransitionEquation(x + h*k3,u);

  return x + h/6.0*(k1 + 2.0*k2 + 2.0*k3 + k4);
}

vector StateTransitionEquation(const vector &x, const double &u)
{
  vector dx(4);

    dx[0] = x[2];
    dx[1] = x[3];
    dx[2] = cos(u) - 1./7.*x[2];
    dx[3] = sin(u) - 1./7.*x[3];
  return dx;
}

bool Extend(const vector &x, node &nn, node &n_best,int itr)
{
  vector nx;
  double u_best,u_nbest;
  bool success;

  n_best = NearestNeighbor(x);

  if(G.inf(n_best) == InitState)u_nbest = 0.;
  else
     u_nbest =G.inf( G.first_in_edge(n_best));

  u_best = SelectBestInput(G.inf(n_best),u_nbest,x,nx,success,itr);

  if (success) {   // If a collision-free input was found
    nn = G.new_node(nx); // Make a new node with state nx
    G.new_edge(n_best,nn,u_best);
  }

if(x == GoalState && success){
//cout<<"Metric1=   "<<Metric(G.inf(n_best),x)<<endl;
//cout<<G.inf(n_best)<<endl;
cout<<itr<<"   Metric2=   "<<Metric(nx,x)<<endl;
//cout<<success<<"    "<<nx<<"     "<<u_nbest<<endl;
}


  return success;
}


bool RRT_SEARCH(int max_iter,node &nn, double tolerance)
{
  vector x_random,x1,x2;
  bool success; 
  node n_best;
  int iter=0;
  double distance_to_goal=100.0;

  while(iter <= max_iter && distance_to_goal>tolerance)
    {
      x_random = RandomState();
      success = Extend(x_random, nn, n_best,iter);
      x1 = G.inf(n_best);
      x2 = G.inf(nn);
      if(success)
         {
 //cout<<"                   xdraw"<<endl;
            W.draw_segment(x1[0],x1[1],x2[0], x2[1]);
            Fps.draw_segment(x1[0],x1[1],x2[0], x2[1]);

         }
//      distance_to_goal = sqrt((x2[0]-GoalState[0])*(x2[0]-GoalState[0]) + 
//                              (x2[1]-GoalState[1])*(x2[1]-GoalState[1]));

distance_to_goal = DIST_TO_GOAL(x1,x2);

//cout<<"iter  "<<iter;
//cout<<"   distance_to_goal  "<<distance_to_goal<<"   "<<tolerance<<endl;
     iter++;
    }
if(distance_to_goal < tolerance)return true;
return false;
}


double DIST_TO_GOAL(vector x1,vector x2)
{
 double d1,d2,d3,d4,d;
 d1 = Metric(x2,GoalState);
 d2 = Metric(x1+0.5*(x2-x1),GoalState);
 d3 = Metric(x1+0.25*(x2-x1),GoalState);
 d4 = Metric(x1+0.75*(x2-x1),GoalState);
 
 if(d1 > d2)d = d2;
 if(d2 >= d1)d = d1;
 if(d > d3)d = d3;
 if(d > d4)d = d4;

return d;
}  


list<vector> PathToLeaf(const node &n) 
{

   list<vector> path;
   node ni;
   int i;
   vector graph_node,path_node(3);

   path.clear();
   i = 0;
   ni = n;
graph_node = G.inf(ni);
  path_node[0] =   graph_node[0];
  path_node[1] =   graph_node[1];  
  path_node[2] = G.inf(G.first_in_edge(ni)); 

path.push(path_node);
   while ((ni != G.first_node())&&(i < G.number_of_nodes())) {
graph_node = G.inf(ni);
  path_node[0] =   graph_node[0];
  path_node[1] =   graph_node[1];  
  path_node[2] = G.inf(G.first_in_edge(ni)); 
  path.push(path_node);
     ni = G.source(G.first_in_edge(ni));
     i++;
   }

   return path;
}


void SHOW_PATH(const list<vector> &ar)
{
int my,i=0;
point p;
vector robot,former_position;
double thrust_angle;
vector robot_state,rr;
W.set_line_width(2);
Fps.set_line_width(2);

 SHOW_TRAJ(ar);

   forall(robot,ar) {
if(i%3 ==0)
SHOW_ROBOT_ARROW(robot[0],robot[1],robot[2]);
i++;
     }
Fps.close();
cout<<"Finished:"<<endl;
return;
}


void SHOW_TRAJ(const list<vector> &ar)
{
 vector rob,former_position;
 int i=0;
   forall(rob,ar) {
     if(i>0){
        W.draw_segment(rob[0],rob[1],former_position[0], former_position[1]);  
        Fps.draw_segment(rob[0],rob[1],former_position[0], former_position[1]);}
     former_position = rob;
     i++;
   } 
//Fps.close();
}


void SHOW_ROBOT_ARROW(double x, double y,double alpha)
{

  arrow_point.mass = arrow_point.mass.translate(x,y);
     W.draw_disc(arrow_point.mass,arrow_point.center);
     Fps.draw_disc(arrow_point.mass,arrow_point.center);

  arrow_point.arrow = arrow_point.arrow.translate(x,y);
  arrow_point.arrow = arrow_point.arrow.rotate(arrow_point.mass,alpha);
     W.draw_filled_polygon(arrow_point.arrow);
     Fps.draw_filled_polygon(arrow_point.arrow);

  arrow_point.arrow = arrow_point.arrow.rotate(arrow_point.mass,-alpha);
  arrow_point.mass = arrow_point.mass.translate(-x,-y);
  arrow_point.arrow = arrow_point.arrow.translate(-x,-y);
}


void MakeAnimatedGif(const list<vector> &Path)
{
 vector n,robot,former_position,starting_point;
 int anim,i=0,j=1,k=0;
 polygon arrow; 
 circle obs;
 string fname;
 anim = Path.length() / 50;
    forall(n,Path) {
if(i == 0)starting_point = n;
if(i%anim ==0)
  {
      fname = string("frame%04d.ps",j);
      ps_file f(15.0,15.0,fname);
f.init(-20,120,-20); 
      //Draw the path
former_position = starting_point;
    forall(robot,Path) {
     if(k>0)
        f.draw_segment(robot[0],robot[1],former_position[0], former_position[1]);     
     former_position = robot;
     k++;
   }  
      // Draw obstacles
      forall(obs,OBST) 
	f.draw_disc(obs);

      // Draw initial and goal states
     f.draw_circle(point(GoalState[0],GoalState[1]),delta);
     f.draw_disc(arrow_point.mass,arrow_point.center);

      // Draw robot

  arrow_point.mass = arrow_point.mass.translate(n[0],n[1]);
     f.draw_disc(arrow_point.mass,arrow_point.center);
  arrow_point.arrow = arrow_point.arrow.translate(n[0],n[1]);
  arrow_point.arrow = arrow_point.arrow.rotate(arrow_point.mass,n[2]);
     f.draw_filled_polygon(arrow_point.arrow);

  arrow_point.arrow = arrow_point.arrow.rotate(arrow_point.mass,-n[2]);
  arrow_point.mass = arrow_point.mass.translate(-n[0],-n[1]);
  arrow_point.arrow = arrow_point.arrow.translate(-n[0],-n[1]);
j++;
      f.close();
  }
      i++;
    }
}


double sigma_angle(double e)
{
 int index, i,j,index_search_up, index_search_low;
double sigma,e_end,e_node[20],sigma_node[20], x_from_for[21]={ 0.29798605861855E+02,
                       0.45073835878815E+02,
                       0.45074330854816E+02,
                       0.45127493357663E+02,
                       0.45149470263317E+02,
                       0.45206197490287E+02,
                       0.45301094579988E+02,
                       0.45376963235648E+02,
                       0.45466891226267E+02,
                       0.45587751683131E+02,
                       0.45776492335464E+02,
                       0.45999053729059E+02,
                       0.46435713061261E+02,
                       0.46737998670813E+02,
                       0.49038469438218E+02,
                       0.45631733371576E+02,
                       0.73849986106490E+02,
                      -0.55099619267434E+02,
                      -0.16284131679238E+03,
                      -0.13994670439578E+03,
                      -0.14215171108078E+03};

e_end = x_from_for[0];
for(i=0;i<20;i++)sigma_node[i]=x_from_for[i+1]/57.296;
for(j=0;j<20;j++)e_node[j] = j * e_end/19.0;
index_search_up = 19;
index_search_low= 0;
index = (index_search_up + index_search_low)/2;
if(e<=e_node[0])index = 0;
if(e>=e_node[19])index = 19;
if(e>e_node[0] && e<e_node[19])

  {
    while(index_search_up > index_search_low+1)
         {
          if(e >= e_node[index])index_search_low = index;
          if(e <= e_node[index])index_search_up = index;
          index = (index_search_up + index_search_low)/2;
	 }
    sigma =  sigma_node[index] + (sigma_node[index+1] - sigma_node[index])*
              (e - e_node[index])/(e_node[index+1] - e_node[index]);
  }
else
    sigma =  sigma_node[index];
return sigma;
}