//----------------------------------------------------------------------
//               The Motion Strategy Library (MSL)
//----------------------------------------------------------------------
//
// Copyright (c) 1998-2000 Iowa State University and Steve LaValle.  
// All Rights Reserved.
// 
// Permission to use, copy, and distribute this software and its 
// documentation is hereby granted free of charge, provided that 
// (1) it is not a component of a commercial product, and 
// (2) this notice appears in all copies of the software and
//     related documentation. 
// 
// Iowa State University and the author make no representations
// about the suitability or fitness of this software for any purpose.  
// It is provided "as is" without express or implied warranty.
//----------------------------------------------------------------------
// Implementation of Visibility PRM by Kevin Crotty and Andrew Olson
// in fulfillment of 2001 Com S 476 final project requirements
//
// VisPRM could be easily added into the mainstream msl code. One
// word of note, though: the call to VisPRM::DrawGraphs should be
// removed or at least examined. Custom drawing of 2D Graph Projections
// isn't easily incorporated into the msl framework, making DrawGraphs a bit
// of a hack.
//----------------------------------------------------------------------

#include <math.h>
#include <stdio.h>

#include <LEDA/graph_alg.h>

#include "prm.h"
#include "visprm.h"
#include "defs.h"

#include <LEDA/graph.h> 
#include <LEDA/vector.h> 
#include <LEDA/REDEFINE_NAMES.h>
#include <LEDA/window.h>

VisPRM::VisPRM(Problem *problem): PRM(problem){
}

/////////////////////////////////////////////////////////////////
// void VisPRM::DrawGraphs()
//
// This function is kind of a hack. It was added in order to
// show a graph that is more pertinant to Visibility PRM than 
// the default graph could allow. It is copied with minimal 
// possible modification from the code employed under "Display: 
// 2D Graph Projection".  It draws all edges, then adds
// dots to distinguish guard nodes and connector nodes. Guards
// are red and Connectors are green.
/////////////////////////////////////////////////////////////////
void VisPRM::DrawGraphs()
{

  //this hack remove "Pl->" from the beginning of each instance of "G" and "P"
  int DrawIndexX=0;
  int DrawIndexY=1;
  int LineWidth=1;


  edge e;
  vector x1,x2;
  double tranx,trany,scalex,scaley;

  window dw(600,600,string("VisPRM Display Window:     x[%d] versus x[%d]",
			   DrawIndexY,DrawIndexX));
  dw.set_line_width(LineWidth);
  dw.display();

  // The default window coords are [0,100]x[0,100]
  scalex = 100.0/(P->UpperState[DrawIndexX] - 
		  P->LowerState[DrawIndexX]); 
  scaley = 100.0/(P->UpperState[DrawIndexY] - 
		  P->LowerState[DrawIndexY]); 
  tranx = -1.0 * scalex * P->LowerState[DrawIndexX];
  trany = -1.0 * scaley * P->LowerState[DrawIndexY];

  // Show first tree
  if (G.number_of_nodes() > 0) {
    x1 = G.inf(G.first_node());
    //dw.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
    //		 0.5,blue);
    forall_edges(e,G) {
      x1 = G.inf(G.source(e));
      x2 = G.inf(G.target(e));
      dw.draw_segment(x1[DrawIndexX]*scalex+tranx,
		      x1[DrawIndexY]*scaley+trany,
		      x2[DrawIndexX]*scalex+tranx,
		      x2[DrawIndexY]*scaley+trany,blue);
    }
  }
  node nodeCurrent;
  forall(nodeCurrent, guards){
    dw.draw_disc(G.inf(nodeCurrent)[DrawIndexX]*scalex+tranx,
		 G.inf(nodeCurrent)[DrawIndexY]*scaley+trany,
		 0.5,red);
  }
  forall(nodeCurrent, connectors){
    dw.draw_disc(G.inf(nodeCurrent)[DrawIndexX]*scalex+tranx,
		 G.inf(nodeCurrent)[DrawIndexY]*scaley+trany,
		 0.5,green);
  }

  dw.read_mouse();
  dw.close();
}

