/*
 * Alec Jacobson
 * 
 */
import java.awt.*;
import java.util.LinkedList;
import java.util.ArrayList;
import java.util.PriorityQueue;
/**
 *
 * @author ajx
 */



/**
 * Class to hold a character
 *
 */
class Character implements Comparable{
  double x,y,z;//location of character 
  double cam_z;
  double rotZ, rotX, rotY;
  double sx = 1.;
  double sy = 1.;
  double sz = 1.;
  double radius;
  Point velocity;
  LinkedList<Surface> s_list;//list of surfaces associated with this Character
  Matrix4x4 mat;//transformation to entire character
  
  public Character(){
    init();
  }
  
  public void init(){
    //init matrix for machine as whole
    mat = new Matrix4x4();
    mat.identity();
    
    s_list = new LinkedList<Surface>();
    //make individual parts here
  }
  
  /**go through all of the surfaces and update matrices*/
  public void updateMatrices(){

    this.mat.identity();
    this.mat.translate(x,y,z);
    this.mat.rotateZ(rotZ);
    this.mat.rotateX(rotX);
    this.mat.rotateY(rotY);
    this.mat.scale(sx ,sy, sz);
    
    //update matrices of all surfaces
    for(Surface s: s_list)
      s.matrix = mat.getCopy();
  }
  
  public void applyVelocity(double dt){
    x += dt*velocity.x;
    y += dt*velocity.y;
    z += dt*velocity.z;
    
  }
  
  public void addToVelocity(double x, double y, double z){
    velocity.x += x;
    velocity.y += y;
    velocity.z += z;
  }
  
  public void multiplyToVelocity(double x, double y, double z){
    velocity.x *= x;
    velocity.y *= y;
    velocity.z *= z;
  }
  
  public int compareTo(Object o){
    Double t = this.cam_z;
    Double s = ( (Character) o).cam_z;
    int cam = t.compareTo(s);
    if( cam == 0){
      t = this.z;
      s = ( (Character) o).z;
      return t.compareTo(s);
    }
    return cam;
  }
  
}

/**
 * Boxer character with punching arms 
 */
class Boxer extends Character{
  final double TORSO_RATIO = 1.5;
  final double HEAD_RATIO = 2.0;
  int res;
  double headWidth;//used to define all other distances
  double headDepth;//used to define all other distances
  double headHeight;//used to define all other distances
  double torsoWidth;
  double torsoDepth;
  double torsoHeight;
  double shoulderWidth;
  double shoulderHeight;
  double shoulderDepth;
  double elbowWidth;
  double elbowDepth;
  double elbowHeight;
  double fistWidth;
  double fistDepth;
  double fistHeight;
  double lowWidth;
  double lowDepth;
  double lowHeight;
  double upWidth;
  double upDepth;
  double upHeight;
  double donutWidth;
  double donutDepth;
  double donutHeight;
  double head2torso;
  double head2shoulder;
  double torso2shoulder;
  double torso2donut;
  double shoulder2up;
  double up2elbow;
  double elbow2low;
  double low2fist; 
  double mass;
  Color color;//or should each surface have its own color?
  boolean isPunchingLeft;
  boolean isPunchingRight;
  boolean isChargingLeft;
  boolean isChargingRight;
  boolean inMovie;
  Sphere head;
  Sphere shoulder_l;
  Sphere shoulder_r;
  Sphere elbow_l;
  Sphere elbow_r;
  Sphere fist_l;
  Sphere fist_r;
  Cylinder torso;
  Cylinder up_r; 
  Cylinder low_r;
  Cylinder up_l; 
  Cylinder low_l;
  Torus donut;
  double top_twist_l;
  double top_rot_l;
  double bot_rot_l;
  double time_punch_l;
  double time_charge_l;
  double time_r;
  double leg_velo;
  double leg_spin;
  double pow;
  Point left_v;
  double u;
  
  public Boxer(int r, double hh){
    res = r;
    headHeight = hh;
    init();
  }
  
  public void init(){
    inMovie = false;
    //initialize a bunch of variables, rmemeber headHeight is golden
    initMeasurements(); 
    //THIS SHOULD ALWAYS BE THE Y-Height to location of character
    radius = head2torso;
    //radius = head2torso+torso2donut+donutHeight/3; 
    
    u = 1.0;
    left_v = new Point(0,0,0);
  
    mat = new Matrix4x4();
    mat.identity();
    
    velocity = new Point(0.0,0.0,0.0);
    
    s_list = new LinkedList<Surface>();
    isPunchingLeft = false;
    isPunchingRight = false;
    //make individual parts here

    //make the head (a oblong sphere)
    head = new Sphere(res/2, res);
    s_list.add(head);
   
    //make torso (cylinder slight oblong?)
    torso = new Cylinder(res);
    s_list.add(torso);
    
    //make "legs"
    donut = new Torus(res, res/2, .3, 1.0);
    s_list.add(donut);
    
    //make arms
    //left
    shoulder_l = new Sphere(res/2, res);
    s_list.add(shoulder_l);
    up_l = new Cylinder(res);
    s_list.add(up_l);
    elbow_l = new Sphere(res/2,res);
    s_list.add(elbow_l);
    low_l = new Cylinder(res);
    s_list.add(low_l);
    fist_l = new Sphere(res/2,res);
    s_list.add(fist_l);
    
    
    
  }
  
  private void initMeasurements(){
    //measurements
    headWidth = headHeight;
    headDepth = headHeight;
    torsoHeight = headHeight*2;
    torsoDepth = headDepth*2;
    torsoWidth = headHeight;
    donutHeight = headHeight*2;
    donutWidth = donutHeight;//these are strange
    donutDepth = donutHeight;//these are strange
    shoulderHeight = headHeight*0.5;
    shoulderDepth = shoulderHeight;
    shoulderWidth = shoulderHeight;
    upHeight = headHeight;
    upWidth = shoulderWidth*.6;
    upDepth = shoulderDepth*.6;
    elbowHeight = shoulderHeight;
    elbowWidth = elbowHeight;
    elbowDepth = elbowHeight;
    lowHeight = headHeight;
    lowWidth = elbowWidth*.5;
    lowDepth = elbowDepth*.5;
    fistHeight = headHeight*0.75;
    fistWidth = fistHeight;
    fistDepth = fistHeight;
    setDisplacements();
}
  private void setDisplacements(){
    //(dis)placements
    head2torso = u*(headHeight+torsoHeight);
    head2shoulder = u*(headHeight+shoulderHeight);
    torso2shoulder = u*(torsoDepth + shoulderWidth);
    torso2donut = u*(torsoHeight+donutHeight/3);
    shoulder2up = u*(shoulderHeight + upHeight);
    up2elbow = u*(upHeight + elbowHeight);
    elbow2low = u*(elbowHeight + lowHeight);
    low2fist = u*(lowHeight + fistHeight);//change to + 0 for half sphere

  }
  
