/***********************************************************************
 * CLASS:	 RandPField
 * AUTHORS:	 Nick Mayer & Shaun Jurgemeyer
 * CLASS:    Computer Science 476
 * MODIFIED: 04/03/2001
 ***********************************************************************
*/


#include <fstream.h>
#include <math.h>
#include <MSL/planner.h>
#include "randpfield.h"
#include <LEDA/REDEFINE_NAMES.h>

/**
 * RandPField <constructor>
 *
 * The default constructior for Planner. Initilizes all constants and
 * golbal variables to their default values.
 */
RandPField::RandPField(Problem *p):Planner(p) 
{
  cout << "Welcome to our Randomized Potential Field" << endl;

  // Init my global variables
  m_problem = p;
  
/*
  // Debug info (commented out for final version)
  cout << "The problem has:" << endl;
  cout << p->FilePath << " FilePath" << endl;
  cout << p->NumBodies << " NumBodies" << endl;
  cout << p->StateDim << " StateDim" << endl;
  cout << p->InputDim << " InputDim" << endl;
  cout << p->GeomDim << " GeomDim" << endl;
  cout << p->LowerState << " LowerState" << endl;
  cout << p->UpperState << " UpperState" << endl;
  cout << p->InitialState << " InitialState" << endl;
  cout << p->GoalState << " GoalState" << endl;
  cout << p->MaxDeviates << " MaxDeviates" << endl;
*/
  
  init(50, 15, 3, 100, 50, 50/1, 1, 1 / (2* 3.1416), 3, false);
  
  m_lastNode = NULL;
  EMPTY_VECTOR = vector();
  m_totalTimeToSolve = 0;
  m_stuckCount = 0; 
  m_lastNode = G.new_node(m_problem->InitialState);  
} // end constructor


/**
 * init(...)
 *
 * This function allows you to change most of the path planning constants with
 * this function. It is reccomended that you call init only before calling plan
 *
 * numberOfRandomStates: The number of random states to generate when
 !   searching for a new state of lowest potential
 * stuckPathSize: The number of states to go back to generate the metric
 !   to determine if we are stuck
 * numberOfBounces: The number of bounces in the bounce walk when stuck
 * numberOfBounceSteps: The max number of seteps to take in a bounce walk
 * backtrackLength: The number of states to go back when choosing a state
 !   to begin the bounce walk from after we are stuck
 * stuckRatio: The max ratio of distance traveled over distance from start to
 !   end state. Used to determine when you are stuck.
 * translationStepSize: The max translation size when taking a step
 * rotationStepSize: The max rotation angle when taking a step
 * smoothingIterations: The number of times to go over a holonomic path 
 !   and smooth it
 * forceHolonomic: Force the model to be holonomic
 */
void RandPField::init(int numberOfRandomStates, int stuckPathSize, 
     int numberOfBounces, int numberOfBounceSteps, int backtrackLength, 
     double stuckRatio, double translationStepSize, double rotationStepSize,
     int smoothingIterations, bool forceHolonomic)
{
  TRANSLATION_STEP_SIZE = translationStepSize;
  ROTATION_STEP_SIZE = rotationStepSize;
  
  RANDOM_STEP_SIZES = vector(m_problem->StateDim);
  for (int i = 0; i < m_problem->StateDim; i++)
    RANDOM_STEP_SIZES[i] = TRANSLATION_STEP_SIZE;
  if (m_problem->StateDim == 3)
    RANDOM_STEP_SIZES[2] = ROTATION_STEP_SIZE;
  else if (m_problem->StateDim == 6)
    for (int i = 3; i < 6; i++)
      RANDOM_STEP_SIZES[i] = ROTATION_STEP_SIZE;

  NUMBER_RANDOM_POINTS = numberOfRandomStates;
  STUCK_NUMBER_OF_NODES = stuckPathSize;
  BACKTRACK_NODES = backtrackLength;
  STUCK_RATIO = stuckRatio;

  RAND_BOUNCES = numberOfBounces;
  MAX_BOUNCE_WALK_SIZE = numberOfBounceSteps;
  SMOOTH_ITERATIONS = smoothingIterations;

  if (forceHolonomic)
    Holonomic = true;
    
  cout << "The planner is holonomic? "; 
  if (Holonomic)
  {
    cout << "true" << endl;
  } // end if holonomic
  else
  {
    cout << "false" << endl;
  } // end if not holonomic

  if (!Holonomic)
  {
    cout << "Has inputs: " << endl;
    list<vector> listOfInputs = m_problem->GetInputs();
    m_possibleInputSize = listOfInputs.size();
    m_possibleInputs = new vector[m_possibleInputSize];
    
    vector v;
    int i = 0;
    forall(v, listOfInputs)
    {
      m_possibleInputs[i] = v;
      i++;
    }
  
    cout << "Possible inputs are:" << endl;
    for (int i = 0; i < m_possibleInputSize; i++)
    { 
      cout << m_possibleInputs[i] << endl;
    }
  }
  else
  {
    m_possibleInputSize = 0;
  }  
  
} // end init


