import java.awt.*;

class Ball{
  double x,y,z;//location of character 
  double rotZ, rotX, rotY;
  double sx = 1.;
  double sy = 1.;
  double sz = 1.;
  double radius = 1;
  Matrix4x4 matrix;
  Geometry[] object;
  double vx,vy,vz;
  double fx, fy, fz;
  double mass = 1;
  boolean floor = true;
  
  double ambientColor[] = { 0.3, 0.3, 0.3  };
  double diffuseColor[] = { 0.7, 0.7, 0.7 };
  double specularColor[] = {.9	,.9,.9};
  double specularPower = 8.0;
  
  public Ball(int m, int n){
    init(m, n);
  }
  
  public void init(int m, int n){
    //init matrix for machine as whole
    matrix = new Matrix4x4();
    matrix.identity();
    
    object = new Geometry[1];
    //make individual parts here
    object[0] = new Geometry();
    object[0].globe(m,n);
  }
  
  /**go through all of the geometry objects and update matrices*/
  public void updateMatrices(){

    this.matrix.identity();
    this.matrix.translate(x,y,z);
    this.matrix.rotateZ(rotZ);
    this.matrix.rotateX(rotX);
    this.matrix.rotateY(rotY);
    this.matrix.scale(sx ,sy, sz);
    
    //update matrices of all surfaces
    for(Geometry g: object)
      g.matrix = matrix.getCopy();
  }
  
  public void applyForce(double dt){
    vx += dt*fx;
    vy += dt*fy;
    vz += dt*fz;
  }
  
  public void resetForce(){
    fx = 0;
    fy = 0;
    fz = 0;
  }
  
  public void addToForce(double x, double y, double z){
    fx += x;
    fy += y;
    fz += z;
  }
  
  public void applyVelocity(double dt){
    x += dt*vx;
    y += dt*vy;
    z += dt*vz;
    
  }
  
  public void addToVelocity(double x, double y, double z){
    vx += x;
    vy += y;
    vz += z;
  }
  
  public void multiplyToVelocity(double x, double y, double z){
    vx *= x;
    vy *= y;
    vz *= z;
  }
}


public class HW09 extends MISApplet{
  // MATERIAL PROPERTIES
  double ambientColor[] = { 0.02, 0.025, 0.0  };
  double diffuseColor[] = { 0.7, 0.75, 0.0 };
  double specularColor[] = {.7,.8,.0};
  double specularPower = 8.0;
  // LIGHTING PROPERTIES
  double lightDirections[][] = { {1,1,1}, {1,0,0} ,{-1,0,0}   };
  double lightColors[][]     = { {1.0,1.0,1.0}, {0.9,0.0,0.45}, {0.45,0.0,0.9} };

  double grav[] = new double[] {0,-.4,0};//gravity force


  Matrix4x4 camera;
  Matrix4x4 cameraInverse;
  double camZ = 10.0;
  double rotY = 0.0;
  double rotX = 0.0;
  double rotZ = 0.0;
  double foc = -1.0;//focal length of pinhole camera

  double vec1[] = new double[3];//temporary 3D vector
  double vec2[] = new double[3];//temporary 3D vector
  double[][][] clippedTriangles;
  double[][][] triangles;
  boolean wired = false;
  Ball ball;
  Ball left;
  Ball right;

  boolean goFloor = true;
  int rand[];
  double zBuffer[];//the z buffer, duh

  boolean showInside = true;

  double oldTime;
  double dt;
  
