import java.util.Vector;

public class TreeNode
  /*
  This class implements the Tree used for the RRT algorithm.  It contains
  the functions for EXTEND.

  */

{
   public SpaceShip ship;
   public double thrust, changeInDirection;
   public int depth;
   public TreeNode previous;
   public Vector children;

   public TreeNode() // constructor
   {
     ship = null;
     thrust = 0.0;
     changeInDirection = 0.0;
     depth = 0;
     previous = null;
     children = new Vector();
   }

   public TreeNode extendTowards(SpaceShip target)
   {
     /*
     This is the function that acquires a new future position and extends the RRT
     tree to that new point.
      */
     TreeNode newNode = new TreeNode();
     SpaceShip newShip = new SpaceShip(ship);
     newNode.depth = depth + 1;
     newNode.previous = this;

     DPoint s, // source
            t, // target
            a, // next (with inertia)
            i, // intersection
            p, // plus
            q, // minus
            c; // choice

     s = ship.center;
     t = target.center;
     a = new DPoint(s, ship.speed, ship.direction); // point where the ship WOULD go
     i = new DPoint(s,t,a);  // perpendicular intersection of line ST with A

     // find where the ship COULD go, with thrust
     double add  = Math.sqrt(sqr(SpaceShip.MAX_PURSUER_THRUST) - sqr(i.distanceFrom(a)));

     if (Double.isNaN(add))
       c = new DPoint(a, SpaceShip.MAX_PURSUER_THRUST, DPoint.angle(a,i));
     else // vector can be adjusted completely
     {
       double theta = DPoint.angle(s,t);
       p = new DPoint(i,  add, theta);
       q = new DPoint(i, -add, theta);
       c = (p.distanceFrom(t) < q.distanceFrom(t) ? p : q);

       if (s.distanceFrom(c) > SpaceShip.MAX_PURSUER_SPEED)
         c = new DPoint(s, SpaceShip.MAX_PURSUER_SPEED, theta);
     }

     newNode.thrust = a.distanceFrom(c);
     newShip.rotation = (a.equals(c) ? ship.rotation : DPoint.angle(a,c));

     // this line makes the pursuer point towards the target ship
     if (newNode.thrust < 0.1) { newNode.thrust = 0.0; newShip.rotation = DPoint.angle(s,t); }

     // update newShip, and determine the commands for the pursuer ship to follow its path 
     newNode.changeInDirection = newShip.rotation - ship.rotation;
     newShip.rotate(newShip.center, newNode.changeInDirection);
     newShip.fireThrusters(newNode.thrust);
     newShip.move();
     newNode.ship = newShip;

     return newNode;
   } // extendTowards()

   private static double sqr(double d) { return d*d; }

   public static TreeNode getClosestNodeLessThanDepth(TreeNode node, SpaceShip target, int depth)
   {
      /*
      Retreives a node from the RRT tree that is closest to the target less than some depth limit
      */      

      TreeNode closest = node, testClosest;

      if (node.depth + 1 < depth)
        for (int i = 0; i < node.children.size(); i++)
        {
          testClosest = getClosestNodeLessThanDepth((TreeNode)(node.children.elementAt(i)), target, depth);
          if (target.center.distanceFrom(testClosest.ship.center) <
              target.center.distanceFrom(closest.ship.center) )
              closest = testClosest;
        }

      return closest;
   }

   public TreeNode getClosestNodeToGoalAtDepth(int depth, SpaceShip goal)
   {
     //Retreives a node from the RRT tree that is closest to the goal at the depth limit

     Vector nodes = new Vector();
     addNodesAtDepth(depth, this, nodes);
     TreeNode testClosest, closest = getDeepest(this, this);

     for (int i = 0; i < nodes.size(); i++)
     {
       testClosest = (TreeNode)(nodes.elementAt(i));

       if ( goal.center.distanceFrom(testClosest.ship.center) <
            goal.center.distanceFrom(closest.ship.center) )
         closest = testClosest;
     }

     return closest;
   }

   private static void addNodesAtDepth(int depth, TreeNode currNode, Vector nodes)
   {
     //adds all nodes at this depth to the vector nodes
     if (currNode.depth == depth)
        nodes.addElement(currNode);

     for (int i = 0; i < currNode.children.size(); i++)
        addNodesAtDepth(depth, (TreeNode)(currNode.children.elementAt(i)), nodes);
   }

   private static TreeNode getDeepest(TreeNode current, TreeNode deepest)
   {
     //gets the deepest node in the RRT tree starting at some current node.

     TreeNode testDeepest;
     if (current.depth > deepest.depth) deepest = current;

     for (int i = 0; i < current.children.size(); i++)
     {
       testDeepest = getDeepest((TreeNode)(current.children.elementAt(i)), deepest);
       if (testDeepest.depth > deepest.depth) deepest = testDeepest;
     }

     return deepest;
   }
}