  public void setColor(Color c){
    color = c;
    for(Surface s: s_list)
      s.color = color;
  }
  
  public void applyVelocity(double dt){
    x += dt*velocity.x;
    y += dt*velocity.y;
    z += dt*velocity.z;
    leg_spin += dt*Math.PI*leg_velo;
    
  }
  public static double lerp(double f , double y0, double y1){
    return y0 + (y1-y0)*f;
  }
  
  double top_rot_punch[] =  {0,-0.25,-0.25,0.05};
  double bot_rot_punch[] =  {0,-1.25,-1.375,-1.5};
  double top_twist_punch[] = {0,0.15,0.15,0.05};
  double timing_punch[] = {0.0,0.1, .2,.4};
  
  
  double old_left[];
  public  void setLeftV(double vec[], double dt){
    if(null!=old_left){
      left_v = new Point((vec[0]-old_left[0])/dt,(vec[1]-old_left[1])/dt,(vec[2]-old_left[2])/dt);
    }
    //save last position
    old_left = vec;
  }
  
  
  private void animatePunchLeft(){
  
  
  
    double t = System.currentTimeMillis()/1000.0 - time_punch_l;
    //find index
    int i;
    isPunchingLeft = false;
    
    
    
    for(i=0;i<timing_punch.length-1;i++){
      
      if((pow*timing_punch[i]<=t)&&(t<pow*timing_punch[i+1])){
        isPunchingLeft = true;
        break;
      }
    }
    if(!isPunchingLeft){
      return;
    }
    
    //compute the fraction of time interval
    double fraction = (t - pow*timing_punch[i]) / (pow*timing_punch[i+1] - pow*timing_punch[i]);
    top_rot_l = Math.PI*lerp(fraction, top_rot_punch[i], top_rot_punch[i+1]);
    bot_rot_l = Math.PI*lerp(fraction, bot_rot_punch[i], bot_rot_punch[i+1]);
    top_twist_l = -1*Math.PI*lerp(fraction, top_twist_punch[i], top_twist_punch[i+1]);
  }



  
  public void punchLeft(){
    if(isPunchingLeft)
      return;
    pow = (System.currentTimeMillis()/1000.0 - time_charge_l);
    pow = Math.max(1.0, 2.0/pow);
    
    isChargingLeft = false;
    time_punch_l = System.currentTimeMillis() /1000.0;
    isPunchingLeft = true;
    //fill in dummy values
    top_rot_punch[0] = top_rot_l/Math.PI;
    bot_rot_punch[0] = bot_rot_l/Math.PI;
    top_twist_punch[0] = top_twist_l/Math.PI/-1;

  }
  
  
  double top_rot_charge[] =  {0.05,0.15};
  double bot_rot_charge[] =  {-1.5,-1.6};
  double top_twist_charge[] = {0.05,0.15};
  double timing_charge[] = {0.0, 0.2};
  
  

  
  
  private void animateChargeLeft(){
    if(leg_velo>-1) leg_velo=leg_velo*1.1;
      
      
    double t = System.currentTimeMillis()/1000.0 - time_charge_l;
    //find index
    int i;
    for(i=0;i<timing_charge.length-1;i++){
      if((timing_charge[i]<=t)&&(t<timing_charge[i+1])){
        break;
      }
    }
    if(i>=timing_charge.length-1){
      return;
    }
    
    //compute the fraction of time interval
    double fraction = (t - timing_charge[i]) / (timing_charge[i+1] - timing_charge[i]);
    top_rot_l = Math.PI*lerp(fraction, top_rot_charge[i], top_rot_charge[i+1]);
    bot_rot_l = Math.PI*lerp(fraction, bot_rot_charge[i], bot_rot_charge[i+1]);
    top_twist_l = -1*Math.PI*lerp(fraction, top_twist_charge[i], top_twist_charge[i+1]);
  }
  
  public void chargeLeft(){
    if(isChargingLeft||isPunchingLeft)
      return;
    leg_velo = -0.1;
    time_charge_l = System.currentTimeMillis() /1000.0;
    isChargingLeft = true;
  }
  
  
  
  
  /**go through all of the surfaces and update matrices*/
  public void updateMatrices(){
    u = (u-1.0)*0.9+1.0;
    setDisplacements();
    
    if(isChargingLeft){
      animateChargeLeft();
    }
    else if(isPunchingLeft){
      animatePunchLeft();
    }
    else if(inMovie){
    }else{
      top_twist_l = -1*Math.PI*top_twist_charge[0];
      top_rot_l = Math.PI*top_rot_charge[0];
      bot_rot_l = Math.PI*bot_rot_charge[0];
      leg_velo=leg_velo*0.9;
    }
    
    this.mat.identity();
    this.mat.translate(x,radius+y,z);
    this.mat.rotateZ(rotZ);
    this.mat.rotateX(rotX);
    this.mat.rotateY(rotY);
    
    //update matrices of all surfaces
    
    head.matrix = this.mat.getCopy();
    head.matrix.scale(headWidth, headHeight, headDepth);
    
    torso.matrix = this.mat.getCopy();
    torso.matrix.translate(0,-head2torso,0);
    torso.matrix.scale(torsoWidth, torsoHeight,torsoDepth);
    torso.matrix.rotateX(Math.PI*.5);
    
    donut.matrix = this.mat.getCopy();
    donut.matrix.translate(0,-head2torso-torso2donut,0);
    donut.matrix.scale(donutWidth, donutHeight, donutDepth);
    donut.matrix.rotateX(Math.PI*.5);
    donut.matrix.rotateZ(leg_spin);
    
    shoulder_l.matrix = this.mat.getCopy();
    shoulder_l.matrix.translate(0,-head2shoulder,torso2shoulder);
    shoulder_l.matrix.scale(shoulderWidth, shoulderHeight, shoulderDepth);
    shoulder_l.matrix.rotateX(Math.PI*top_twist_l);
    shoulder_l.matrix.rotateZ(Math.PI*top_rot_l);

    up_l.matrix = shoulder_l.matrix.getCopy();
    up_l.matrix.scale(1./shoulderWidth, 1./shoulderHeight, 1./shoulderDepth);
    up_l.matrix.translate(0,-shoulder2up,0);
    up_l.matrix.scale(upWidth, upHeight, upDepth);
    up_l.matrix.rotateX(Math.PI*.5);
    
    elbow_l.matrix = up_l.matrix.getCopy();
    elbow_l.matrix.rotateX(-Math.PI*.5);
    elbow_l.matrix.scale(1./upWidth, 1./upHeight,1./upDepth);
    elbow_l.matrix.translate(0,-up2elbow,0);
    elbow_l.matrix.scale(elbowWidth, elbowHeight, elbowDepth);
    elbow_l.matrix.rotateZ(Math.PI*bot_rot_l);
    
    low_l.matrix = elbow_l.matrix.getCopy();
    low_l.matrix.scale(1./elbowWidth, 1./elbowHeight,1./elbowDepth);
    low_l.matrix.translate(0,-elbow2low,0);
    low_l.matrix.scale(lowWidth, lowHeight, lowDepth);
    low_l.matrix.rotateX(Math.PI*0.5);
    
    fist_l.matrix = low_l.matrix.getCopy();
    fist_l.matrix.rotateX(-Math.PI*.5);
    fist_l.matrix.scale(1./lowWidth, 1./lowHeight,1./lowDepth);
    fist_l.matrix.translate(0,-low2fist,0);
    fist_l.matrix.scale(fistWidth, fistHeight, fistDepth);

  }
  
  
}