  /**  Do this once per run */
  public void initialize() {
    /** normalize the all of the ligh directions to unit length*/
    for (int l = 0 ; l < lightDirections.length ; l++)
      normalize(lightDirections[l]);
    /** instanciate a z_buffer to the size of the frame */
    zBuffer = new double[pix.length];
    /** initialize the camera */
    camera = new Matrix4x4();
    camera.identity();
    /** initialize all characters and their geometry*/
    ball = new Ball(20,10);
    ball.z = -11;
    ball.y = 5;
    left = new Ball(20,10);
    left.z = -15;
    left.y = ball.radius;
    left.x = -10;
    right = new Ball(20,10);
    right.z = -15;
    right.y = ball.radius;
    right.x = 10;
    
    
    left.ambientColor  = new double[]  { 0.0, 0.0, 0.8 };
    left.diffuseColor  = new double[] { 0.3, 0.3, 0.9 };
    left.specularColor = new double[] {1,1,1};
    left.specularPower = 10.0;
    
    right.ambientColor  = new double[]  { 0.8, 0.0, 0.0 };
    right.diffuseColor  =new double[] { 0.9, 0.3, 0.3 };
    right.specularColor =new double[] {1,1,1};
    right.specularPower = 8.0;

   
    rand =  new int[W];
    for(int i=0;i<W;i++){
        rand[i] = (int)(200+Math.random()*100);
    }
    
    
//    ball = new Geometry();
//    ball.cylinder(20);
//    ball.globe(20,10);
 //   ball.tube(20);
//    ball.showInside = true;
//    ball.cube();
  }
  
  /** Do these before compute the frame */
  public void initFrame(double time) {
    /** remember to normalize light directions if you mess with vectors*/
    //lightColors[0][1] = 0.5+Math.cos(1.5*time)*0.5;
    //lightColors[0][1] = 0.5+Math.cos(1.0*time)*0.5;
    //lightColors[0][2] = 0.5+Math.cos(2.0*time)*0.5;
    lightDirections[0][0] = Math.cos(time) *Math.cos(time) * .5;
    lightDirections[0][1] = Math.sin(time)*Math.cos(time) * .5;
    lightDirections[0][2] = Math.sin(time) * .5;
    normalize(lightDirections[0]);
    //mess with camera
    //camera.translate(0,0,10*Math.sin(time));
    camera.identity();
    camera.rotateX(rotX);
    camera.translate(0,0,camZ);
  }
  
  /** Overriden method that must define frame in doube pix[] array */
  public void computeImage(double time){
	dt = Math.min(1,time - oldTime);
    oldTime = time;
    initFrame(time);
    //clear the pix array (paint the backdrop)
    drawBackdrop(time);
    //clear the z_buffer
    zClear(zBuffer);
    
  	//update animation matrix of geometry object
    updatePhysics();
    
    ball.rotZ += Math.PI/16;
    ball.rotX += Math.PI/15;
    ball.rotY += Math.PI/14;
    left.rotZ += Math.PI/13;
    left.rotX += Math.PI/12;
    left.rotY += Math.PI/11;
    right.rotZ += Math.PI/10;
    right.rotX += Math.PI/9;
    right.rotY += Math.PI/8;
  	
	ball.updateMatrices();
	left.updateMatrices();
	right.updateMatrices();
	//g.matrix = new Matrix4x4();
	//g.matrix.identity();
	//g.matrix.translate(0,0,-10);
    
    //perpare C^-1
    cameraInverse = new Matrix4x4();
    Matrix4x4.invert(camera.getCopy().data, cameraInverse.data);    
    //loop over all Geometry objects
    writeCharacter(ball);
    writeCharacter(left);
    writeCharacter(right);
  }
  
  private void writeCharacter(Ball b){
    
	ambientColor = b.ambientColor;
	diffuseColor = b.diffuseColor;
	specularColor = b.specularColor;
	specularPower = b.specularPower;
    for(Geometry obj : b.object)
      writeObject(obj);
  }
  
  private void updatePhysics(){
  
    if(!goFloor) ball.floor = false;
    if(goFloor && ball.y>ball.radius) ball.floor = true;
    
  	ball.applyForce(dt);
  	ball.applyVelocity(dt);
  	ball.resetForce();
  	if(ball.floor) updateChar(ball);
    applySun(left);
    applySun(right);
  }
 
