#include <LEDA/file.h>
#include <LEDA/list.h>
#include <LEDA/point.h>
#include <LEDA/polygon.h>
#include <LEDA/random.h>
#include <LEDA/stream.h>
#include <LEDA/string.h>
#include <LEDA/vector.h>

#include <LEDA/REDEFINE_NAMES.h>

class RandPField: public Planner 
{
 public:


  /**
   * RandPField <constructor>
   *
   * The default constructior for Planner. Initilizes all constants and
   * golbal variables to their default values.
   */
  RandPField(Problem *p);

  /**
   * Destructor (not implemented)
   */
  virtual ~RandPField() {};

  /**
   * Connect(...)
   *
   * This funcion is not necessary for the Randomized potential field.
   * calling it has no effect.
   */
  virtual bool Connect(const vector &x, GRAPH<vector,vector> &g, node &nn,
		       bool forward);

  /**
   * Construct(...)
   *
   * This funcion is not necessary for the Randomized potential field.
   * calling it just calls Plan()
   */
  virtual void 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()
   */
  virtual bool Plan();

  /**
   * 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 init(int numberOfRandomStates, int stuckPathSize, 
     int numberOfBounces, int numberOfBounceSteps, int backtrackLength, 
     double stuckRatio, double translationStepSize, double rotationStepSize,
     int smoothingIterations, bool forceHolonomic);
  
  /**
   * makeProgress()
   *
   * This function adds one node and one edge to the graph. This is the 
   * standard descend function in the rpf.
   */
  void makeProgress();
  
  /**
   * 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 stuck();

  /**
   * 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 takeBounceWalk();
  
  /**
   * 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 backtrack();
  
  /**
   * 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 Smooth();
  
  /**
   * 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 makeRandomState(vector fromState);
  
  /**
   * makeRandomInput(&vector, &vector, 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 makeRandomInput(vector &newState, vector &newInput, vector startState);
  
  /**
   * insideWorld(vector)
   *
   * 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 insideWorld(vector v);
  
  /**
   * 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 fixVector(vector &v);
  
  /**
   * Metric(vector, vector)
   *
   * 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 Metric(vector state1, vector state2);


  // User controlled constants (can be easily set via the init(...) function):
  int NUMBER_RANDOM_POINTS;
  int BACKTRACK_NODES;
  int STUCK_NUMBER_OF_NODES;
  int RAND_BOUNCES;
  int MAX_BOUNCE_WALK_SIZE;
  double STUCK_RATIO;
  double TRANSLATION_STEP_SIZE;
  double ROTATION_STEP_SIZE; 
  int SMOOTH_ITERATIONS;

  // Golbal variables
  Problem *m_problem;
  int m_currentState;
  vector *m_possibleInputs;
  int m_possibleInputSize;
  int m_stuckCount;
  node m_lastNode;
  edge m_lastEdge;
  double m_totalTimeToSolve;
  vector EMPTY_VECTOR;
  vector RANDOM_STEP_SIZES; 
};


#include <LEDA/UNDEFINE_NAMES.h>