/**
 * Table or base, just a simple cube stretched out
 */
class Table extends Character{
  Color color;
  public Table(double lr, double wr, double hr, double r){
    sx = lr;
    sy = wr;
    sz = hr;
    radius = r;
    init();
  }

  public void setColor(Color c){
    color = c;
    for(Surface s: s_list)
      s.color = color;
  }
 
 public void init(){
    //init matrix for machine as whole
    mat = new Matrix4x4();
    mat.identity();
    
    //velocity to zero
    velocity = new Point(0.,0.,0.);
    
    s_list = new LinkedList<Surface>();
    //make individual parts here
    s_list.add( new Cube());
    
  }
     
     
     /**go through all of the surfaces and update matrices*/
  public void updateMatrices(){
    
    
    this.mat.identity();
    this.mat.translate(x,y,z);
    this.mat.rotateX(rotX);
    this.mat.rotateY(rotY);
    this.mat.rotateZ(rotZ);
    this.mat.scale(sx*radius ,sy*radius, sz*radius);
    
    //update matrices of all surfaces
    for(Surface s: s_list)
      s.matrix = mat.getCopy();
  
  }
 
 
}


/** class to hold surface info**/
class Surface{
  ArrayList<Point> v_list;//vertex list
  LinkedList<int[]> f_list;//facelist
  Matrix4x4 matrix;
  Color color;
  
  public Surface(){
    v_list = new ArrayList<Point>();
    f_list = new LinkedList<int[]>();
    matrix = new Matrix4x4();
    matrix.identity();//start with identity
    color = Color.WHITE;//just in case color is not later defined
  }
  
  public void addVertex(Point p){
    v_list.add(p);
  }
  
  //assume distinct is all false
  public void addFace(Point[] a){
    boolean[] distinct = new boolean[a.length];//default false
    addFace(a, distinct);
  }

  
  public void addFace(Point[] a, boolean[] distinct){
    int[] face = new int[a.length];
    boolean[] found = new boolean[a.length];
    for(int i = 0; i < v_list.size(); i++){
      for(int j = 0; j < a.length; j++){
        if(!distinct[j]
          &&a[j].equals(v_list.get(i))){
          face[j] = i;
          found[j] = true;
        }
      }
    }
    
    //add all the vertices which didn't already exist
    for( int j = 0; j< a.length;j++){
      if(!found[j]){
        int i = v_list.size();
        v_list.add(a[j]);
        face[j] = i;
      }
    }
    
    f_list.add(face);
  
  }
  
}



class Torus extends Surface{
  public Torus(int lat, int lon, double lr, double br){
    v_list = new ArrayList<Point>();
    f_list = new LinkedList<int[]>();
    matrix = new Matrix4x4();
    matrix.identity();//start with identity
    color = Color.WHITE;//just in case color is not later defined
    make(lat, lon, lr, br);
  }
  
  private void make(int lat, int lon, double lr, double br){
    double theta = 2.*Math.PI/(double)lat;//change in angle
    double phi = 2.*Math.PI/(double)lon;
    double r = lr;
    double R = br;
    double x = R+r;
    double y = 0;
    double z = Math.sin(Math.PI/lat);
    //make all of the sides
    Point[] prev = new Point[lon];
    Point[] first = new Point[lon];
    for(int j = 0; j<prev.length; j++){
      x = Math.cos( 0*theta)*(R + r*Math.cos(j*phi)); 
      y = Math.sin( 0*theta)*(R + r*Math.cos(j*phi));
      z = r*Math.sin(j*phi);
      prev[j] = new Point(x,y,z);
      first[j] = new Point(x,y,z);
    }
    
    Point[] cur;
    for(int i = 1; i<= lat; i++){
      //make the cur ring
      cur = new Point[lon];
      for(int j = 0; j<lon; j++){
		x = Math.cos( i*theta)*(R + r*Math.cos(j*phi)); 
		y = Math.sin( i*theta)*(R + r*Math.cos(j*phi));
		z = r*Math.sin(j*phi);
        cur[j]  = new Point(x,y,z);
      }
      
      //make sides from prev ring to cur ring
      for(int j=0; j< lon; j++){
        int fj = (j)%lon;
        int fi = (j+1)%lon;
        Point[] side = new Point[4];//really is always 4
        side[3] = prev[fj];
        side[2] = cur[fj];
        side[1] = cur[fi];
        side[0] = prev[fi];
        this.addFace(side);
      }
      prev = cur;
    }
    
    //make sides from last ring to first ring
	/*for(int j=0; j< lon; j++){
	  int fj = (j)%lon;
	  int fi = (j+1)%lon;
	  Point[] side = new Point[4];//really is always 4
	  side[0] = prev[fj];
	  side[1] = first[fj];
	  side[2] = first[fi];
	  side[3] = prev[fi];
	  this.addFace(side);
	}*/
    
    
  }
}