  private void applySun(Ball sun){
  	double r[] = new double[] {ball.x-sun.x, ball.y-sun.y,ball.z-sun.z};
    double g = -norm(r);
    normalize(r);
    ball.addToForce(g*0.5*sun.mass*r[0], g*0.5*sun.mass*r[1],g*0.5*sun.mass*r[2]);
  }
  
  public void updateChar(Ball b){
    if(b.y>b.radius){
		b.addToForce(grav[0],grav[1],grav[2]);
    }else{
      b.y = b.radius;

      b.multiplyToVelocity(1,-1,1);
      b.multiplyToVelocity(dampening,dampening,dampening);
      if(b.y>0) 
		b.addToForce(grav[0],grav[1],grav[2]);
    }
    
  }
  
  
  private void writeObject(Geometry g){

	//compute C^-1M
	Matrix4x4 cim = new Matrix4x4();
	Matrix4x4.copyData(g.matrix.data, cim.data);
	cim.preMultiply(cameraInverse);
	//compute [[C^-1M]^-1]T = Mn
	Matrix4x4 cimit = new Matrix4x4();
	Matrix4x4.invert(cim.getCopy().data, cimit.data);
	Matrix4x4.transpose(cimit.data, cimit.data);
	// Loop through all vertices of the geometry object
	g.transformed = new double[g.vertices.length][6];
	for(int i=0; i<g.vertices.length ;i++){
	  // Use matrix M to transform x,y,z to x',y',z'
	  cim.transform(g.vertices[i][0], g.vertices[i][1], g.vertices[i][2], vec1);
	  g.transformed[i][0] = vec1[0];
	  g.transformed[i][1] = vec1[1];
	  g.transformed[i][2] = vec1[2];
	  //Use matrix MN to transform nx,ny,nz to nx',ny',nz'
	  cimit.transform(g.vertices[i][3], g.vertices[i][4], g.vertices[i][5], vec1);
	  g.transformed[i][3] = vec1[0];
	  g.transformed[i][4] = vec1[1];
	  g.transformed[i][5] = vec1[2];
	  
//	  g.transformed[i][3] =g.vertices[i][3];
//	  g.transformed[i][4] = g.vertices[i][4];
//	  g.transformed[i][5] =g.vertices[i][5];
	  
	  
	  
	  //Use phong algorithm to transform nx',ny',nz' to r,g,b
	  phong(g.transformed[i]);
	}
	//loop over each face of the geometry object
	for(int i=0; i<g.faces.length;i++){
	  //if the face has more than 3 vertices, split into triangles
	  triangles = new double[g.faces[i].length-2][3][6];
	  for(int j=0; j<triangles.length;j++){
		//first corner is always the same
		// THIS WAS AN ENORMOUS MEMORY LEAK ?? was it?
		copy(g.transformed[g.faces[i][0]], triangles[j][0]);
		copy(g.transformed[g.faces[i][1+j]], triangles[j][1]);
		copy(g.transformed[g.faces[i][2+j]], triangles[j][2]);
		//triangles[j][0] = g.transformed[g.faces[i][0]];
		//triangles[j][1] = g.transformed[g.faces[i][1+j]];
		//triangles[j][2] = g.transformed[g.faces[i][2+j]];
		
	  }
	  //loop over triangles
	  for(int j_=0; j_<triangles.length;j_++){
		//clip triangle to the plane z = -\epsilon
		clippedTriangles = new double[1][3][6];
		clip(triangles[j_]);
		int j=0;
		while(j<clippedTriangles.length && null!=clippedTriangles[j]){
		  //clip triangle to the plane z = -\epsilon
		  if( (g.showInside || normalZ(clippedTriangles[j]) > 0 ) 
			//&&
			//clippedTriangles[j][0][2]<=0 &&
			//clippedTriangles[j][1][2]<=0 &&
			//clippedTriangles[j][2][2]<=0
			)
		  {
			//if clipped shape has four vetices split into two triangles
			for(int k=0; k< NUM_SIDES_ON_TRIANGLE; k++){
			  //System.out.println("Trans " + k+": "+ triangles[j][k][0] +", "+triangles[j][k][1]+", "+triangles[j][k][2]);
			  //apply perspective transform to each vertex. Now you have (px,py,pz,r,g,b)
			  clippedTriangles[j][k][0] = foc * clippedTriangles[j][k][0] / (clippedTriangles[j][k][2] +foc) ;
			  clippedTriangles[j][k][1] = foc * clippedTriangles[j][k][1] / (clippedTriangles[j][k][2] +foc) ;
			  clippedTriangles[j][k][2] = 1.0 / (clippedTriangles[j][k][2] +foc) ;
			  //triangles[j][k][2] = (triangles[j][k][2]) ;
			  //apply viewport transform to each vertex.  Now you have (i,j,pz,r,g,b).        
			  clippedTriangles[j][k][0] = (int)(W/2 + H * clippedTriangles[j][k][0]);
			  clippedTriangles[j][k][1] = (int)(H/2 - H * clippedTriangles[j][k][1]);
			  //triangle[j][k][2] = (int)triangle[j][k][2];
			  //System.out.println("PV " + k+": "+ triangles[j][k][0] +", "+triangles[j][k][1]);
			}
			//split triangle into two trapezoids, in preparation for scan-conversion
			double[][] topTrapezoid = new double[4][6];
			double[][] bottomTrapezoid = new double[4][6];
			split(clippedTriangles[j], topTrapezoid, bottomTrapezoid);
			writeTrapezoid(topTrapezoid);
			writeTrapezoid(bottomTrapezoid);
		  }
		  j++;
		}
	  }
	}
  }
  
