//#include <iostream>
#include <math.h>


 
#include <LEDA/graph_alg.h>
#include <LEDA/dictionary.h>
#include <LEDA/node_partition.h>
#include <LEDA/sortseq.h>
#include "MSL/prm.h"
#include "MSL/defs.h"
#include "Obprm.h"

// This includes the core header files of the Motion Strategy Library
#include <MSL/MSL.h>
#include <MSL/rendergl.h>

#include <LEDA/REDEFINE_NAMES.h>

#define MAX2 10
#define K22 10
#define MAX3 10


//Constructor
Obprm::Obprm(Problem *problem):PRM(problem)
{
  Radius = 40.0;
}



// A new plan()
bool Obprm::Plan()
{    
  list<node> nhbrs;
  node n,ni,ng;
  vector u_best;
  bool success;
  list<node> path;

  float t = used_time();

  // Set the step size
  StepSize = P->Metric(P->InitialState,P->Integrate(P->InitialState,
 	     P->GetInputs(P->InitialState).head(),PlannerDeltaT));

  n = ni = ng = G.choose_node(); // Initialize to make warnings go away

  ni = G.new_node(P->InitialState);

  // Connect to the initial state
  nhbrs = Closest(ni, G.all_nodes(), 10);

  if (nhbrs.length() <= 0) {
    cout << "No neighboring nodes to the Initial State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    G.del_node(ni);
    return false;
  }

  success = false;
  forall(n, nhbrs) {
    if ( Connect(G.inf(ni), G.inf(n), u_best) ) {
      success = true;
      G.new_edge(ni,n,u_best);
      G.new_edge(n,ni,u_best);
    }
  }

  if (!success) {
    cout << "Failure to connect to Initial State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    G.del_node(ni);
    return false;
  }

  /**********************************************************
  success = Connect(G.inf(ni) ,G.inf(n),u_best);
  while((nhbrs.length() > 0)&&
	(!success)) {
    n = nhbrs.pop();
    success = Connect(G.inf(ni) ,G.inf(n),u_best);
  }

  if (!success) {
    cout << "Failure to connect to Initial State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    G.del_node(ni);
    return false;
  }

  // Make ni a new node in the PRM
  //ni = G.new_node(P->InitialState);
  G.new_edge(ni,n,u_best);
  G.new_edge(n,ni,u_best);
  ***********************************************************/

  GEdgeTime[ni] = 0.0;  // This sets the initial time

  ng = G.new_node(P->GoalState);

  // Connect to the goal state
  nhbrs = Closest(ng, G.all_nodes(), 10);

  if (nhbrs.length() <= 0) {
    cout << "No neighboring nodes to the Goal State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    G.del_node(ni);
    G.del_node(ng);
    return false;
  }

  success = false;
  forall(n, nhbrs) {
    if ( Connect(G.inf(ng), G.inf(n), u_best) ) {
      success = true;
      G.new_edge(ng,n,u_best);
      G.new_edge(n,ng,u_best);
    }
  }

  if (!success) {
    cout << "Failure to connect to Initial State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    G.del_node(ni);
    G.del_node(ng);
    return false;
  }

  /*********************************************************
  success = Connect(G.inf(ng), G.inf(n), u_best);
  while((nhbrs.length() > 0)&&
	(!success)) {
    n = nhbrs.pop();
    success = Connect(G.inf(ng), G.inf(n), u_best);
  }

  if (!success) {
    cout << "Failure to connect to Goal State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    G.del_node(ni);
    G.del_node(ng);
    return false;
  }

  // Make ng a new node in the PRM
  //ng = G.new_node(P->GoalState);
  G.new_edge(ng,n,u_best);
  G.new_edge(n,ng,u_best);
  ********************************************************/

  GEdgeTime[ng] = 1.0;

  // Perform the search

  //G.make_undirected(); // Make sure the graph is undirected!!!

  EdgeCost = edge_array<double>(G,1.0);
  CostToGo = node_array<double>(G);
  NextEdge = node_array<edge>(G);

  // This does dynamic programming on the graph
  SHORTEST_PATH_T(G,ng,EdgeCost,CostToGo,NextEdge);

  CumulativePlanningTime += ((double)used_time(t));
  cout << "Planning Time: " << CumulativePlanningTime << "s\n"; 

  if ((CostToGo[ni] > 0)&&(ni != ng)) {
    //cout << "Cost to goal node: " << CostToGo[ni] << "\n";

    // Get the path
    n = ni;
    while (n != ng) {
      path.append(n);
      n = G.opposite(n,NextEdge[n]);
    }
    path.append(ng);
  }
  else {
    cout << "  Failure to find a path in the graph.\n";
    G.del_node(ni);
    G.del_node(ng);
    return false;
  }

  RecordSolution(path);
  cout << "  Success\n";

  return true;
}