class Sphere extends Surface{
  public Sphere(int lat, int lon){
    v_list = new ArrayList<Point>();
    f_list = new LinkedList<int[]>();
    matrix = new Matrix4x4();
    matrix.identity();//start with identity
    color = Color.WHITE;//just in case color is not later defined
    make(lat, lon);
  }
  
  private void make(int lat, int lon){
    //make top and bottom
    double theta = 2*Math.PI/lon;//change in angle
    double x = 1;
    double y = 0;
    double z = Math.cos(Math.PI/(lat+1));
    double w = Math.sin(Math.PI/(lat+1));
    //make all of the sides
    Point[] prev = new Point[lon];
    for(int j = 0; j<prev.length; j++)
      prev[j] = new Point(0,0,1);
    
    Point[] cur;
    double phi = Math.PI/(lat+1);
    for(int i = 1; i<= (lat+1); i++){
      //make the cur ring
      cur = new Point[lon];
      z = Math.cos(i*Math.PI/(lat+1));
      w = Math.sin(i*Math.PI/(lat+1));
      for(int j = 0; j<lon; j++){
        x = w*Math.cos(j*theta);
        y = w*Math.sin(j*theta);
        cur[j]  = new Point(x,y,z);
      }
      
      //make sides from prev ring to cur ring
      for(int j=0; j< lon; j++){
        int fj = (j)%lon;
        int fi = (j+1)%lon;
        Point[] side = new Point[4];//really is always 4
        side[0] = prev[fj];
        side[1] = cur[fj];
        side[2] = cur[fi];
        side[3] = prev[fi];
        this.addFace(side);
      }
      prev = cur;
    }
    
    
    
    
  }
}


class Cylinder extends Surface{
  public Cylinder(int n){
    v_list = new ArrayList<Point>();
    f_list = new LinkedList<int[]>();
    matrix = new Matrix4x4();
    matrix.identity();//start with identity
    color = Color.WHITE;//just in case color is not later defined
    make(n);
  }
  
  private void make(int n){
    Point[] front = new Point[n];
    Point[] back = new Point[n];
    double theta = 2*Math.PI/n;//change in angle
    double x = 1;
    double y = 0;
    double z = 1;
    //make front and back
    for(int j=n-1;j>=0; j--){
      x = Math.cos(j*theta);
      y = -Math.sin(j*theta);
      front[j]  = new Point(x,y,z);
      //be sure this is going the other way around!!
      x = Math.cos(j*theta);
      y = Math.sin(j*theta);
      back[j]  = new Point(x,y,-z);
    }
    this.addFace(front);
    this.addFace(back);
    
    //make side panels all around
    for(int j=0; j< n; j++){
      int fj = (j)%n;
      int bj = (n-j)%n;
      int bi = (n-j-1)%n;
      int fi = (j+1)%n;
      
      Point[] side = new Point[4];//really is always 4
      side[0] = front[fj];
      side[1] = back[bj];
      side[2] = back[bi];
      side[3] = front[fi];
      this.addFace(side);
    }
    
  }
}

/**
 * Cube rimitive (though something is funky with the painter's algorithm
 * when drawing visible sides...normal? not CCW?
 */
class Cube extends Surface{
  public Cube(){
    v_list = new ArrayList<Point>();
    f_list = new LinkedList<int[]>();
    matrix = new Matrix4x4();
    matrix.identity();//start with identity
    color = Color.WHITE;//just in case color is not later defined
    make();
  }
  
  private void make(){
    Point[] front = new Point[4];
    Point[] back = new Point[4];
    Point[] top = new Point[4];
    Point[] bottom = new Point[4];
    Point[] left = new Point[4];
    Point[] right = new Point[4];
    
    front[3] = new Point(1,-1,1);
    front[2] = new Point(1,1,1);
    front[1] = new Point(-1,1,1);
    front[0] = new Point(-1,-1,1);
    
    back[0] = new Point(1,-1,-1);
    back[1] = new Point(1,1,-1);
    back[2] = new Point(-1,1,-1);
    back[3] = new Point(-1,-1,-1);
    
    left[3] = new Point(-1,1,1);
    left[2] = new Point(-1,1,-1);
    left[1] = new Point(-1,-1,-1);
    left[0] = new Point(-1,-1,1);
    
    right[0] = new Point(1,1,1);
    right[1] = new Point(1,1,-1);
    right[2] = new Point(1,-1,-1);
    right[3] = new Point(1,-1,1);
    
    top[0] = new Point(-1,1,-1);
    top[1] = new Point(1,1,-1);
    top[2] = new Point(1,1,1);
    top[3] = new Point(-1,1,1);
    
    bottom[3] = new Point(-1,-1,-1);
    bottom[2] = new Point(1,-1,-1);
    bottom[1] = new Point(1,-1,1);
    bottom[0] = new Point(-1,-1,1);
    
    
    //make front and back
    this.addFace(front);
    this.addFace(back);
    this.addFace(left);
    this.addFace(right);
    this.addFace(top);
    this.addFace(bottom);
    
  }
}




class ZPoly implements Comparable{
  Double z;
  int xpoints[],ypoints[];
  Color color;
  int len;
  boolean facing;
  public ZPoly(int[] x, int[] y, double n, Color c, boolean f){
    xpoints = x;
    ypoints = y;
    len = xpoints.length;
    z = n;
    color = c;
    facing = f;
  }
  
  public int compareTo(Object o){
    return this.z.compareTo(( (ZPoly)o).z);
  }

}


/**
 * Point objects...could also hold normals..no reason for this to exist
 * besides convenience...might get too slow...
 */
class Point{
  double x, y, z;
  
  public Point(double i, double j, double k){
    x = i;
    y = j;
    z = k;
  }
  public boolean equals(Point p){
     return (this.x==p.x)&&(this.y==p.y)&&(this.z==p.z);
  }

  public String toString(){
    return "<"+x+","+y+","+z+">";
  }
  