/**
 * Connect(...)
 *
 * This funcion is not necessary for the Randomized potential field.
 * calling it has no effect.
 */
bool RandPField::Connect(const vector &x, 
		       GRAPH<vector,vector> &g,
		       node &nn, bool forward = true) 
{
  bool success = false;
  cout << "Connect not implemented :-(" << endl;
  return success;
} // end Connect


/**
 * Construct(...)
 *
 * This funcion is not necessary for the Randomized potential field.
 * calling it just calls Plan()
 */
void RandPField::Construct()
{
  cout << "Construct just calling plan" << endl;
  Plan();
} // end Construct
  

/**
 * Plan()
 *
 * This function is responsible for finding the path from the initial
 * state to the goal state. If a solution path is not found within the
 * specififed number of iterations, you can call the function again to
 * have it continue searching from where it left off.
 *
 * This function makes use of the following functions:
 * makeProgress()
 * stuck()
 * backtrack()
 * takeBouncWalk()
 */
bool RandPField::Plan()
{
  cout << "Planning..." << endl;  
  bool success = false;
   
  // Initialize the timer
  float t = used_time();
  
  cout << "NumNodes = " << NumNodes << endl;
  
  for (int i = 0; i < NumNodes; i++)
  {
    //cout << "iteration " << i << "  from " << G[m_lastNode] << endl;
    makeProgress();
    if (stuck())
    {
      cout << "." << flush;
      m_stuckCount++;
      backtrack();
      takeBounceWalk();
    }    
    if (GapSatisfied (G[m_lastNode], m_problem->GoalState))
    {
      success = true;
      node goalNode = G.new_node(m_problem->GoalState);
      G.new_edge(m_lastNode, goalNode, EMPTY_VECTOR);
      m_lastNode = goalNode;
      RecordSolution(G.all_nodes());
      break;
    }
  } // end for NumNodes (how many iterations the user wants
  
  double planningTime = used_time(t);
  
  m_totalTimeToSolve += planningTime;
  cout << endl;
  cout << "There are currently " << G.number_of_nodes() 
       << " nodes in the graph." << endl;
  cout << "Done planning, returning ";
  if (success)
  {
    cout << "success" << endl;

    cout << "Smoothing...." << endl;
    for (int i = 0; i < SMOOTH_ITERATIONS; i++)
    {
      Smooth();
    }
    
    double smoothingTime = used_time(t);
    cout << "It took " << smoothingTime << "s to smooth the graph" << endl;

  }
  else
  {
    cout << "failure" << endl;
  }
    
  cout << "We have gotten stuck " << m_stuckCount << " times so far." << endl;
  cout << "Planning Time: " << planningTime << "s" << endl;
  cout << "Total Planning Time: " << m_totalTimeToSolve << "s" << endl;
  return success;
} // end plan


/**
 * makeProgress()
 *
 * This function adds one node and one edge to the graph. This is the 
 * standard descend function in the rpf.
 */