  private void triangulate(double poly[][], double[][][] triangles){
	for(int j=0; j<triangles.length;j++){
	  //first corner is always the same
	  copy(poly[0], triangles[j][0]);
	  copy(poly[1+j], triangles[j][1]);
	  copy(poly[2+j], triangles[j][2]);
	  //triangles[j][0] = g.transformed[g.faces[i][0]];
	  //triangles[j][1] = g.transformed[g.faces[i][1+j]];
	  //triangles[j][2] = g.transformed[g.faces[i][2+j]];
	  
	}
  }
  
  private void clip(double[][] triangle){
    double poly[][] = new double[4][6];
    int j = 0;
    for(int i=0; i<triangle.length; i++){
      double a[] = triangle[i];
      double b[] = triangle[(i+1)%triangle.length];
      double dpa =dot(new double[] {0.,0.,-1.,1./1000.},a);
      double dpb =dot(new double[] {0.,0.,-1.,1./1000.},b);
      double sign = dpa*dpb;
      if(dpa>=0){
        copy(a,poly[j]);
        j++;
      }
      //Consider the edge A,B, where B is the next vertex around the polygon.
      if(sign<0){
        double t = (0. - dpa);
        t = t/(dpb-dpa);
        double c[] = new double[6];
        lerp(t,a,b,c);
        copy(c,poly[j]);
        j++;
      }
    }    
    if(j>=3){
	  clippedTriangles = new double[j-2][3][6];
	  triangulate(poly, clippedTriangles);
    }
  }
  
  
  private void writeTrapezoid(double[][] trapezoid){
    //System.out.println(trapezoid[0][2]);
    //System.out.println(trapezoid[1][2]);
    //System.out.println(trapezoid[2][2]);
    //System.out.println(trapezoid[3][2]);
    int TL[] = new int[] {
      (int) trapezoid[0][0] , 
      (int) trapezoid[0][1] , 
      (int)(trapezoid[0][2]*Integer.MAX_VALUE), 
      (int) trapezoid[0][3], 
      (int) trapezoid[0][4], 
      (int) trapezoid[0][5]};
    int TR[] = new int[] {
      (int) trapezoid[1][0] +1, 
      (int) trapezoid[1][1] -1, 
      (int)(trapezoid[1][2]*Integer.MAX_VALUE), 
      (int) trapezoid[1][3], 
      (int) trapezoid[1][4], 
      (int) trapezoid[1][5]};
    int BL[] = new int[] {
      (int) trapezoid[2][0] , 
      (int) trapezoid[2][1] , 
      (int)(trapezoid[2][2]*Integer.MAX_VALUE), 
      (int) trapezoid[2][3], 
      (int) trapezoid[2][4], 
      (int) trapezoid[2][5]};
    int BR[] = new int[] {
      (int) trapezoid[3][0] +1, 
      (int) trapezoid[3][1] +1, 
      (int)(trapezoid[3][2]*Integer.MAX_VALUE), 
      (int) trapezoid[3][3], 
      (int) trapezoid[3][4], 
      (int) trapezoid[3][5]};
      
    int L[] = new int[6];
    int R[] = new int[6];
    int pixel[] = new int[6]; //though we'll only use last 4
    
    // LOOP THRU SCAN LINES
    for (int j = TL[1]; j < BL[1]; j++){
      // COMPUTE FRACTION tj
      double tj = (j - TL[1]);
      tj = tj/ (BL[1] - TL[1]);
      lerp(tj, TL, BL, L);
      lerp(tj, TR, BR, R);
      for (int i = L[0] ; i < R[0] ; i++){    
        int index = xy2i((int)i,(int)j);
        // COMPUTE FRACTION ti
        double ti = (i - L[0]);
        ti = ti/ (R[0] - L[0]);
        lerp(ti, L, R, pixel);
        //fake clipping
        if(i>=0&&i<W&&j>=0&&j<H){
          if(pixel[2]< zBuffer[index]){
            zBuffer[index] = pixel[2];
            pix[index] = pack((int)pixel[3],(int)pixel[4],(int)pixel[5]);
          }else{
            //System.out.println(""+pixel[2]+"<"+zBuffer[index]);
          }
        }
      }
    }
  }
  