  /**
   * return z value of surface normal
   * Uses first 3 values
   */
  public static double normalZ(Point[] f){
    Point p1 = f[0];
    Point p2 = f[1];
    Point p3 = f[2];
    
    //if rect then I can do a little err check and get a true triangle
    if (f.length>3){
	  if(p1.equals(p2)){
		p1 = f[3];
      }else if(p1.equals(p3)){
		p1 = f[3];
      }else if(p2.equals(p3)){
		p3 = f[3];
      }
    }
    
    
    Point a = new Point(p2.x-p1.x,p2.y-p1.y,p2.z-p1.z);
    Point b = new Point(p3.x-p1.x,p3.y-p1.y,p3.z-p1.z);
    
    /* Surface normal is (p2-p1)x(p3-p1)
     * of which we only need the z-coordinate
     * so if n = axb then the z-coordinate is 
     * a1b2 - a2b1
     * which in this case is
     * (p21 - p11)(p32-p12)-(p32-p12)(p31-p11)
     */
     
//     double z =  (p2.x - p1.x)*(p3.y-p1.y)-(p3.y-p1.y)*(p3.x-p1.x);

//let's try that again
//(a2b3 - a3b2, a3b1 - a1b3, a1b2 - a2b1)
     Point cross = new Point(a.y*b.z - a.z*b.y,
       a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);

     return cross.z;
     
  }
  
  public double norm(){
    return Math.sqrt(x*x+y*y+z*z);
  }
  
}


public class HW06 extends BufferedApplet{

  final double INIT_ZOOM = 5.0;
  final double INIT_ROTX =0.45;
  final double INIT_ROTY =0.0;
  final double INIT_ROTZ =0.0;
  boolean debug = true;
  boolean label = false;
  boolean wired = true;//show edges...
  boolean trans = true;//transparent? no faces...
  
  boolean movie = false;
  double timeMovie;
  
  int scoreSeq = 0;//either score seq opr laying a round
  boolean rightWon = false;
  boolean leftWon = false;
  int leftWins = 0;
  int rightWins = 0;
  
  int w, h;//bounds of applet
  boolean uninit = true;//start applet as uninitiallezedasd 
  double vec1[] = new double[3];//temporary 3D vector
  double vec2[] = new double[3];//temporary 3D vector
  Matrix4x4 pre;
  double foc  = -1.0;//focal length of pinhole camera
  double scale;//camera scale
  double zoom = INIT_ZOOM;//camera
  double rotX = INIT_ROTX;//camera 
  double rotY = 0;//camera 
  double rotZ = 0;//camera 
  Matrix4x4 zoo;
  final double dampening = 0.99;
  final double START_X_OFF = 0.75;
  final double headWidth = 0.1;
  double res = 0.16;  

  double time0;
  double cur_time;
  Boxer boxer_r;
  Boxer boxer_l;
  Table base;

  Point grav = new Point(0,-.4,0);//gravity force
  
  PriorityQueue<ZPoly> faceQ;//queue of faces
  
  public void initVals(){
    scale = 1;
    connect = false;
    time0 = System.currentTimeMillis() / 1000.0;
    
    //gravity
    
    
    boxer_r = new Boxer( (int)(res*100), headWidth);
    boxer_r.setColor(Color.CYAN);
    boxer_r.x = START_X_OFF;
    
    boxer_l = new Boxer( (int)(res*100), headWidth);
    boxer_l.setColor(Color.RED);
    boxer_l.x = -START_X_OFF;
    boxer_l.rotY = Math.PI;
    
    //construct characters
    base = new Table(1.0,0.1,1.0,2.0);
    base.setColor(Color.WHITE);
    //base.rotY -= Math.PI/4;
    base.y = -base.sy*base.radius;

  }
  
  public void render(Graphics g) {
    if (true||w == 0) {//true here for resizing appletviewer
      w = bounds().width;
      h = bounds().height;
      if(uninit){
        initVals();
        uninit = false;
      }
    }
    
    if(movie)
      updateMovie();
    
    
    if(scoreSeq>0){                                                                                                                                                                      
	  int u,v;
	  if(rightWon){
		u = 0;                                                                                                                                                                              
		v = w/2;                                                                                                                                                                            
	  }else{                                                                                                                                                                                
		u = w/2;                                                                                                                                                                            
		v = w;                                                                                                                                                                              
	  }                                                                                                                                                                                     
	  g.setColor(Color.MAGENTA);                                                                                                                                                               
	  g.fillRect(0, 0, w, h);                                                                                                                                                         
	  if(scoreSeq%2==0){                                                                                                                                                                    
	   g.setColor(Color.WHITE);                                                                                                                                                             
	   g.fillRect(0, 0, w, h);                                                                                                                                                          
	  }                                                                                                                                                                                     
																																															
	  g.setColor(Color.WHITE);                                                                                                                                                              
	  g.drawString("SCORE: "+rightWins,100,100);                                                                                                                                            
	  g.drawString("SCORE: "+leftWins,w-150,100);                                                                                                                                           
		                                                                                                                                                                                   
	 scoreSeq--;                                                                                                                                                                            
	 if(scoreSeq==0) initVals();                                                                                                                                                            
	}else{   
      checkForWin();  
	  //clear the screen
	  g.setColor(Color.MAGENTA );                                                                                                                                                               
	  g.fillRect(0, 0, w, h);                                                                                                                                                         
    }
	double past_time = cur_time;
	cur_time = System.currentTimeMillis() / 1000.0;
	double cur_step = Math.min(1,cur_time - past_time);
	
	
	
	//Make pre camera matrix
	zoo = new Matrix4x4();
	zoo.makeTranslation(0,0,-zoom);
	zoo.rotateX(rotX);
	zoo.rotateY(rotY);
	zoo.rotateZ(rotZ);
      
	
	//update all the physics going on
	update(cur_step);
	
	base.updateMatrices();
    boxer_r.updateMatrices();	
	boxer_l.updateMatrices();	
	
	//draw characters here
        PriorityQueue<Character> charQ = new PriorityQueue<Character>();
        setCamZ(base);
        drawCharacter(g, base);
        //charQ.add(base);
	
    setCamZ(boxer_r);
	charQ.add(boxer_r);
	
	setCamZ(boxer_l);
	charQ.add(boxer_l);
	
	while(!charQ.isEmpty()){
	 Character ch = charQ.remove();
	 drawCharacter(g, ch);
	}
  
    
    
    animating = true; //set animating to true;
  }
  
  
  