//A new Construct
void Obprm::Construct()
{
  int i,c, j;
  vector u_best, pfree, pobs, Cin, Cout;
  node nn;
  list<node> nhbrs;
  const int NUM_OF_DIR = 50;
  vector direction[NUM_OF_DIR];
 
  int r = 1;
  cout << "Initial State dim: " << P->InitialState.dim() << endl;
  // Set the step size
  StepSize = P->Metric(P->InitialState,
		       P->Integrate(P->InitialState,
				    P->GetInputs(P->InitialState).head(), 
				    PlannerDeltaT));
  
  i = 0;

  // wcchang+
  //cout << "Obstacle size: " << P->G->NumBodies << endl;
  // ... always be 1...
  list<polygon> obstlist;
  polygon pg;
  obstlist = P->EnvironmentToLedaPolygons();
  cout << "Obstacle size: " << obstlist.size() << endl;


  int nPg = 0;

  // for first run, create the empty list.
  if ( nLL.size() != obstlist.size() ) {
    for ( int iPg = 0; iPg < obstlist.size(); iPg++ ) {
      list<node> nl;
      nl.clear();
      nLL.append(nl);
    }
  }


  forall( pg, obstlist )
    {

      // wcchang-
      do{
	pobs = ChooseState( i, NumNodes, P->InitialState.dim() );
	SatisfiedCount++;
	i++;
      }while( P->Satisfied(pobs) || !pg.inside(P->StateToLedaPoint(pobs)) );
 
      Cin = pobs;

      G2.new_node(Cin);
    
      //from cin we take NUM_OF_DIR random directions to generate new nodes
      for (j = 0; j < NUM_OF_DIR; j++)
	{
	  direction[j] = RandomDirection();
	}


     //go in each direction and find new points in cfree (if they exists)
      for(j = 0; j < NUM_OF_DIR; j++)
      {
	//cout << "Readly to pick " << j << " direction.\n";
	r = 1;
	pfree = (r*direction[j]) + Cin;
	while(!P->Satisfied(pfree) && r < 20)
	  {
	    r++;
	    SatisfiedCount++;
	    pfree = (r*direction[j]) + Cin;
	  }
	if(P->Satisfied(pfree))
	  {
	    SatisfiedCount++;
	    Cout = BinarySearch(Cin, pfree);
	    if ( Cout.dim() != P->StateDim ) {
	      cout << "Invalid Cout.\n";
	      //break;
	    }
	    else
	      {
		nn = G.new_node(Cout);	// don't forget to insert the new node!
		nLL[nLL[nPg]].append(nn);	
	      }
	    /**************************************
	    cout << "Cout: " << G.inf(nn) << endl;
	    cout << "Vector: " << Cout << endl;
	    *************************************/
	  } // found new Cout!
      }
      nPg++;			// for next polygon; next node list
    }// forall(polygon)....
  

  

  int K1 = 10;
  SimpleConnect(K1);	

  node_array<int> labels(G);	// node_array, store nodes into int set.

 
  c = COMPONENTS(G,labels); 	// Count connected components
  cout << "Before we connect, we have : " << c << endl;
  // johanj

  ConnectComponents(labels, c);

  c = COMPONENTS(G,labels);

  cout << "After first connect, we have : " << c << endl; 

  ExpandSmallComponents(labels, c);

  ConnectComponents(labels, c);

  c = COMPONENTS(G,labels);	
  cout << "PRM Nodes: " << G.number_of_nodes() <<     
    "  Edges: " << G.number_of_edges() <<
    "  ColDet: " << SatisfiedCount << 
    "  Components: " << c << "\n";
}


