#include <LEDA/point.h>
#include <LEDA/graph.h>
#include <LEDA/vector.h>
#include <LEDA/node_array.h>
#include <LEDA/edge_array.h>
#include <LEDA/window.h>
#include <LEDA/basic_graph_alg.h>
#include <LEDA/list.h>
#include <LEDA/segment.h>
#include <LEDA/ps_file.h>
#include <stdlib.h>

#define PI 3.1415926535897932385
#define MAXCYCLES 1000
#define INFINITE 999999

class Planner
{
 protected:
  float maxTime;
  point goal;

  list<segment> seg;
  void RedrawPSFile (obs &o);
  int fileNum = 1;
  float initTime;

  float Metric (Ship, point, float);

  void PathToLeaf(const node &end, list<vector> &path, window &w);
  void RandomState(float &angle, bool &fire);
  void ChooseState(Ship x, point rndPt, obs &o, float &angle, bool &fire);
  point ChooseNewPoint ();
  node NearestNeighbor(point &x, float time);
  bool Extend(point &x, node &newNode, obs &o, window &w);


 public:
  // The RRT graph
  GRAPH<Ship,vector> G;
  Planner();
  ~Planner();
  int Plan(point goal, list<vector> &path, 
	   Ship initShip, float time, obs &o, window &w, int num);
};

Planner::Planner()
{
}

Planner::~Planner()
{
}

float Planner::Metric (Ship s, point p1, float time)
{
  double x, dm;
  point p2 = s.GetPosition ();
  point p3 = p2.translate (double (s.vx), double (s.vy));

  x = p1.distance (p2);
  dm = p1.distance (p3);

  return (x - (x - dm) * 5);
}

int Planner::Plan(point pgoal, list<vector> &path, 
		   Ship initShip, float time, obs &o, window &w, int num)
{
  node homeNode, newNode;
  point rndPt;
  bool foundGoal = false;

  fileNum = num;
  seg.clear ();

  G.clear ();
  goal = pgoal;
  maxTime = 0;
  initTime = time;

  w.clear ();
  o.Display (w, time);

  w.draw_segment (int (goal.xcoord ()) - 1, int (goal.ycoord ()) - 1, 
		  int (goal.xcoord ()) + 1, int (goal.ycoord ()) - 1, 9);
  w.draw_segment (int (goal.xcoord ()) - 1, int (goal.ycoord ()) - 1, 
		  int (goal.xcoord ()) - 1, int (goal.ycoord ()) + 1, 9);
  w.draw_segment (int (goal.xcoord ()) - 1, int (goal.ycoord ()) + 1, 
		  int (goal.xcoord ()) + 1, int (goal.ycoord ()) + 1, 9);
  w.draw_segment (int (goal.xcoord ()) + 1, int (goal.ycoord ()) + 1, 
		  int (goal.xcoord ()) + 1, int (goal.ycoord ()) - 1, 9);
  

  w.draw_point (goal, 9);
  homeNode = G.new_node (initShip);

  for (int cntr = 0; cntr < MAXCYCLES && !foundGoal; cntr++)
  {
    cerr << cntr << '\n';
    rndPt = ChooseNewPoint ();

    if (Extend (rndPt, newNode, o, w))
    {
      cout << "FOUND" << flush;
      foundGoal = true;
    }
    if (cntr % 20 == 0)
    {
      RedrawPSFile (o);      
      fileNum++;
    }
  }

  if (!foundGoal)
  {
    newNode = NearestNeighbor (goal, maxTime);
    PathToLeaf (newNode, path, w);
    for (int i = 0; i < 10 && path.size () != 0; i++)
    {
      path.pop_back ();
    }
  } else PathToLeaf (newNode, path, w);

  RedrawPSFile (o);

  return fileNum;
}

void Planner::PathToLeaf(const node &end, list<vector> &path, window &w)
{
  node curr = end, prev = end;

  path.clear ();
  path.push (G.inf (G.first_in_edge (end)));

  w.draw_point (G.inf (end).GetPosition ()[0], G.inf (end).GetPosition ()[1]);

  for (int cntr = 0; prev != G.first_node () && cntr < G.number_of_nodes (); cntr++) 
  {
    path.push (G.inf (G.first_in_edge (prev)));
    prev = G.source(G.first_in_edge(prev));
    w.draw_point (G.inf (prev).GetPosition ()[0], G.inf (prev).GetPosition ()[1]);
  }
}

void Planner::RandomState(float &angle, bool &fire)
{
  random_source R;
  fire = true;

  R >> angle;

  if (angle < 0.05)
  {
    angle = 0;
    fire = false;
  } else R >> angle;

  angle *= (2 * PI);
}