  double timing[] = {0.0,  0.5, 1.0, 2.7,  2.8, 2.9, 3.1, 3.2, 4.0, 8.0};
  double zoom_a[] = {INIT_ZOOM, 2.0, 1.75, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, INIT_ZOOM};
  
  double zoomX[] = {INIT_ROTX, 1.0, .7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, INIT_ROTX};
  double zoomY[] = {INIT_ROTY, -.4, -1.0, -1.1, -1.3, -1.6, -1.9, -2.5, -3.0, -6.28};
  double zoomZ[] = {INIT_ROTZ, INIT_ROTZ/2, INIT_ROTZ/2, INIT_ROTZ/2, INIT_ROTZ/2, 
    INIT_ROTZ/2, INIT_ROTZ/2, INIT_ROTZ/2, INIT_ROTZ/2, 2*INIT_ROTZ};
  
  double v_l[] =    {0.0, 0.1,  0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0};
  
  double top_rot[]= {0.05,0.05,0.05,0.15, -0.25,-0.25,0.05,0.05,0.05,0.05};
  double bot_rot[]= {-1.5,-1.5,-1.5,-1.6, -1.25,-1.375,-1.5,-1.5,-1.5,-1.5};
  double top_twist[]={0.05,0.05,0.05,0.15,0.15,0.15,0.05,0.05,0.05,0.05};
 
 boolean connect = false;
  public void updateMovie(){
    if(boxer_r.leg_velo==0) boxer_r.leg_velo = 0.1;
    if(boxer_r.leg_velo>-1) boxer_r.leg_velo=boxer_r.leg_velo*1.1;
    
    boxer_r.inMovie = true;
    double t = System.currentTimeMillis()/1000.0 - timeMovie;
    //find index
    int i;
    for(i=0;i<timing.length-1;i++){
      
      if((timing[i]<=t)&&(t<timing[i+1])){
        break;
      }
    }
    if(i>=timing.length-1){
      boxer_r.inMovie = false;
      movie = false;
      return;
    }
    
    if(t>3.2 && !connect){
      connect = true;
      boxer_l.u = 3.0;
      boxer_l.addToVelocity(-1*0.4*Math.cos(boxer_r.rotY),0.01,0.4*Math.sin(boxer_r.rotY));
      boxer_r.rotY = (Math.PI/2);
      boxer_r.velocity = new Point(0,0,0);
      boxer_r.addToVelocity(-1*0.1*Math.cos(boxer_r.rotY),0,0.1*Math.sin(boxer_r.rotY));
    }
    //compute the fraction of time interval
    double fraction = (t - timing[i]) / (timing[i+1] - timing[i]);
    zoom = Boxer.lerp(fraction, zoom_a[i], zoom_a[i+1]);
    rotX = Boxer.lerp(fraction, zoomX[i], zoomX[i+1]);
    rotY = Boxer.lerp(fraction, zoomY[i], zoomY[i+1]);
    rotZ = Boxer.lerp(fraction, zoomZ[i], zoomZ[i+1]);

    double v = Boxer.lerp(fraction, v_l[i], v_l[i+1]);
    boxer_r.addToVelocity(-1*v*Math.cos(boxer_r.rotY),0,v*Math.sin(boxer_r.rotY));
    boxer_r.top_rot_l = Math.PI*Boxer.lerp(fraction, top_rot[i], top_rot[i+1]);
    boxer_r.bot_rot_l = Math.PI*Boxer.lerp(fraction, bot_rot[i], bot_rot[i+1]);
    boxer_r.top_twist_l = -1*Math.PI*Boxer.lerp(fraction, top_twist[i], top_twist[i+1]);
  }
  
  public void setCamZ(Character c){
    double dummy[] = new double[3];
    zoo.transform(c.x,c.y,c.z,dummy);
    c.cam_z = dummy[2];
  }
  

  
  public void checkForWin(){
    if(boxer_r.y<boxer_r.radius*-10){
       scoreSeq = 10;
       leftWon = true;
       leftWins++;
    }
    if(boxer_l.y<boxer_l.radius*-10){
       scoreSeq = 10;
       rightWon = true;
       rightWins++;
    }
  }
  