//binary serach, precondition: x1 is in Cobs and x2 is in Cfree
vector Obprm::BinarySearch(vector x1, vector x2)
{
  //cout << "In Binary serach" << endl;
  float radius = 0.3;
  vector v;

  //Metric returns distance between two configurations
  while(P->Metric(x1,x2) > radius) 
    {
      v = 0.5*(x1+x2);
      if(P->Satisfied(v))
	{
	  x2 = v;
	  //cout << "in Cfree, temp is assigned to x2" << endl;
	}
      else
	{
	  x1 = v;
	  //cout << "in Cobs, temp is assigned to x1" << endl;
	}
      SatisfiedCount++;
    }
  
  return x2;
}



// Generate a vector in a random direction with unit magnitude
vector Obprm::RandomDirection()
{
  vector delta;
  int i,j,dim;
  double r,w;

  dim = P->StateDim;

  delta = vector(dim);

  // Pick a random direction
  w = 0.0;
  for (i = 0; i < dim; i++) {
    // Generate sample from N(0,1)
    delta[i] = 0.0;
    for (j = 0; j < 12; j++) {
      R >> r; delta[i] += r;
    }
    delta[i] -= 6.0;
    w += delta[i]*delta[i];
  }
  w = sqrt(w);  
  for (i = 0; i < dim; i++) {
    delta[i] = delta[i]/w;
  }
  //delta[2] = 0.0;
  //cout << "delta: " << delta << "\n";
  return delta;
}


//connect components to each other
//compnum is used to check where we have been
void Obprm::ConnectComponents(node_array<int>& compnum, int c)
{
  list< list<node> > L;
  for(int s = 0 ; s < c ; s++)
    {
      list<node> n;
      L.append(n);
    }
  
  int i = 0;
  node v;
  int count = 0;
  
  //initialize all to -1
  forall_nodes(v,G) compnum[v] = -1;

  forall_nodes(v,G)
    if (compnum[v] == -1) //we have not been here yet!
      { 
	dfs(v,compnum, L.contents(L.get_item(i)), count);	//expand this component
	count++;
	i++;
      }    

  L.sort(&compareList,1);
  
  cout << "in Connectcomp - start printing" << endl;

  // just a print out to debug with
  list<node> list;
  int t= 0;
  list_item k;
  forall_items(k, L) 
    {
      list = L.contents(k);
      cout << "Component " << t << " has length " << list.length() << endl; 
      t++;
    }

  //connect components, smaller components are connected to larger
  //ones. i is the small component and j will be the larger or equally
  //large node
  for(int i = 0; i < (c-1); i++)
    {
      for(int j = i + 1; j < (c-1); j++)
	{
	  if(L.contents(L.get_item(i)).length() < MAX2) //if component i is smaller that MAX2
	    {
	      Attempt(L.contents(L.get_item(i)),   L.contents(L.get_item(j)), L.contents(L.get_item(i)).length() );
	    }
	  else
	    {
	      Attempt(L.contents( L.get_item(i)),   L.contents(L.get_item(j)), K22);
	    }
	}
    }
}


//this implements 3-sc Expanding small components
//note: this is an expensive operation!
//By doing this we add new nodes close to the ones
//we already have in small components.
void Obprm::ExpandSmallComponents(node_array<int>& compnum, int c)
{ 
  vector direction[10];
  list<node> nbr;
  list<node> trash;
  int length =0;
  float rad = 2.0; //this is the radius used to find neighbors
  node n, nn, temp;
  double r;
  vector vec;

  //same procedure as in ConnectComponents
  list< list<node> > L;
  for(int s = 0 ; s < c ; s++)
    {
      list<node> n;
      L.append(n);
    }
  
  int i = 0;
  node v;
  int count = 0;
  
  //initialize all to -1
  forall_nodes(v,G) compnum[v] = -1;


  forall_nodes(v,G)
    if (compnum[v] == -1) //we have not been here yet!
      { 
	dfs(v,compnum, L.contents(L.get_item(i)), count); //expand this component
	count++;
	i++;
      }    

  L.sort(&compareList,1);
  //now everything is sorted

  list<node> l;
  list_item it;
  forall_items(it, L) 
    {
      
      l = L.contents(it);
      length = l.length();
      if( length < MAX3)
	{
	  R >> r;
	  n = l.contents(l.get_item( (int)r*length));
	  int j;
	  //used to find random neighbors to n, maybe this should be more systematic???
	  for (j = 0; j < 10; j++)
	    {
	      direction[j] = RandomDirection();
	    }			
	  
	  for (j = 0; j < 10; j++)
	    {
	      vec = direction[j]*rad + G.inf(n); //create neighbors
	      if(P->Satisfied(vec))
		{
		  SatisfiedCount++;
		  temp = G.new_node(vec); //add a temproray node to the graph
		  nbr.append(temp);	  //adds this node to the neighbor list
		}
	    }



	   vector u_best; //why???
	   
	   forall_items(it, nbr)	//for all neighbors
	     {
	       nn = nbr.contents(it);

	       //try to connect
	       if (Connect(G.inf(n),G.inf(nn),u_best)) {
		 G.new_edge(nn,n,u_best);
		 G.new_edge(n,nn,u_best);
	       }
	       else	//if we can't connect we need to remove the node from the graph
		 { 
		   trash.append(nn);
		 }
	     }

	   G.del_nodes(trash);	//remove from graph 
	   nbr.clear();		//clear neighbors
	   trash.clear();	//empty trash
      
	}
    }
}