void VisPRM::Construct()
{
  int c;
  vector dummy,qvector;
  node guardCurrent;
  node qnode;
  //list<node> nhbrs;
  // instead of the list "nhbrs" normally used,
  // VisPRM uses *all* graph nodes (G.all_nodes)
  
  float t = used_time();
  // Set the step size
  StepSize = P->Metric(P->InitialState,
		       P->Integrate(P->InitialState,
				    P->GetInputs(P->InitialState).head(),
				    PlannerDeltaT));

  int i=0;// total number of random state generated
  int rejects=0;// number of *consecutive* states in Cfree not added
  // to the VisPRM graph


  // NOTICE: NumNodes has a nontraditional meaning in the VisPRM context
  // Instead of just a linear count of nodes added, it is compared to 
  // the number of consecutive rejects. This is a good compromise between
  // the traditional, reiterable approach and the (i/k) free space 
  // approximation discussed in class

  while ((rejects < NumNodes)) {
    do{//while (!P->Satisfied(qvector));
      qvector = ChooseState(i,NumNodes,P->InitialState.dim());  
      SatisfiedCount++;
      i++;
      if (i % 1000 == 0){
	cout << G.number_of_nodes() << " nodes in the VisPRM."<<endl;
	cout << "i= "<<i<<endl;
	
	cout << "PRM Nodes: " << G.number_of_nodes() << 
	  "  Edges: " << G.number_of_edges() <<
	  "  ColDet: " << SatisfiedCount << "\n";
      }
      
      // Keep trying until a good sample is found
    }while (!P->Satisfied(qvector));
       

    list<node> visibleGuards;
    list<node> connectedComponentNodes;

    forall(guardCurrent, guards){
      if (Connect(G.inf(guardCurrent),qvector,dummy)) {
	visibleGuards.push(guardCurrent);
      }
    }
    //now visibleGuards holds a list of pointers to
    //all the guards visible by qvector
    
    if(visibleGuards.empty()){//qvector will become a guard
      qnode = G.new_node(qvector);
      guards.push(qnode);
      rejects=0;
    }else{//!visibleGuards.empty()
      //now we need to prune down visibleGuards to a single
      //representative from each connected component
      while(!visibleGuards.empty()){
	connectedComponentNodes.push(visibleGuards.pop());
	node_array<bool> Gnode(G, false);
	list<node> reachable;
	reachable = DFS(G, connectedComponentNodes.front(), Gnode);
	node tempnode;
	forall(tempnode, reachable){
	  visibleGuards.remove(tempnode);
	}
      }
      if(connectedComponentNodes.length()>1){//qvector will become connector
	qnode = G.new_node(qvector);
	connectors.push(qnode);
	rejects=0;
	node tempnode;
	forall(tempnode, connectedComponentNodes){
	  G.new_edge(qnode,tempnode,dummy);
	  G.new_edge(tempnode,qnode,dummy);
	}
      }else {
	//discard node, it is only connected to one connected component
	rejects++;
      }
    }
  }
  
  node_array<int> labels(G);
  c = COMPONENTS(G,labels); // Compute connected components

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

  CumulativeConstructTime += ((double)used_time(t));
  cout << "Construct Time: " << CumulativeConstructTime << "s\n"; 
  DrawGraphs();
}

/////////////////////////////////////////////////////////////////////
// bool VisPRM::Plan()
//
// Only two lines have been changed from the original 
// PRM::Plan(). They are commented below as shown:
//   nhbrs = G.all_nodes();//was NeighboringNodes(P->InitialState);
/////////////////////////////////////////////////////////////////////
bool VisPRM::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

  // Connect to the initial state
  nhbrs = G.all_nodes();//was NeighboringNodes(P->InitialState);
  if (nhbrs.length() > 0)
    n = nhbrs.pop();
  else {
    cout << "No neighboring nodes to the Initial State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    return false;
  }

  success = Connect(P->InitialState,G.inf(n),u_best);
  while((nhbrs.length() > 0)&&
	(!success)) {
    n = nhbrs.pop();
    success = Connect(P->InitialState,G.inf(n),u_best);
  }

  if (!success) {
    cout << "Failure to connect to Initial State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    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

  // Connect to the goal state
  nhbrs = G.all_nodes();//was NeighboringNodes(P->GoalState);
  if (nhbrs.length() > 0)
    n = nhbrs.pop();
  else {
    cout << "No neighboring nodes to the Goal State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    return false;
  }

  success = Connect(P->GoalState,G.inf(n),u_best);
  while((nhbrs.length() > 0)&&
	(!success)) {
    n = nhbrs.pop();
    success = Connect(P->GoalState,G.inf(n),u_best);
  }

  if (!success) {
    cout << "Failure to connect to Goal State\n";
    cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
    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";
    return false;
  }

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

  return true;
}