  private void copy(double src[], double dst[]){
    for(int i=0; i<src.length; i++)
      dst[i] = src[i];
  }
  
  /** put leftest possible value in a */
  private void zClear(double a[]){
    for(int i=0; i<a.length; i++) a[i] = Double.MAX_VALUE;
  }
  
  private void drawBackdrop(double time){
    for(int i=0;i<pix.length;i++){
      if(i%rand[i/W]!=0)
        pix[i] = pack(0,0,0);
      else
        pix[i] = pack(255,255,255);
        
    }
  }
  
  private void split(
    double[][] triangle, 
    double[][] topTrapezoid, 
    double[][] bottomTrapezoid)
  {
    //sort the vertices on py
    triangle = mergeSort(triangle);
    //lerp fraction
    double f  = (triangle[1][1] - triangle[0][1]);
    f= f/ (triangle[2][1] - triangle[0][1]);
    //middle vertex projected onto opposite side
    double proj[] = new double[6];
    lerp(f,triangle[0],triangle[2],proj);
    
    double left[] = triangle[1];
    double right[] = proj;
    // figure out if projection is on left of middle, if so swap
    if(proj[0]<triangle[1][0]){
      left = proj;
      right = triangle[1];
    }
    
    //TL and TR of top trapezoid are just top corner
    copy(triangle[0], topTrapezoid[0]);
    copy(triangle[0], topTrapezoid[1]);
    
    copy(left, topTrapezoid[2]);
    copy(right, topTrapezoid[3]);
    
    //BL and BR in bottom trapezoid are just bottom corner
    copy(triangle[2], bottomTrapezoid[2]);
    copy(triangle[2], bottomTrapezoid[3]);
    
    copy(left, bottomTrapezoid[0]);
    copy(right, bottomTrapezoid[1]);
    
  }

  
  /** Phong algorithm for shading vertices
    * This changes last 3 doubles in arrray but leave first 3 doubles alone
    */
  private void phong(double[] og){
    int[] rgb = new int[3];
    normal = new double[]{ og[3],og[4],og[5]};
    // START OUT WITH A BLACK PIXEL
    rgb[0] = rgb[1] = rgb[2] = 0;
    // ADD AMBIENT
    for (int i = 0 ; i < 3 ; i++)
      color[i] = ambientColor[i];
    // LOOP THROUGH ALL LIGHT SOURCES
    for (int l = 0 ; l < lightDirections.length ; l++) {
      // COMPUTE DIFFUSE
      double n_dot_l = dot(lightDirections[l], normal);
      if (n_dot_l > 0)
        for (int i = 0 ; i < 3 ; i++)
          diffuse[i] = diffuseColor[i] * n_dot_l;
      else
        for (int i = 0 ; i < 3 ; i++)
          diffuse[i] = 0;

      // COMPUTE REFLECTION DIRECTION
      for (int i = 0 ; i < 3 ; i++)
        reflection[i] = 2 * n_dot_l * normal[i] - lightDirections[l][i];

      double r_dot_e = dot(reflection, eyeDirection);

      // COMPUTE SPECULAR
      if (r_dot_e > 0) {
        double specularFactor = Math.pow(r_dot_e, specularPower);
        for (int i = 0 ; i < 3 ; i++)
        specular[i] = specularColor[i] * specularFactor;
      }
      else
        for (int i = 0 ; i < 3 ; i++)
          specular[i] = 0;
      
      // ADD DIFFUSE AND SPECULAR
      for (int i = 0 ; i < 3 ; i++)
        color[i] += lightColors[l][i] * (diffuse[i] + specular[i]);
    }
      
      
    // SEND COLOR TO THE FRAME BUFFER
    for (int i = 0 ; i < 3 ; i++) {
      double shade = Math.max(0, Math.min(1.0, color[i]));
      rgb[i] = (int)(255 * shade);
    }
    og[3] = rgb[0];
    og[4] = rgb[1];
    og[5] = rgb[2];
  }
  
  
  