//attempt to connect to components using k closest nodes
void Obprm::Attempt(const list<node>& l1, const list<node>& l2, int k)
{
  int m = 0;
  int l = 0;
  node nn, n; 
  vector u_best;
  list<node> nbr; 
  
  while(m < k && m < l1.length()) //if Vi is smaller then ten connect all else try k closest connections to Vj
    { 
      l = 0;
      n = l1.contents(l1.get_item(m));
      nbr = Closest(n, l2, k);	//pick the ten closest points in l2


      while(l < l2.length() && !nbr.empty())//if Vj i
	{ 
	  nn = nbr.pop();
	  //nn = l2.contents(l2.get_item(l));
	  if (Connect(G.inf(n),G.inf(nn),u_best)) {	//try to do simple connection between components
	    G.new_edge(nn,n,u_best);
	    G.new_edge(n,nn,u_best);
	    //as soon as we find a connection - return
	    return;
	  }
	  else {					//if simple doesn't work we do a rotate-at-s
	    //cout << "Trying rotate-at-0.5.\n";
	    if ( Rotate_at_S( 0.5, n, nn, 10 ) ) {
	     //as soon as we find a connection - return
	      return;
	    }
	  }
	  l++;
	}
      m++;
      nbr.clear();
    }
}



//to compare to lists by length
int compareList(const list<node>& l1, const list<node>& l2)
{
  if(l1.length() > l2.length())
    return 1;
  else if (l1.length() < l2.length())
    return -1;
  else 
    return 0;
}



//a depth first search to mark nodes and add them to components
void Obprm::dfs(node v, node_array<int>& compnum, list<node>& l, int count)
{
  stack<node>  S;
  
  S.push(v);
  compnum[v] = count;
  
  while (!S.empty())
    { 
      v = S.pop();
      edge e;
      l.append(v);
      forall_inout_edges(e,v)
	{ 
	  node w = G.opposite(v,e);
	  if (compnum[w] == -1)
	    { 
	      l.append(w);
	      compnum[w] = count;
	      S.push(w);
	    }
	}
    }
}




// simple connection to connect nodes in the graph.
// using straight-line connection.
void Obprm::SimpleConnect( int K )
{
  // check the list first
  node n, nn;
  vector u_best;
  //edge_set eS(G);
  //edge e;

  for (int i = 0; i < nLL.size(); i++) {
    cout << "Obstacle " << i << " has " << nLL[nLL[i]].size() << " nodes.\n";
  }
  //cin.get();
  //cout << "Ready to stage 1 connection.\n";

  forall_nodes(n,G) {

    // take a look at G.
    //Ginfo(G);

    for (int i = 0; i < nLL.size(); i++) {
      list<node> nhbrs;
      //cout << "Finding neighbors..." << endl;
      nhbrs.clear();
      nhbrs = Closest(n, nLL[nLL[i]], K);
      //cout << "Got a list of " << nhbrs.size() << " neighbors from obst " << i << endl;
      forall(nn, nhbrs) {
	if ( Connect(G.inf(n), G.inf(nn), u_best) ){
	  //cout << "Connecting...\n";
	  G.new_edge(nn,n,u_best);
	  G.new_edge(n,nn,u_best);
	}
	/*****************************
	else {
	    cout <<"Couldn't connect.\n";
	}
	*****************************/
      }
    }
  }
}