void Planner::ChooseState(Ship pShip, point rndPt, 
			  obs &o, float &angle, bool &fire)
{
  float minF = INFINITE, currF, 
        x, y, vx, vy, currAngle, time = pShip.GetTime ();
  Ship *temp;
  bool currFire;
  circle tempCircle;
  x = pShip.GetPosition ()[0];
  y = pShip.GetPosition ()[1];
  vx = pShip.GetVelocity ()[0];
  vy = pShip.GetVelocity ()[1];

  angle = -1;
  fire = false;

  for (int i = 0; i < 15; i++)
  {
    temp = new Ship (x, y, vx, vy, time);
    RandomState (currAngle, currFire);
    temp->NewPosition (currAngle, currFire, 1);
    currF = Metric (*temp, rndPt, temp->GetTime ());

    if (minF > currF && o.InFree (temp->x, temp->y, 
                                  temp->GetTime ()))
    {
      minF = currF;
      angle = currAngle;
      fire = currFire;
    }
    delete temp;
  }
}


point Planner::ChooseNewPoint ()
{
  random_source R;
  float x, y, f;

  R >> f;
  if (f < 0.05)
  {
    return goal;
  } else 
  {
    R >> x;
    R >> y;
    x *= 100;
    y *= 100;
  }
  point z (x, y);

  return z;
}


node Planner::NearestNeighbor(point &x, float time)
{
  node testNode, minNode;
  float minMetric = INFINITE, testMetric;

  forall_nodes (testNode, G)
  {
    testMetric = Metric (G.inf (testNode), x, time);

    if (testMetric < minMetric)
    {
      minMetric = testMetric;
      minNode = testNode;
    }
  }
  
  return minNode;
}


bool Planner::Extend(point &x, node &newNode, obs &o, window &w)
{
  float angle, time;
  bool fire;
  random_source R;

  R >> time;
  time = time * maxTime;
  if (time - int (time) >= 0.5)  //This rounds time up or down
  {
    time++;
  }
  time = int (time);

  node nearestNode = NearestNeighbor (x, time);
  Ship tempShip = G.inf (nearestNode);
  Ship tShip (tempShip.GetPosition ()[0], tempShip.GetPosition ()[1], 
	      tempShip.GetVelocity ()[0], tempShip.GetVelocity ()[1], 
	      tempShip.GetTime ());

  ChooseState (tShip, x, o, angle, fire);


  if (angle != -1)
  {
    tShip.NewPosition (angle, fire, 1);
    if (abs (G.inf (nearestNode).x - tShip.x) < 2 && 
        abs (G.inf (nearestNode).y - tShip.y) < 2)
    {
      segment s (G.inf (nearestNode).GetPosition ()[0],
		 G.inf (nearestNode).GetPosition ()[1],
		 tShip.GetPosition ()[0], tShip.GetPosition ()[1]);
      w.draw_segment (G.inf (nearestNode).GetPosition ()[0],
		      G.inf (nearestNode).GetPosition ()[1], 
		      tShip.GetPosition ()[0], tShip.GetPosition ()[1], 7);

      seg.push (s);
    }
    if (tShip.GetTime () > maxTime)
    {
      maxTime = tShip.GetTime ();
    }

    newNode = G.new_node (tShip);
    vector input (angle, fire);
    G.new_edge (nearestNode, newNode, input);

    if (((int(goal.xcoord ()) - 1) <= int(tShip.GetPosition ()[0])) && 
	((int(goal.xcoord ()) + 1) >= int(tShip.GetPosition ()[0])) &&
	((int(goal.ycoord ()) - 1) <= int(tShip.GetPosition ()[1])) &&
	((int(goal.ycoord ()) + 1) >= int(tShip.GetPosition ()[1])))
      return true;
  }
  return false;
}


void Planner::RedrawPSFile (obs &o)
{
  char myFile[15];
  segment s;

  sprintf (myFile, "ast%.3d.ps", fileNum);
  ps_file myps (10, 10, myFile);

  o.Display (myps, initTime);

  myps.draw_segment (int (goal.xcoord ()) - 1, int (goal.ycoord ()) - 1, 
		  int (goal.xcoord ()) + 1, int (goal.ycoord ()) - 1, 9);
  myps.draw_segment (int (goal.xcoord ()) - 1, int (goal.ycoord ()) - 1, 
		  int (goal.xcoord ()) - 1, int (goal.ycoord ()) + 1, 9);
  myps.draw_segment (int (goal.xcoord ()) - 1, int (goal.ycoord ()) + 1, 
		  int (goal.xcoord ()) + 1, int (goal.ycoord ()) + 1, 9);
  myps.draw_segment (int (goal.xcoord ()) + 1, int (goal.ycoord ()) + 1, 
		  int (goal.xcoord ()) + 1, int (goal.ycoord ()) - 1, 9);

  forall (s, seg)
  {
    myps.draw_segment (s, 7);
  }

  myps.close ();
}