void RandPField::makeProgress()
{
  // If it is holonomic
  if (m_possibleInputSize == 0)
  {
    vector newState = makeRandomState(G[m_lastNode]);

    while(!m_problem->CollisionFree(newState))
    {
      newState = makeRandomState(G[m_lastNode]);
    } // end pick a new point that isn't in collision
  
    float distanceToPotentialState = 
      Metric(newState, m_problem->GoalState);
  
	// generate random points, and choose the point with the lowest potential
    for (int i = 0; i < NUMBER_RANDOM_POINTS; i++)
    {
       vector potentialState = makeRandomState(G[m_lastNode]);
     
       float temp = Metric(potentialState, m_problem->GoalState);
       if (temp < distanceToPotentialState)
       {
         if (m_problem->CollisionFree(potentialState))
         {
           newState = potentialState;
           distanceToPotentialState = temp;
         }
       }
    } // end choose new point
  
    node newNode = G.new_node(newState);
    edge newEdge = G.new_edge(m_lastNode, newNode, EMPTY_VECTOR);
    m_lastNode = newNode;
    m_lastEdge = newEdge;    
  } // end if no inputs

  else
  { // if it has inputs (non-holonomic)
    vector newState, newInput, potentialState, potentialInput;
        
    makeRandomInput(newState, newInput, G[m_lastNode]);
    int i = 0;
    while(!m_problem->CollisionFree(newState))
    {
      makeRandomInput(newState, newInput, G[m_lastNode]);
      i++;
      if (i > m_possibleInputSize * 2)
      {
        backtrack();
        return;
      } 
    } // end pick a new point that isn't in collision

    float distanceToPotentialState = 
      Metric(newState, m_problem->GoalState);
    
	// generate random points, and choose the point with the lowest potential
    for (int i = 0; i < NUMBER_RANDOM_POINTS; i++)
    {
      makeRandomInput(potentialState, potentialInput, G[m_lastNode]);
      float temp = Metric(potentialState, m_problem->GoalState);
      if (temp < distanceToPotentialState)
      {
        makeRandomInput(potentialState, potentialInput, G[m_lastNode]);
        if (insideWorld(potentialState) 
            && m_problem->CollisionFree(potentialState))
        {
          newState = potentialState;
          newInput = potentialInput;
          distanceToPotentialState = temp;
        }
      } // end if distance shorter
    } // end for random points
  
    node newNode = G.new_node(newState);
    edge newEdge = G.new_edge(m_lastNode, newNode, newInput);
    m_lastNode = newNode;
    m_lastEdge = newEdge;
  } // end if no inputs 
} // end makingProgress


/* We aren't using this function because it isn't as efficient, and it is
   harder to implent

// Take a normalized step of length RANDOM_STEP_SIZES[dim]
vector RandPField::makeRandomState(vector fromState)
{
  
  double r, r2;
  vector rx;// = vector();
  
  rx = fromState;
  
  if (m_problem->StateDim == 3)
  {
    R >> r1;
    R >> r2;
    r1 = r1 * 3.1416 * 2;
    r2 = (r2 - .5) * 2;
    rx[0] += cos(r1) * RANDOM_STEP_SIZES[0];
    rx[1] += sin(r1) * RANDOM_STEP_SIZES[1];
    rx[2] += r2 * RANDOM_STEP_SIZES[2];
  }
  else
  {
  
  }
  // make sure the new vector has valid rotations
  fixVector(rx);

  return rx;
} // end makeRandomState
*/


/**
 * makeRandomState(vector)
 *
 * Generates a new state at random, based on the passed in state. This new state
 * is within the given step sizes for all angles (given in TRANSLATION_STEP_SIZE
 * and ROTATION_STEP_SIZE)
 *
 * This function is used ONLY when the problem is holonomic
 */
vector RandPField::makeRandomState(vector fromState)
{
  int i;
  double r, randomStep;
  vector rx = vector();

  rx = fromState;
  for (i = 0; i < m_problem->StateDim; i++) 
  {
   R >> r;
   randomStep = r * (RANDOM_STEP_SIZES[i] * 2) - RANDOM_STEP_SIZES[i];
   rx[i] += randomStep;
  }
  
  // make sure the new vector has valid rotations
  fixVector(rx);

  return rx;
} // end makeRandomState