  /** Universal Merge sort using dist function and point P */
  private double[][] mergeSort(double[][] m){

    //sub-problem arrays and result
    double[][] left, right, result;
    
    //base cases
    if(m.length <= 1)
      return m;

    //find middle and create and fille sub-problem arrays
    int middle = m.length/2;
    left = new double[middle][6];
    right = new double[m.length-middle][6];
    for(int j=0;j<left.length;j++)
      left[j] = m[j];
    for(int j=0;j<right.length;j++)
      right[j] = m[middle+j];

    //recursive calls
    left = mergeSort(left);
    right = mergeSort(right);

    //merge sub-problem results into result
    result = merge(left, right);
    return result;
  }

  private double[][] merge(double[][] left, double[][] right){
    //place for merged result 
    double[][] result = new double[left.length+right.length][6];
    int li = 0;
    int ri = 0;
    //merge using dist to compare
    while(li < left.length && ri < right.length){
      if(left[li][1] <= right[ri][1]){
        result[li+ri] = left[li];
        li++;
      }else{
        result[li+ri] = right[ri];
        ri++;
      }
    }

    //clean up left overs
    while(li<left.length){
      result[li+ri] = left[li];
      li++;
    }    
    while(ri<right.length){
      result[li+ri] = right[ri];
      ri++;
    }
    
    return result;
  }



