public class ConfigurationSpace
{
  /*
  This class represents the configuration space.  More precisely, it represents a set of
  functions that can be used to dynamically represent the configuration space as needed.
  In our project, we use a configuration space that changes over time, and to some extent
  changes unpredictably over time.  This class allows us to generate not the entire
  configuration space, because this cannot be known, but a small window of the configuration
  space--a sequence of predicted snapshots of the field and goal in the immediate future.
  Within this window we grow an RRT tree to determine a path towards the goal that avoids
  (hopefully) the asteroids.
  */

  protected final static int MAX_PATH_LENGTH = 16, MAX_TREE_SIZE = 60;
  protected final static double BIAS = 0.14; // probability that we extend towards the goal

  protected double[] thrust, changeInDirection;
  protected int pathLength; // length of the path left to follow


  public ConfigurationSpace() // allocate memory for the configuration space
  {
    thrust = new double[MAX_PATH_LENGTH];
    changeInDirection = new double[MAX_PATH_LENGTH];
    pathLength = 0;
  } // constructor


  public void generatePath(SpaceShip pursuer, SpaceShip goal, AsteroidField field)
  {
    /* This function generates a list of commands that, when followed, lead the pursuer ship
    along a path to the goal.  We do this with our own special type of RRT.
    */

    SpaceShip target;
    TreeNode node, child, root = new TreeNode();
    root.ship = pursuer;

    // predict the future positions of the asteroid field
    AsteroidField [] fieldFrame = new AsteroidField [MAX_PATH_LENGTH];
    fieldFrame[0] = field;
    for (int i = 1; i<MAX_PATH_LENGTH; i++){
      fieldFrame[i] = new AsteroidField(fieldFrame[i-1]);
      fieldFrame[i].move();
    }

    // predict the future positions of the goal
    SpaceShip [] goalFrame = new SpaceShip [MAX_PATH_LENGTH];
    goalFrame[0] = goal;
    for (int i = 1; i<MAX_PATH_LENGTH; i++){
	goalFrame[i] = new SpaceShip(goalFrame[i-1]);
        goalFrame[i].move();
    }


    // build the RRT with up to MAX_TREE_SIZE nodes, but don't try more than twice this many
    /*
    RRT notes:

    First, there's a lot of open space.  We can use this to help optimize.  The first
    MAX_PATH_LENGTH nodes are extended straight towards the goal.  The rest only extend towards
    the goal with probability equal to the BIAS.  This allows us to fly directly to the goal if
    possible.

    Second, we have a tricky "Which comes first?" situation.  When extending to the goal, we want
    to use a future position of the moving goal, based on the depth of the node extended.  The
    node extended, however, depends on which node is closest to the goal we choose!  Which comes
    first?  They both require the other!  Well, we just assume that the future position of the
    goal is close to its original position and use the original position to choose a node.  Then
    we extend that node towards the future goal.  It seems to work fine.

    Third, this ain't no traditional tree.  Not only does it start out just as one long "stem",
    it cannot grow beyond a maximum height.  Why?  Well, time constraints don't allow us to look
    too for ahead.  And related to this, it usually isn't worth it since the configuration space
    changes unpredictably and it changes often.  Asteroids getting shot may be rare, but the goal
    ship accelerating and decelerating occurs often.  A short little RRT, in a configuration
    space the takes into account a predicted immediate future will be good enough.
    */

    int treeSize = 0;
    for (int i = 0; i < MAX_TREE_SIZE*2 && treeSize < MAX_TREE_SIZE; i++)
    {
       target = (Math.random() < BIAS || i < MAX_PATH_LENGTH ? goal : new SpaceShip());

       // find the node closest to the target, but the node must be less
       // than maximum depth so we can create a child.
       node = TreeNode.getClosestNodeLessThanDepth(root, target, MAX_PATH_LENGTH);

       if (target == goal) target = goalFrame[node.depth];

       child = node.extendTowards(target);

       if (!fieldFrame[child.depth-1].inCollision(child.ship))
       {
         node.children.addElement(child);
         treeSize++;
       }
    }

    // Choose the best path by finding the deepest node closest to the final goal.
    // Then follow that node back to the root, saving the commands to recreate each step.

    node = root.getClosestNodeToGoalAtDepth(MAX_PATH_LENGTH, goalFrame[MAX_PATH_LENGTH-1]);

    pathLength = node.depth;
    for (; node.depth > 0; node = node.previous)
    {
      changeInDirection[node.depth-1] = node.changeInDirection;
      thrust[node.depth-1] = node.thrust;
    }

  } // generatePath()


  public void followPath(SpaceShip pursuer)
  {
    // moves a space ship along a path, according to a list of commands previously generated

    if (pathLength > 0)
    {
      // the pursuer ship gets exact control over rotation and thrust
      pursuer.rotate(pursuer.center, changeInDirection[MAX_PATH_LENGTH - pathLength]);
      pursuer.rotation += changeInDirection[MAX_PATH_LENGTH - pathLength];
      pursuer.fireThrusters(thrust[MAX_PATH_LENGTH - pathLength]);
      pathLength--;
    }
  } // followPath()


  public boolean pathNearlyCompleted()
  {
    // Returns true if we have followed most of the commands in the path.
    // It is a good idea to never follow a path to completion since an asteroid field can change.

    return pathLength < 5;
  }
}