/**
 * makeRandomInput(vector)
 *
 * Generates a new state and it's corresponding input based on the passed in
 * state. This new state is generated by integrating one of the valid inputs,
 * chosen at random, for PalnnerDeltaT time units.
 *
 * This function is used ONLY when the problem is non-holonomic
 */
void RandPField::makeRandomInput(vector &newState, vector &newInput, 
                                  vector startState)
{
  double r;
  R >> r;
  int randInput = (int) (r * m_possibleInputSize);
  newInput = m_possibleInputs[randInput];

  // Perform integration from state x, using input u, over time deltat.
  newState = m_problem->Integrate(startState, newInput, PlannerDeltaT);
} // end makeRandomInput


/**
 * stuck()
 *
 * Determines if the planner is stuck by using the total distance between 
 * some number of nodes, STUCK_NUMBER_OF_NODES, over distance between the 
 * last of the nodes, and the most recent node, compared to STUCK_RATIO.
 */
bool RandPField::stuck()
{
  if (G.number_of_nodes() <= STUCK_NUMBER_OF_NODES)
    return false;

  node oldNode = m_lastNode;
  node olderNode = oldNode;
  double sumOfEdges = 0;
  
  for (int i = 0; i < STUCK_NUMBER_OF_NODES; i++)
  {
    olderNode = oldNode;
    oldNode = G.source(G.first_in_edge(oldNode));
    sumOfEdges += Metric(G[oldNode], G[olderNode]);
  }

  double distance = Metric(G[oldNode], G[m_lastNode]);

  if (sumOfEdges / distance > STUCK_RATIO)
    return true;
  else
    return false;
} // end stuck

/* Not using this mehtod, because it is much more sensitive to the
   world in which it is running.
// Stuck using distance traveled vs STUCK_DISTANCE
bool RandPField::stuck()
{

  if (G.number_of_nodes() <= STUCK_NUMBER_OF_NODES)
    return false;

  node oldNode = m_lastNode;
  
  for (int i = 0; i < STUCK_NUMBER_OF_NODES; i++)
    oldNode = G.source(G.first_in_edge(oldNode));

  double distance = Metric(G[oldNode], G[m_lastNode]);

  if (distance < STUCK_DISTANCE)
  {
    return true;
  }

  return false;

} // end stuck
*/


/**
 * backtrack()
 *
 * This function is called when we are stuck before calling bounceWalk().
 * It removes the last BACKTRACK_NODES nodes from the graph, so that the
 * bouncewalk dosen't start from the location where we are stuck.
 */
void RandPField::backtrack() 
{
  node oldNode = m_lastNode;
  edge oldEdge = m_lastEdge;


  if (G.number_of_nodes() > BACKTRACK_NODES) 
  {
    // backtrack, removing now abandoned nodes
    for(int i = 0; i < BACKTRACK_NODES; i++) 
    {
      oldNode = G.last_node();
      oldEdge = G.last_edge();
      G.del_edge(oldEdge);
      G.del_node(oldNode);
    } // end for
  } // end if
  // If there aren't BACKTRACK_NODES nodes in the graph, just go all the
  //  way back to the beginning of the graph.
  else
  {
    for(int i = 0; i < G.number_of_nodes() - 1; i++) 
    {
      oldNode = G.last_node();
      oldEdge = G.last_edge();
      G.del_edge(oldEdge);
      G.del_node(oldNode);
    } // end for
  } // end else

  m_lastNode = G.last_node();
  m_lastEdge = G.last_edge();  
} // end backtrack


/**
 * takeBounceWalk()
 *
 * This function picks a random direction if holonomic or a random input if
 * non-holonomic and just goes untill it is in collision, or untill it has
 * gone MAX_BOUNCE_WALK_SIZE iterations. Once it stops, it will start a new
 * bounce RAND_BOUNCES number of times before returning.
 */