    public boolean collide(double fist[], Point fist_v, double fist_rad,
    double head[], Point head_v, double head_rad){
     if(null==fist) return false;    
     if(null==head) return false;
     
     Point b1 = new Point(fist[0],fist[1],fist[2]);
     Point b2 = new Point(head[0],head[1],head[2]);
     
     
     Point diff = new Point(b1.x-b2.x,
       b1.y-b2.y, b1.z-b2.z);
     double d = diff.norm();
     if( d < (fist_rad+ head_rad) ){

       Point v1 = fist_v;
       Point v2 = head_v;
// Between the comment bars is modified code from
// http://www.plasmaphysics.org.uk/programs/coll3d_cpp.htm
//////////////////////////////////////////////////////////////////


       double distance =fist_rad+ head_rad;
       double x21=b2.x-b1.x;
       double y21=b2.y-b1.y;
       double z21=b2.z-b1.z;
       double m21=2.0/1.0;
//       System.out.println("m21: "+m21+" b2.mass: "+b2.mass+" b1.mass: "+b1.mass);
       double vx21=v2.x-v1.x;
       double vy21=v2.y-v1.y;
       double vz21=v2.z-v1.z;
             
//     **** calculate relative distance and relative speed ***
       double v=Math.sqrt(vx21*vx21 +vy21*vy21 +vz21*vz21);
       
//     **** return if distance between balls smaller than sum of radii ****
       if (d>distance){
       }else if (v==0){
       }else{
   //     **** shift coordinate system so that ball 1 is at the origin ***
         b2.x=x21;
         b2.y=y21;
         b2.z=z21;
  //     **** boost coordinate system so that ball 2 is resting ***
         v1.x=-vx21;
         v1.y=-vy21;
         v1.z=-vz21;
  //     **** find the polar coordinates of the location of ball 2 ***
         double theta2=Math.acos(b2.z/d);
         double phi2 = 0;
         if (b2.x==0 && b2.y==0) {
           phi2=0;
         } else {
           phi2=Math.atan2(b2.y,b2.x);
         }
         double st=Math.sin(theta2);
         double ct=Math.cos(theta2);
         double sp=Math.sin(phi2);
         double cp=Math.cos(phi2);
  
  //     **** express the velocity vector of ball 1 in a rotated coordinate
  //          system where ball 2 lies on the z-axis ******
         double v1xr=ct*cp*v1.x+ct*sp*v1.y-st*v1.z;
         double v1yr=cp*v1.y-sp*v1.x;
         double v1zr=st*cp*v1.x+st*sp*v1.y+ct*v1.z;
         double thetav=Math.acos(v1zr/v);
         
         double phiv = 0;
         if (v1xr==0 && v1yr==0) {
           phiv=0;
         } else {
           phiv=Math.atan2(v1yr,v1xr);
         }
  
                                  
  //     **** calculate the normalized impact parameter ***
         double dr=d*Math.sin(thetav)/distance;
  
         
  //     **** calculate impact angles if balls do collide ***
         double alpha=Math.asin(-dr);
         double beta=phiv;
         double sbeta=Math.sin(beta);
         double cbeta=Math.cos(beta);
          
         
  //     **** calculate time to collision ***
         double t=(d*Math.cos(thetav) -distance*Math.sqrt(1-dr*dr))/v;
  
       
  //     **** update positions and reverse the coordinate shift ***
         b2.x=b2.x+v2.x*t +b1.x;
         b2.y=b2.y+v2.y*t +b1.y;
         b2.z=b2.z+v2.z*t +b1.z;
         b1.x=(v1.x+v2.x)*t +b1.x;
         b1.y=(v1.y+v2.y)*t +b1.y;
         b1.z=(v1.z+v2.z)*t +b1.z;
          
   
         
  //  ***  update velocities ***
  
         double a=Math.tan(thetav+alpha);
  
//         double dv2z=1*(v1zr+a*(cbeta*v1xr+sbeta*v1yr))/((1+a*a)*(1+m21));
         double dv2z=1*(v1zr+a*(cbeta*v1xr+sbeta*v1yr))/((1+a*a)*(1+m21));
         
         double v2zr=dv2z;
         double v2xr=a*cbeta*dv2z;
         double v2yr=a*sbeta*dv2z;
         v1zr=v1zr-m21*v2zr;
         v1xr=v1xr-m21*v2xr;
         v1yr=v1yr-m21*v2yr;
  
         
  //     **** rotate the velocity vectors back and add the initial velocity
  //           vector of ball 2 to retrieve the original coordinate system ****
                       
         v1.x=ct*cp*v1xr-sp*v1yr+st*cp*v1zr +v2.x;
         v1.y=ct*sp*v1xr+cp*v1yr+st*sp*v1zr +v2.y;
         v1.z=ct*v1zr-st*v1xr               +v2.z;
         v2.x=ct*cp*v2xr-sp*v2yr+st*cp*v2zr +v2.x;
         v2.y=ct*sp*v2xr+cp*v2yr+st*sp*v2zr +v2.y;
         v2.z=ct*v2zr-st*v2xr               +v2.z;
         return true;
       }
       
            
     }
    return false;
  }
  
  
  /**update all the physics*/
  public void update(double dt){
    boxer_r.applyVelocity(dt);
    boxer_l.applyVelocity(dt);
    
    
    boolean leftHit = collide(boxer_r.old_left, boxer_r.left_v, boxer_r.fistWidth,
      new double[] {boxer_l.x,boxer_l.radius+boxer_l.y,boxer_l.z}, boxer_l.velocity, boxer_l.headWidth);
    
    boolean rightHit =     collide(boxer_l.old_left, boxer_l.left_v, boxer_l.fistWidth,
      new double[] {boxer_r.x,boxer_r.radius+boxer_r.y,boxer_r.z}, boxer_r.velocity, boxer_r.headWidth);
    
    if(rightHit)
      boxer_r.u = 1.5;
    if(leftHit)
      boxer_l.u = 1.5;
    
    collide(boxer_l.old_left, boxer_l.left_v, boxer_l.fistWidth,
      boxer_r.old_left, boxer_r.left_v, boxer_r.fistWidth);
    
    updateBoxer(boxer_r, dt);
    updateBoxer(boxer_l, dt);
  }
  
  public void updateBoxer(Boxer b, double dt){
    double dummy[] = new double[3];
    b.fist_l.matrix.transform(b.x,b.y,b.z,dummy);
    b.setLeftV(dummy,dt);
    
    updateChar(b, dt);
  }
  
  public void updateChar(Character b, double dt){
    if(  (b.y>b.radius) ||
      ((-base.sx*base.radius> b.x || b.x > base.sx*base.radius) ||
        (-base.sz*base.radius> b.z || b.z > base.sz*base.radius))
      ){
      b.addToVelocity(dt*grav.x,dt*grav.y,dt*grav.z);
    }else{
      b.y = b.radius;

      b.multiplyToVelocity(1,-1,1);
      b.multiplyToVelocity(dampening,dampening,dampening);
      if(b.y>0) 
        b.addToVelocity(dt*grav.x,dt*grav.y,dt*grav.z);
    }
//    System.out.println("p = <"+cannon_r.x+","+cannon_r.y+","+cannon_r.z+">");
//    System.out.println("v = <"+cannon_r.velocity.x+","+cannon_r.velocity.y+","+cannon_r.velocity.z+">");
//    System.out.println("dt = "+dt);
  }
  
  /**iterate over the surfaces in machine, drawing each one**/
  public void drawCharacter(Graphics g, Character c){
    faceQ = new PriorityQueue<ZPoly>();
    for(Surface s: c.s_list)
        placeSurface(g, s);
    Color old = g.getColor();
    while(!faceQ.isEmpty()){
      ZPoly p = faceQ.remove();
      //actually draw the polygon
      g.setColor(p.color);
      if(!trans&&p.facing) g.fillPolygon(p.xpoints,p.ypoints,p.len);    
      g.setColor(Color.BLACK);
      if((wired&&p.facing)||
            (wired&&trans)) g.drawPolygon(p.xpoints,p.ypoints,p.len);
    }
    g.setColor(old);
  }
  
  
  /**Method to draw a surface in the 3D space*/
  public void placeSurface(Graphics g, Surface s){
	//get matrix for surface
	Matrix4x4 mat = s.matrix;
 
	//sort the faces based on proximity to camera
	for(int[] face: s.f_list){
	   Point[] f = new Point[face.length];
	   for(int j =0; j<face.length;j++){
		 f[j] = s.v_list.get(face[j]);
	   }
	   drawFace(g, f, s.matrix, s.color);
	 }
  } 	 
  
 
  
