package activeTable;
import render.*;

public class ActiveTableDisplay extends RenderApplet implements ActiveTableInterface
{
public static boolean debug = false;
   // DATA

   VehicleInfo vi[];
   public Geometry table, vehicle[];
   Material white, silver, black, red, shade;
   double Y = -.1;

   final static double MAXSPEED = .1355;

   public boolean connect() {

      // MAKE SURE TO DECLARE A REASONABLE NUMBER OF VEHICLES

      N = Math.max(0, Math.min(10, N));;
      this.N = N;

      // CREATE ALL THE VEHICLE INTERFACES

      vi = new VehicleInfo[N];
      for (int i = 0 ; i < N ; i++)
         vi[i] = new VehicleInfo();

      // INITIALIZE MATERIALS

      white  = (new Material()).setColor(.5,.5,.5, 0,0,0,20, .6,.6,.6);
      silver = (new Material()).setColor(.3,.3,.3, 1,1,1,20, .4,.4,.4);
      black  = (new Material()).setColor(.5,.5,.5, 0,0,0,20);
      red    = (new Material()).setColor(0,0,0, 0,0,0,20, 1,0,0);
      shade  = (new Material()).setColor(0,0,0).setTransparency(.7);

      push();
         translate(0,Y,0);
         transform(world);
      pop();

      // INITIALIZE THE TABLE TOP

      push();
	 table = world.add().cube();
	 Material tableColor = new Material();
	 tableColor.setColor(.3,.3,.3, 0,0,0,1, .5,.5,.5);
	 table.setMaterial(tableColor);
	 translate(0,-.01,0);
	 scale(.5,.01,.5);
	 transform(table);
      pop();

      // INITIALIZE THE TABLE BASE

      push();
	 Geometry base = world.add().cube();
	 base.setMaterial(black);
	 translate(0,-.504,0);
	 scale(.504,.5,.504);
	 transform(base);
      pop();

      // CREATE ALL THE VEHICLES

      vehicle = new Geometry[N];
      for (int i = 0 ; i < N ; i++)
	 createVehicle(vehicle[i] = world.add());

      return true;
   }

   // INITIALIZATION (CALLED ONCE)

   public void initialize() {

      // LIGHTS, CAMERA, ...

      setBgColor(.4,.4,.5); // SKY BLUE BACKGROUND
      setFOV(.1);
      addLight(1,1,1, .7,.7,1);
      addLight(-1,0,-1, .5,.4,.3);
      addLight(0,-1,-.5, .2,.2,.2);
      renderer.headsUp(true);

      // MAKE SURE AT LEAST ONE VEHICLE HAS BEEN DECLARED

      if (vi == null)
	 connect();

      // INITIALIZE VEHICLE POSITIONS/DIRECTIONS

      for (int i = 0 ; i < N ; i++) {
	 push();
	    translate(vi[i].x, 0, -vi[i].y);
	    transform(vehicle[i]);
	 pop();
	 push();
	    rotateY(-vi[i].theta);
	    transform(vehicle[i].child[0]);
	 pop();
      }
   }

   double dt = 0, oldTime = 0;
   public void animate(double time) {
      dt = time - oldTime;
      oldTime = time;
      for (int j = 0 ; j < N ; j++)
	 vehicle[j].child[0].child[0].setMaterial(isAtGoal(j) ? black : red);
   }

   int puck;
   public void setPuck(int i) { puck = i; }
   public int getPuck() { return puck; }

   int N;
   public void setMaxVehicles(int n) { N = n; }
   public int getMaxVehicles() { return N; }

   public VehicleInfo getVehicleInfo(int i) {
      return vi[i];
   }

   public boolean isAtGoal(int i) {
      Matrix MV = vehicle[i].getMatrix();
      double dx = vi[i].x - MV.get(0,3);
      double dy = vi[i].y + MV.get(2,3);
      double distanceToTarget = Math.sqrt(dx*dx + dy*dy);
      return distanceToTarget <= .02
             && Math.abs(getTheta(i, vi[i]) - vi[i].theta) <= .02;
   }

   public double getTheta(int i, VehicleInfo vi) {
      Matrix P = vehicle[i].child[0].getMatrix();
      double theta = Math.atan2(P.get(0,2),P.get(0,0));
      if (theta > vi.theta + Math.PI) theta -= 2*Math.PI;
      if (theta < vi.theta - Math.PI) theta += 2*Math.PI;
      return theta;
   }

   double speed = 0;