// try to find K closest nodes from the list l to node n.
list<node> Obprm::Closest(const node& n, const list<node>& l, const int& k)
{
  node nn;
  sortseq<double, node> s;
  list<node> nbr;
  double d;

  // sort all nodes by its distance to n,
  forall(nn,l) {
     if ( G.inf(nn).dim() == P->StateDim ) {
      d = P->Metric(G.inf(n),G.inf(nn));
      if ( d > 0 ) {
	// in case they are the same node
	s.insert(d, nn);
      }
    }
    /*************************************
    else
      cout << "REJECT!" << endl;
    *************************************/
  }
 
  seq_item sit;
  sit = s.min();

  // extract closest K nodes from the list
  for ( int i = 0; i < s.size() && i < k; i++, sit = s.succ(sit) ) {
    nbr.append(s[sit]);
  }

  return nbr;
}


bool Obprm:: Rotate_at_S(const double& s, const node& n1, const node& n2, const int& k)
{
  vector c1, c2, u_best;
  int trial;
  node nc1, nc2;

  c1 = ( G.inf(n2) - G.inf(n1) ) * s + G.inf(n1);

  // c1 collides to obstacles.
  if ( !P->Satisfied(c1) || !Connect(G.inf(n1),c1,u_best) )
    return false;
  SatisfiedCount++;
  //cout << "c1 found.\n";

  trial = 0;
  do{
    c2 = RandomRotate(c1);
    trial++;
    SatisfiedCount++;
  } while( ( !P->Satisfied(c2) || !Connect(c1,c2,u_best) ) && trial <= k );

  // so, it's satisfied and connected, or maybe trial > k;

  if ( trial > k )
    return false;


  if ( !Connect(c2,G.inf(n2),u_best) )
    return false;


  // so far, c1, c2 should be valid to connect x1, x2 at s
  nc1 = G.new_node(c1);
  nc2 = G.new_node(c2);

  if( Connect(G.inf(n1),G.inf(nc1),u_best) ) {
    G.new_edge(n1,nc1,u_best);
    G.new_edge(nc1,n1,u_best);
  }else{
    cout << "Wrong!\n";
    return false;
  }
  if( Connect(G.inf(nc1),G.inf(nc2),u_best) ) {
    G.new_edge(nc1,nc2,u_best);
    G.new_edge(nc2,nc1,u_best);
  }else{
    cout << "wRong!\n";
    return false;
  }
  if( Connect(G.inf(nc2),G.inf(n2),u_best) ) {
  G.new_edge(nc2,n2,u_best);
  G.new_edge(n2,nc2,u_best);
  }else{
    cout << "wrOng!\n";
    return false;
  }
  return true;
}


//rotates in a random direction
vector Obprm:: RandomRotate(const vector& c1)
{
  int dim;
  double r, d;
  vector c2 = c1;

  dim = P->StateDim;

  R >> r;
  r = r * 2 * PI;		// random angle;

  //cout << "Random rotating..."

  switch(dim){
  case (3):
    // 2D case
    c2[2]+=r;
    while (c2[2] > 2*PI)
      c2[2]-=2*PI;
    break;
  case (6):
    // 3D case
    R >> d;			// random axis to rotate;
    d*=3;
    if ( d >= 3 )
      d = 2;
    c2[(int)d+3] += r;
    while ( c2[(int)d+3] > 2*PI)
      c2[(int)d+3]-=2*PI;
    break;
  default:
    // what else could it be???
    cout << "Oops!! invalid dimention???\n";
    cout << "Paused. Press <Enter> to continue.\n";
    cin.get();
    break;
  }
  return c2;
}




int main(int argc, char *argv[])
{
  string path;
  GuiPlanner *gui;
  Model *m = NULL;
  Geom *g = NULL;
  Problem *prob;

  if (argc < 2) {
    cout << "Try this:    Obprm projectdata\n";
    exit(-1);
  }

  path = string(argv[1])+"/";

  if (!is_directory(path)) {
    cout << "Error:   Directory does not exist\n";
    exit(-1);
  }


  m = new Model2DRigid(path); // Define the differential model
  g = new GeomPQP2DRigid(path); // Use PQP and 3D triangle representations


  prob = new Problem(g,m,path); // This is the input to a planner

  // Initialize everything: Gui, Scene, Render, and RRT
  // All of them will use the same file path
  gui = new GuiPlanner(new RenderGL(new Scene(prob, path), path),
		       new Obprm(prob));

  // Activate the GUI
  gui->Start();

  return 0;
}

#include <LEDA/UNDEFINE_NAMES.h>