void RandPField::takeBounceWalk()
{
  for (int i = 0; i < RAND_BOUNCES; i++)
  {
    //cout << "Starting a new bounce from " << G[m_lastNode] << endl;
    vector step = vector(m_problem->StateDim);
    vector input;
    
    if (m_possibleInputSize > 0)
    {
      makeRandomInput(step, input, step);
    }
    else
    {
      step = makeRandomState(step);
      input = EMPTY_VECTOR;
    }

    vector lastState = G[m_lastNode];
    int j = 0;
    while(true)
    {
      if (j > MAX_BOUNCE_WALK_SIZE)
        break;

      vector newState = lastState + step;

      if (m_problem->CollisionFree(newState) && insideWorld(newState))
      {
        lastState = newState;
        if (m_possibleInputSize > 0)
        {
          fixVector(lastState);
          node newNode = G.new_node(lastState);
          m_lastEdge = G.new_edge(m_lastNode, newNode, input);
          m_lastNode = newNode;
        } // end if has inputs, add a node for each step
      }
      else
      {
        break;
      }
      j++;	
    }

    if (m_possibleInputSize == 0)
    {
      fixVector(lastState);
      node newNode = G.new_node(lastState);
      m_lastEdge = G.new_edge(m_lastNode, newNode, input);
      m_lastNode = newNode;
    } // end if no inputs, only add 1 new node
  } // end for number of RAND_BOUNCES
} // end takeBounceWalk


/**
 * fixVector(vector)
 *
 * This function 'fixes' a vector so that its values are in the acceptable
 * range given in Problem->UpperState, and Problem->LowerState. It makes sure
 * that all rotations are within the specified ranges.
 * This solves the problem of the planner not recognizing rotations as being S1
 */
bool RandPField::fixVector(vector &v)
{
    if (m_problem->StateDim == 3)
    {
      while (v[2] > m_problem->UpperState[2])
        v[2] = v[2] - (m_problem->UpperState[2] -
                     m_problem->LowerState[2]);
      while (v[2] < m_problem->LowerState[2])
        v[2] = v[2] + (m_problem->UpperState[2] -
                     m_problem->LowerState[2]);              
    } // end if 2d
    else if (m_problem->StateDim == 6)
    {
      for (int i = 3; i < 6; i++)
      {
        while (v[i] > m_problem->UpperState[i])
          v[i] = v[i] - (m_problem->UpperState[i] -
                     m_problem->LowerState[i]);
        while (v[i] < m_problem->LowerState[i])
          v[i] = v[i] + (m_problem->UpperState[i] -
                     m_problem->LowerState[i]); 
      } // end for v[3], v[4], and v[5]
    } // end if 3d
    
    return true;
} // end fixVector


/**
 * Determines if the vector v's X and Y (and Z if applicable) coordinates
 * are within the problem's range given by Problem->UpperState and 
 * Problem->LowerState. Problem->CollisionFree() and Problem->Sastified don't
 * seem to perform this function for you, as one may expect.
 */
bool RandPField::insideWorld(vector v)
{
  vector hi = m_problem->UpperState;
  vector lo = m_problem->LowerState;

  if (m_problem->StateDim == 3)
  {
    for (int i = 0; i < 2; i++)
    {
      if (v[i] >= hi[i] || v[i] <= lo[i])
        return false;
    } // end for
  } // end if
  else
  {
    for (int i = 0; i < 3; i++)
    {
      if (v[i] >= hi[i] || v[i] <= lo[i])
        return false;
    } // end for
  } // end if
  return true;
} // end insideWorld


/**
 * Metric
 *
 * This function is a replacement for Problem->Metric. It was written because
 * Problem->Metric didn't seem to account for rotation correctly, and it
 * seemed to adversely affect the potential field.
 */