   public void setVehicleInfo(int i, VehicleInfo vi) {
      Matrix MP = vehicle[i].child[0].getMatrix();

      double theta = getTheta(i, vi);

      Matrix MV = vehicle[i].getMatrix();
      double dx = vi.x - MV.get(0,3);
      double dy = vi.y + MV.get(2,3);
      double distanceToTarget = Math.sqrt(dx*dx + dy*dy);

      double forwardTheta = Math.atan2(dy,dx);
      if (forwardTheta > theta + Math.PI) forwardTheta -= 2*Math.PI;
      if (forwardTheta < theta - Math.PI) forwardTheta += 2*Math.PI;

      double reverseTheta = forwardTheta + Math.PI;
      if (reverseTheta > theta + Math.PI) reverseTheta -= 2*Math.PI;
      if (reverseTheta < theta - Math.PI) reverseTheta += 2*Math.PI;

      // ALREADY AT TARGET?  ROTATE INTO PROPER ORIENTATION

      if (distanceToTarget < .01) {
         MP.rotateY(limit(vi.theta - theta,dt));
	 return;
      }

      // ALREADY AIMED AT TARGET?  MOVE TOWARD TARGET

      double angleDiff = Math.abs(forwardTheta - theta);
      if (angleDiff < .01) {
	 if (! isBlocked(i)) {
	    speed = .5 * speed + .5 * MAXSPEED;
	    double travel = MAXSPEED * dt;
	    double s = Math.sqrt(dx*dx + dy*dy);
            MV.translate(travel * dx/s, 0, travel * -dy/s);
         }
	 else
	    speed = .5 * speed;
	 return;
      }

      // NOT YET AIMED AT TARGET?  ROTATE TO FACE TARGET

      MP.rotateY(limit(forwardTheta - theta,dt));
   }

   boolean isBlocked(int i) {
      Matrix MV = vehicle[i].getMatrix();
      Matrix MP = vehicle[i].child[0].getMatrix();
      double x = MV.get(0,3) + MAXSPEED * dt * MP.get(0,0);
      double z = MV.get(2,3) + MAXSPEED * dt * MP.get(2,0);
      double dd = DIAMETER * DIAMETER;
      for (int j = 0 ; j < N ; j++)
	 if (j != i) {
	    MV = vehicle[j].getMatrix();
            double dx = MV.get(0,3) - x;
            double dz = MV.get(2,3) - z;
	    if (dx * dx + dz * dz < dd)
	       return true;
	 }
      return false;
   }

   double limit(double t, double bound) {
      return Math.max(-bound, Math.min(bound, t));
   }

   // CREATE A SINGLE VEHICLE

   // ALL DIMENSIONS ARE IN METERS

   public final static double DIAMETER       = .0835; // OUTER BODY DIAMETER
   public final static double WHEELBASE      = .0600; // DISTANCE BETWEEN LEFT/RIGHT WHEELS
   public final static double WHEEL_DIAMETER = .0330; // DIAMETER OF ONE WHEEL
   public final static double Y_TOP          = .0410; // Y AT TOP OF VEHICLE BODY
   public final static double Y_BOTTOM       = .0050; // Y AT BOTTOM OF VEHICLE BODY

   void createVehicle(Geometry vehicle) {

      double radius = DIAMETER / 2;
      double height = Y_TOP - Y_BOTTOM;
      double wheel_radius = WHEEL_DIAMETER / 2;

      // THE FIRST CHILD OBJECT HAS THE VEHICLE'S PARTS

      Geometry parts = vehicle.add();

      // MAKE THE (FAKE) GROUND SHADOW

      Geometry shadow = vehicle.add().disk(20);
      shadow.setMaterial(shade);
      push();
	 translate( -.07*radius,.002, -.07*radius);
	 scale(1.07*radius, .001, 1.07*radius);
	 rotateX(-Math.PI/2);
	 transform(shadow);
      pop();

      //----------------- MAKE ALL THE PARTS OF THE VEHICLE

      // THE FRONT-POINTING ARROW ON TOP

      Geometry arrow = parts.add().disk(3);
      arrow.setMaterial(black);
      push();
	 translate(-WHEELBASE/9, 1.1 * Y_TOP, 0);
	 scale(WHEELBASE/2, 1, WHEELBASE/4);
	 rotateX(-Math.PI/2);
	 transform(arrow);
      pop();

      // THE WHITE CYLINDRICAL BODY

      Geometry body = parts.add().cylinder(20);
      body.setMaterial(white);
      push();
	 translate(0, Y_BOTTOM + height/2, 0);
	 scale(radius, height/2, radius);
	 rotateX(Math.PI/2);
	 transform(body);
      pop();

      // THE TWO WHEELS WITH THEIR TIRES

      for (int lr = 0 ; lr <= 1 ; lr++) {

         push();
	    translate(0, wheel_radius, lr==0 ? WHEELBASE/2 : -WHEELBASE/2);
	    scale(wheel_radius, wheel_radius, wheel_radius);

	    if (isDetailedModel) {
               Geometry wheel = parts.add().pill(20, .3,.7);
               wheel.setMaterial(silver);
               push();
	          scale(.9,.9,.22);
	          transform(wheel);
               pop();
            }

            Geometry tire = parts.add().setMaterial(black);
            push();
	       if (isDetailedModel) {
	          scale(1,1,.11);
	          transform(tire.pill(20,.2,.8));
               }
	       else {
	          scale(1,1,.22);
	          transform(tire.cylinder(20));
               }
            pop();
         pop();
      }
   }

   boolean isDetailedModel = false;
}