  public boolean keyUp(Event e, int key) {
	switch (key) {

	  //RIGHT CONTROLLERS        
	  case Event.LEFT:
	  case '4':
		ball.vx-=5;
		break;
	  case Event.RIGHT:
	  case '6':
		ball.vx+=5;
		   break;
	  case Event.UP:
	  case '8':
		ball.vz-=5;
		   break;
	  case Event.DOWN:
	  case '5':
	  case '2':
		ball.vz+=5;
		   break;
	  case '+':
		right.mass*=2;
		   break;
	  case '_':
		right.mass/=2;
		   break;
	  case '=':
		left.mass/=2;
		   break;
	  case '-':
		left.mass*=2;
		   break;
	  case 'F':
	  case 'f':
		goFloor = !goFloor;
		   break;
	  case Event.PGDN:
	    camZ+=10;
		   break;
	  case Event.PGUP:
	    camZ-=5;
		   break;
	  case '>':
	    rotX-=Math.PI/16;
		   break;
	  case '<':
	    rotX+=Math.PI/16;
		   break;
	  case 'c':
	    ball.object[0].cube();
	    left.object[0].cube();
	    right.object[0].cube();
		   break;
	  case 'l':
	    ball.object[0].cylinder(40);
	    left.object[0].cylinder(40);
	    right.object[0].cylinder(40);
		   break;
	  case 'g':
	    ball.object[0].globe(20,10);
	    left.object[0].globe(20,10);
	    right.object[0].globe(20,10);
		   break;
	  case 'S':
	  case 's':
	    showInside= !showInside;
	    ball.object[0].showInside = showInside;
	    left.object[0].showInside = showInside;
	    right.object[0].showInside = showInside;
	    break;
	}
	return true;
  }

    
  // USEFUL MATH
  public static void lerp(double f , double y0[], double y1[], double dst[]){
    for(int i=0;i<y0.length;i++)
      dst[i] = y0[i] + (y1[i]-y0[i])*f;
  }
  public static void lerp(double f , int y0[], int y1[], int dst[]){
    for(int i=0;i<y0.length;i++)
      dst[i] = (int) (y0[i] + (y1[i]-y0[i])*f);
  }
  public static double lerp(double f , double y0, double y1){
    return y0 + (y1-y0)*f;
  }

  public void normalize(double vec[]) {
	 double len = norm(vec);
	 for (int i = 0 ; i < 3 ; i++)
		vec[i] /= len; 
  }
  
  /**
   * return z value of surface normal
   * Uses first 3 values
   */
  public static double normalZ(double[][] triangle){
    double[] p1 = triangle[0];
    double[] p2 = triangle[1];
    double[] p3 = triangle[2];
    
    //if rect then I can do a little err check and get a true triangle
    if (triangle.length>3){
	  if(equals(p1,p2)){
		p1 = triangle[3];
      }else if(equals(p1,p3)){
		p1 = triangle[3];
      }else if(equals(p2,p3)){
		p3 = triangle[3];
      }
    }
    
    
    double[] a = new double[] {p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    double[] b = new double[] {p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};
    
    /* 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)
     */
    //let's try that again
    //(a2b3 - a3b2, a3b1 - a1b3, a1b2 - a2b1)
    double[] cross = new double[] {a[1]*b[2] - a[2]*b[1],
      a[2]*b[0] - a[0]*b[2], a[0]*b[1] - a[1]*b[0]};
    return cross[2];  
  }
  
  public static boolean equals(double p1[], double p2[]){
    for(int i =0; i<p1.length; i++)
      if(p1[i]!=p2[i]) return false;
    return true;
  }
  
  public double norm(double vec[]) {
	 return Math.sqrt(dot(vec, vec));
  }

  public double dot(double a[], double b[]) {
	 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
  }

  //FINAL CONSTANTS
  final int NUM_SIDES_ON_TRIANGLE = 3;
  final double dampening = 0.99;

  // WORKING VARIABLES
  double color[] = new double[3];
  double diffuse[] = new double[3];
  double specular[] = new double[3];
  double point[] = {0,0,0};
  double normal[] = {0,0,0};
  double eyeDirection[] = {0,0,1};
  double reflection[] = {0,0,0};
  double radius = 0.2;
}