double RandPField::Metric(vector state1, vector state2)
{
  // Uncomment the following line to use the metric in the Problem class
  //return m_problem->Metric(state1, state2);

  double distance = 0;
  double total = 0;
  double sum = 0;
  double angleDistance = 0;
  
  if (m_problem->StateDim == 3) 
  {
    for (int i = 0; i < 2; i++) 
    {
       sum = (state1[i] - state2[i]);
       sum = sum * sum;
       total = sum + total;
    }
 
    if (abs(state1[2] -state2[2]) 
		> abs(m_problem->UpperState[2]-m_problem->LowerState[2])/2) 
	{
      angleDistance = abs(m_problem->UpperState[2] 
		  - m_problem->LowerState[2])/2 - abs(state1[2] - state2[2]);
    }  
    else 
    {
      angleDistance = abs(state1[2] - state2[2]);
    }
    
    distance = sqrt(total) + angleDistance; 
  } // end if 2-d problem
  else if (m_problem->StateDim == 6) 
  {
  
    for (int i = 0; i<3; i++) 
    {
       sum = (state1[i] - state2[i]);
       sum = sum * sum;
       total = sum + total;
    }
    
    for (int i = 3; i<6; i++) 
    {
    
      if (abs(state1[i] -state2[i]) 
		  > abs(m_problem->UpperState[i]-m_problem->LowerState[i])/2) 
	  {
        angleDistance = angleDistance + abs(m_problem->UpperState[i] -
	       m_problem->LowerState[i])/2 - abs(state1[i] - state2[i]);
      }  
      else 
      {
        angleDistance = angleDistance + abs(state1[i] - state2[i]);
      }
      
    }
    
    distance = sqrt(total) + angleDistance; 
 
  } // end if 3-d problem
 
  return distance;
} // end Metric


/**
 * Smooth()
 *
 * Smooth tries to connect every other node with a straight line.
 * If there is a path without collision between the points it connects them and
 * deletes teh point in between.  Otherwise it does nothing and moves on to the
 * next points. 
 */
void RandPField::Smooth() 
{
  if (m_possibleInputSize > 0)
    return;
     
  node node1 = G.last_node();
  node node2 = G.pred_node(node1);
  node node3 = G.pred_node(node2);
   
  edge edge1 = G.last_edge();
  edge edge2 = G.pred_edge(edge1);
   
  double total = 0;
   
  //start from the end and work our way to the beginning of the graph
  while (node3 != G.first_node()) 
  {
    
    vector step = G[node3] - G[node1];
    bool canConnect;
  
    //initialize the step 
    if (m_problem->StateDim == 3) 
    {
      total = step[0] * step[0] + step[1] * step[1];
      step[1] = step[1]/sqrt(total) * TRANSLATION_STEP_SIZE;
      step[0] = step[0]/sqrt(total) * TRANSLATION_STEP_SIZE;
      step[2] = step[2]/(sqrt(total)/TRANSLATION_STEP_SIZE);
    } // end 2-d
  
    else if (m_problem->StateDim == 6) 
    {
      total = step[0] * step[0] + step[1] * step[1] + step[3] * step[3];
      step[0] = step[0]/sqrt(total) * TRANSLATION_STEP_SIZE;
      step[1] = step[1]/sqrt(total) * TRANSLATION_STEP_SIZE;
      step[2] = step[2]/sqrt(total) * TRANSLATION_STEP_SIZE;
       
      step[3] = step[3]/(sqrt(total)/TRANSLATION_STEP_SIZE);
      step[4] = step[4]/(sqrt(total)/TRANSLATION_STEP_SIZE);
      step[5] = step[5]/(sqrt(total)/TRANSLATION_STEP_SIZE);
    } // end 3-d
     
    canConnect = false;
    vector testvector = G[node3];
    vector node1vector = G[node1];

    // move towards node3 until there is a collision or we hit the point
    while(m_problem->CollisionFree(testvector) && !GapSatisfied(testvector,
      node1vector) && insideWorld(testvector)) 
    {
      testvector = testvector + step;
       
      if(GapSatisfied(testvector, node1vector)) 
      {
        canConnect = true; //There is a straight path between node1 and node3
      }
    } // end while move not collision or hit

   
    if (canConnect == true) 
    {
      edge tempedge = edge2;
      
      //delete old edges
      G.del_edge(edge1);
      G.del_edge(edge2);
      tempedge = G.new_edge(node1, node3, EMPTY_VECTOR);
     
      //delete middle node
      G.del_node(node2);
      node1 = node3;
      node2 = G.pred_node(node1);
      node3 = G.pred_node(node2);
       
    } // end if can connect
    else 
    {
      node1 = node2;
      node2 = node3;
      node3 = G.pred_node(node2);
    } // end else can't connect
  } // end while
} // end Smooth


#include <LEDA/UNDEFINE_NAMES.h>