  /**Method to draw a face in the 3D space*/
  public void drawFace(Graphics g, Point[] face, Matrix4x4 mat, Color c){
    boolean visible = true;//assume face is in view
    int len = face.length;
    Point[] transformed = new Point[face.length];
    int[] xpoints = new int[len];
    int[] ypoints = new int[len];
    double max_z = Double.MIN_VALUE;
    for(int j =0; j<face.length; j++){
      Point p = face[j];
      Matrix4x4 m = mat.getCopy();

      m.preMultiply(zoo);
      
      m.transform(p.x, p.y, p.z, vec1);

      visible  = visible && (vec1[2] < -foc);
      String text = "("+(int)vec1[0]+","+(int)vec1[1]+","+(int)vec1[2]+")";
      
      max_z = Math.max(max_z,vec1[2]);
      
      vec1[0] = foc * vec1[0] / (vec1[2]+foc) ;
      vec1[1] = foc * vec1[1] / (vec1[2] +foc) ;
      vec1[2] = 1 /(vec1[2] +foc) ;


//      camera.transform(vec1[0],vec1[1],vec1[2], vec2);    
      
      vec2[0] = (int)(w/2 + h * vec1[0]);
      vec2[1] = (int)(2*h/3 - h * vec1[1]);
      vec2[2] = (int)vec2[2];

      xpoints[j] = (int)vec2[0];
      ypoints[j] = (int)vec2[1];
      
      transformed[j] = new Point(vec2[0],vec2[1],vec2[2]);
      
      g.setColor(Color.BLACK); 
      if (label) g.drawString(""+max_z,(int)vec2[0],(int)vec2[1]);
      
    }
    boolean facing = ( Point.normalZ(transformed) > 0);
    
    
    if(visible){
      //actually draw the polygon
      if(!facing) c = Color.BLACK;
      faceQ.add(new ZPoly(xpoints, ypoints, max_z, c, facing));
    }
  }
  
  
  public boolean mouseUp(Event e, int x, int y) {
    damage = true;
    return true;
  }
  
  public boolean mouseDrag(Event e, int x, int y) {
//    mach.x = (((double) x)-w/2)/scale;
//    mach.y = (((double) y)-h/2)/scale;
    damage = true;
    return true;
  }  

  public boolean keyDown(Event e, int key) {
    if(!movie){
     switch (key) {
      case '0':
      case Event.ENTER:
        boxer_r.chargeLeft();
        break;
      case ' ':
        boxer_l.chargeLeft();
        break;
      default:
        break;
    }
    }
    return true;
  }
    
  
  public boolean keyUp(Event e, int key) {
    if(!movie){
          switch (key) {
    
      //RIGHT CONTROLLERS        
      case Event.LEFT:
      case '4':
        boxer_r.rotY += Math.PI/16;
        break;
      case Event.RIGHT:
      case '6':
        boxer_r.rotY -= Math.PI/16;
           break;
      case Event.UP:
      case '8':
        boxer_r.addToVelocity(-.2*Math.cos(boxer_r.rotY),0,.2*Math.sin(boxer_r.rotY));
           break;
      case Event.DOWN:
      case '5':
      case '2':
        boxer_r.addToVelocity(.2*Math.cos(boxer_r.rotY),0,-.2*Math.sin(boxer_r.rotY));
           break;
      case '0':
      case Event.ENTER:
        boxer_r.punchLeft();
        break;
        //cannon_r.fireBall();
      //LEFT CONTROLLERS        
      case 'a':
        boxer_l.rotY += Math.PI/16;
        break;
      case 'd':
        boxer_l.rotY += Math.PI/16;
           break;
      case 'w':
        boxer_l.addToVelocity(-.2*Math.cos(boxer_l.rotY),0,.2*Math.sin(boxer_l.rotY));
           break;
      case 's':
        boxer_l.addToVelocity(.2*Math.cos(boxer_l.rotY),0,-.2*Math.sin(boxer_l.rotY));
           break;
      case ' ':
        boxer_l.punchLeft();
        break;  
      case 'g':
        grav = new Point(grav.x/2, grav.y/2, grav.z/2);
        break;
      case 'G':
        grav = new Point(grav.x*2, grav.y*2, grav.z*2);
        break;  
      case Event.END:
        break;
      case Event.PGDN:
        zoom += .5;
        break;
      case Event.PGUP:
        zoom -= 1.0;
           break;
      case '<':
        rotX += Math.PI/16;
           break;
      case ',':
        rotX -= Math.PI/16;
           break;
      case '>':
        rotY += Math.PI/16;
           break;
      case '.':
        rotY -= Math.PI/16;
           break;
      case '?':
        rotZ += Math.PI/16;
           break;
      case '/':
        rotZ -= Math.PI/16;
           break;
//GAME CONTROLLERS        
      case 'r':
      case 'R':
        leftWins = 0;
        rightWins = 0;
        zoom = INIT_ZOOM;
        rotX = INIT_ROTX;
        rotY = 0;
        rotZ = 0;
        initVals();
        break;   
      }
    }
  
      if(movie)
        movie = key != 'M' || key != 'm' || key != 'R' || key != 'r' ;
      switch (key) {
//GAME CONTROLLERS        
      case 'r':
      case 'R':
        leftWins = 0;
        rightWins = 0;
        zoom = INIT_ZOOM;
        rotX = INIT_ROTX;
        rotY = 0;
        rotZ = 0;
        initVals();
        break;
//DISPLAY CONTROLLERS        
      case '+':
      case '=':
        res = res*1.5;
        if(res>0.5) res=0.5;
        initVals();
        break;
      case '_':
      case '-':
        res = res/1.5;
        if(res<0.02) res=0.02    ;
        initVals();
        break;
      case 'l':
      case 'L':
        label  = !label;
        break;
      case 'e':
      case 'E':
        wired  = !wired;
        break;
      case 't':
      case 'T':
        trans  = !trans;
        break;
      case 'm':
      case 'M':
        leftWins = 0;
        rightWins = 0;
        zoom = INIT_ZOOM;
        rotX = INIT_ROTX;
        rotY = 0;
        rotZ = 0;
        initVals();
        movie  = true;
        timeMovie = System.currentTimeMillis()/1000.0;
        break;

    
  }
    damage = true;
    return true;
  
  }
  
}