//*****************************************************************************
// TITLE:         Project 1 (template)
// DESCRIPTION:   Prediction
// AUTHOR:        Prof Jarek Rossignac
// DATE CREATED:  January 20. 2008
// EDITS:
//*****************************************************************************
boolean showText = true;      // toggled by pressing the 'h'key to show/hide help text
boolean printIt=false;        // temporarily set when the '?' key is pressed and used to print some debugging values
int n=4;                      // number of points
pt [] PP = new pt[n];       // decalres an array of n points
int p=0;                    // index to the point being dragged
color red, yellow, green, cyan, blue, magenta, dred, dyellow, dgreen, dcyan, dblue, dmagenta, white, black, orange, grey, metal;  // declares colors

void setup() { size(600, 600); smooth(); colorMode(HSB,121);  strokeJoin(ROUND); strokeCap(ROUND); // set up window and drawing modes
  red = color(0, 120, 120); yellow = color(20, 120, 120); green = color(40, 120, 120); cyan = color(60, 120, 120); blue = color(80, 120, 120); magenta = color(100, 120, 120); 
  dred = color(0, 120, 60); dyellow = color(20, 120, 60); dgreen = color(40, 120, 60); dcyan = color(60, 120, 60); dblue = color(80, 120, 60); dmagenta = color(100, 120, 60); 
  white = color(0, 0, 120); black = color(0, 120,0); grey = color(0, 120,60); orange = color(10, 100, 120); metal = color(70, 60, 100);
  PFont font = loadFont("Courier-14.vlw"); textFont(font, 14);      // load font
  for (int i=0; i<PP.length; i++) {PP[i]=new pt(width/2,height/2+height/4); PP[i].rotateBy(-i*PI/3, screenCenter());}; // init all points to be on a circle
  } 
 
void draw() {         // rendering loop to refresh the window
  background(121);  strokeWeight(1);                        // sets background
  if (showText) {fill(dblue); pushMatrix(); translate(20,20);
   text("Prediction by Jarek Rossignac",0,0); translate(0,20);
   text("press 'h' to show/hide the help text",0,0); translate(0,20);
   text("click and drag closest point A, B, C, or D",0,0); translate(0,20);
   text("1 is the linear predictor P=D+CD",0,0); translate(0,20);
   text("2 is the quadratic predictor P=B+3CD",0,0); translate(0,20);
   text("3 is the cubic predictor P=B+AD+3CD-3BC",0,0); translate(0,20);
   text("4 is the barycentric predictor",0,0); translate(0,20);
   popMatrix(); noFill();};
  if (keyPressed) {char c=key;  if ( 47<int(c) ) if ( int(c)<58 ) p=int(c)-48;  }; // to select the dragged point by holding down the key with its number
  if (mousePressed) PP[p].setToMouse();
  strokeWeight(2);
  stroke(metal);   for (int i=0; i<4; i++) {PP[i].show();}
  noFill(); beginShape(); for (int i=0; i<4; i++) PP[i].v(); endShape();
  pt A=PP[0]; pt B=PP[1]; pt C=PP[2]; pt D=PP[3]; 
  fill(dblue); A.showLabel("A"); B.showLabel("B"); C.showLabel("C"); D.showLabel("D"); 
  pt P=predict1(A,B,C,D); stroke(red);   D.showSegmentTo(P); fill(red);   P.show(); P.showLabel("1");
     P=predict2(A,B,C,D); stroke(green); D.showSegmentTo(P); fill(green); P.show(); P.showLabel("2");
     P=predict3(A,B,C,D); stroke(blue); D.showSegmentTo(P); fill(blue); P.show(); P.showLabel("3");
     P=predict4(A,B,C,D); stroke(magenta); D.showSegmentTo(P); fill(magenta); P.show(); P.showLabel("4");
     P=predict5(A,B,C,D); stroke(dred); D.showSegmentTo(P); fill(dred); P.show(); P.showLabel("5");
     P=predict6(A,B,C,D); stroke(dgreen); D.showSegmentTo(P); fill(dgreen); P.show(); P.showLabel("6");
     P=predict7(A,B,C,D); stroke(dblue); D.showSegmentTo(P); fill(dblue); P.show(); P.showLabel("7");
//     P=predict8(A,B,C,D); stroke(dmagenta); D.showSegmentTo(P); fill(dmagenta); P.show(); P.showLabel("8");
 
  if(printIt) {print(" Debugging Info:"); print("P="); P.write(); println(); printIt=false;};  // prints in the lower pane (local)
   };

void mousePressed() {p=0; float d=PP[0].disToMouse(); for (int i=1; i<PP.length; i++) if (PP[i].disToMouse()<d) {d=PP[i].disToMouse(); p=i;} }  

void keyPressed() {  
   if (key=='h')   showText = !showText;
   if (key=='?')   printIt = true;        // sets the debugging mode for one pass
   if (key=='X') {String S="predict"+"-####.tif"; saveFrame(S);};   // makes a picture
   };     

pt predict1(pt A, pt B, pt C, pt D) {pt P = new pt(C); vec CD=C.makeVecTo(D); P.translateBy(2,CD); return P; }
pt predict2(pt A, pt B, pt C, pt D) {pt P = new pt(B); vec CD=C.makeVecTo(D); P.translateBy(3,CD); return P; }
pt predict3(pt A, pt B, pt C, pt D) {pt P = new pt(B); 
    vec AD=A.makeVecTo(D); P.translateBy(AD);
    vec CD=C.makeVecTo(D); P.translateBy(3,CD);
    vec BC=B.makeVecTo(C); P.translateBy(-3,BC);
    return P; }
pt predict4 (pt A, pt B, pt C, pt D) {
  float a = D.barycentricA(A,B,C); float b = D.barycentricB(A,B,C); float c = D.barycentricC(A,B,C);
  
  vec CD = C.makeVecTo(D);
   float ang = 180/3.14159 * atan2(CD.y,CD.x);
   //text("a="+a+", b="+b+",c="+c,40,height-40);  
   return barycentric(a,B,b,C,c,D); }

// add your own predictors 5, 6, 7, 8


pt predict5 (pt A, pt B, pt C, pt D) {
  
  vec AB = A.makeVecTo(B);
  float ang1 = atan2(AB.y,AB.x);
  float length1 = sqrt((AB.x*AB.x) + (AB.y*AB.y));
  
  vec BC = B.makeVecTo(C);
  float ang2 = atan2(BC.y,BC.x);
  float length2 = sqrt((BC.x*BC.x) + (BC.y*BC.y));
  
  vec CD = C.makeVecTo(D);
  float ang3 = atan2(CD.y,CD.x);
  float length3 = sqrt((CD.x*CD.x) + (CD.y*CD.y));
  
  vec BC2 = new vec(BC.x*cos(-ang1) - BC.y*sin(-ang1),BC.x*sin(-ang1) + BC.y*cos(-ang1));
  vec CD2 = new vec(CD.x*cos(-ang2) - CD.y*sin(-ang2),CD.x*sin(-ang2) + CD.y*cos(-ang2));
  
  float ang4 = atan2(BC2.y,BC2.x);;
  float ang5 = atan2(CD2.y,CD2.x);;
  
  //float newAng = ((ang2 - ang1) + (ang3 - ang2))/2;
  
  float newAng;
 
 //if((ang4 < 0 && ang5 > 0) || ((ang5 < 0 && ang4 > 0))){
 // newAng = -(ang4 + ang5)/2;
 //}
 //else{
 
   float t = abs(ang4 - ang5)/(2*3.14159); // t varies from 0 (same angle) to 1 (opposite angles)
   
   float avgLength = (length1 + length2 + length3)/3;
   
   float a=1,b=1,c=1;
   //a = (3*avgLength/(length1+length2+length3));
   //b = (2*avgLength/(length2+length3));
   //c = (1*avgLength/(length3));
   
   
   
   //a = ((length1+length2+length3)/(3*avgLength));
   //b = ((length2+length3)/(2*avgLength));
   //c = ((length3)/(1*avgLength));
   
   //newAng = (b*t*ang4 + c*(1-t)*ang5);
   newAng = (t*ang4 + (1-t)*ang5);
   //newAng = (ang4 + ang5)/2;
   //newAng = ang4;
 //}
  //float newLength = avgLength;
  float newLength = (a*length1 + b*length2 + c*length3)/3;
  
  vec V1 = new vec(newLength * CD.x/length3,newLength * CD.y/length3);
  vec V2 = new vec(V1.x*cos(newAng) - V1.y*sin(newAng),V1.x*sin(newAng) + V1.y*cos(newAng));
  
  pt P3 = new pt(D.x + V2.x,D.y + V2.y);
  //text("newAng="+newAng+", ang4="+ang4+", ang5="+ang5+",t="+t,40,height-40);  

  text("Kyle Olszewski (P5), Timothy White (P6), Abhishek Venkatesh (P7)",40,height-40);  

  //pt P3 = new pt(A.x,A.y);
  return P3; }  

pt predict6 (pt A, pt B, pt C, pt D) {
 
  //the vectors connecting each of the points
    vec AB=A.makeVecTo(B);
    vec BC=B.makeVecTo(C);
    vec CD=C.makeVecTo(D);

    //create point by applying previous vectors to next point
    pt dummyA = new pt(B);
    dummyA.translateBy(AB);
    pt dummyB = new pt(C);
    dummyB.translateBy(BC);
    pt dummyC = new pt(D);
    dummyC.translateBy(CD);

    //calculate vector to go from dummy point to next point
    vec dAC = dummyA.makeVecTo(C);
    vec dBD = dummyB.makeVecTo(D);
    vec DdC = D.makeVecTo(dummyC);

    //scale vector
    dAC.scaleBy(.2);
    dBD.scaleBy(.35);
    DdC.scaleBy(.55);

    // combine vectors to get predicted point
    pt fin = new pt(D);
    fin.translateBy(dAC);
    fin.translateBy(dBD);
    fin.translateBy(DdC);
    return fin;
  
}

pt predict7 (pt A, pt B, pt C, pt D) 
{
	float L=(A.makeVecTo(B).norm()+B.makeVecTo(C).norm()+C.makeVecTo(D).norm())/3.0f;
	pt O1=circumCenter (A,B,C); 
	pt O2=circumCenter (B,C,D);
	vec O1O2=O1.makeVecTo(O2);
	pt k=C.makeClone();
	k.scaleBy(.5f);
	k.addScaledPt(.5f,D);
	//k.show(3);
	pt O3_1=O2.makeClone(),O3_2=O2.makeClone();
	O3_1.translateBy(O1O2.norm(), k.makeVecTo(O2).makeUnit());
	O3_2.translateBy(O1O2.norm(), O2.makeVecTo(k).makeUnit());
//	O1.showLabel("O1");
	O1.show(2);
//	O2.showLabel("O2");
//	O2.show(2);
//	O3_1.show(3);
//	O3_2.show(3);
	//pt O3;

//	if(dot(O1O2,O3_1.makeVecTo(O2))<dot(O1O2,O3_2.makeVecTo(O2) ) )
//	if(O3_1.disTo(k)>O3_2.disTo(k))
	//	O3=O3_1.makeClone();
	//else
		//O3=O3_2.makeClone();
	stroke(black);
	float r1=O3_1.makeVecTo(D).norm();
	float r2=O3_2.makeVecTo(D).norm();	
	noFill();
	
//	ellipse(O3_1.x,O3_1.y,2*r1,2*r1);
//	ellipse(O3_2.x,O3_2.y,2*r2,2*r2);	

	
//	D.showSegmentTo(O3_1);
//	D.showSegmentTo(O3_2);

	float alpha1=atan2(D.y-O3_1.y,D.x-O3_1.x);
	if(abs(D.x-O3_1.x)<.001f)
		alpha1=3.14f/2*abs(D.y-O3_1.y)/(D.y-O3_1.y);
	float theta1=2*asin(max(-1.0f,min(1.0f,L/(2*r1))));
	float alpha2=atan2(D.y-O3_2.y,D.x-O3_2.x);
	if(abs(D.x-O3_2.x)<.001f)
		alpha2=3.14f/2*abs(D.y-O3_2.y)/(D.y-O3_2.y);
	float theta2=2*asin(max(-1.0f,min(1.0f,L/(2*r2))));
	
	pt []P=new pt[8];
	P[0]=new pt(O3_1.x+r1*cos(-alpha1+theta1),O3_1.y+r1*sin(-alpha1+theta1));
	P[1]=new pt(O3_1.x+r1*cos(alpha1+theta1),O3_1.y+r1*sin(alpha1+theta1));	
	P[2]=new pt(O3_1.x+r1*cos(-alpha1-theta1),O3_1.y+r1*sin(-alpha1-theta1));
	P[3]=new pt(O3_1.x+r1*cos(alpha1-theta1),O3_1.y+r1*sin(alpha1-theta1));	
	P[4]=new pt(O3_2.x+r2*cos(-alpha2+theta2),O3_2.y+r2*sin(-alpha2+theta2));
	P[5]=new pt(O3_2.x+r2*cos(alpha2+theta2),O3_2.y+r2*sin(alpha2+theta2));	
	P[6]=new pt(O3_2.x+r2*cos(-alpha2-theta2),O3_2.y+r2*sin(-alpha2-theta2));
	P[7]=new pt(O3_2.x+r2*cos(alpha2-theta2),O3_2.y+r2*sin(alpha2-theta2));	
	pt O3;
	float r,alpha,theta;


//	println(180/3.14*acos(dot(B.makeVecTo(A).makeUnit(),B.makeVecTo(C).makeUnit()))+","+180/3.14*acos(dot(C.makeVecTo(B).makeUnit(),C.makeVecTo(D).makeUnit()))+","+180/3.14*acos(dot(D.makeVecTo(C).makeUnit(),D.makeVecTo(A).makeUnit())) +"\n");
	println(180/3.14*acos(dot(B.makeVecTo(A).makeUnit(),B.makeVecTo(C).makeUnit()))+","+180/3.14*acos(dot(C.makeVecTo(B).makeUnit(),C.makeVecTo(D).makeUnit()))+","+180/3.14*acos(dot(D.makeVecTo(C).makeUnit(),D.makeVecTo(A).makeUnit())) +"\n");
	if((acos(dot(B.makeVecTo(A).makeUnit(),B.makeVecTo(C).makeUnit()))+acos(dot(D.makeVecTo(C).makeUnit(),D.makeVecTo(A).makeUnit())) < 3.14f))
//	if((asin(dot(B.makeVecTo(A).makeUnit(),B.makeVecTo(C).makeUnit().makeTurnedLeft()))+asin(dot(D.makeVecTo(C).makeUnit(),D.makeVecTo(A).makeUnit().makeTurnedLeft())) < 3.14f))
		{
		println("bigger\n");
			if(O3_1.disTo(D)<O3_2.disTo(D))
	//		if(dot(O1O2,O3_1.makeVecTo(O2))>dot(O1O2,O3_2.makeVecTo(O2) ) )
				{O3=O3_1.makeClone();r=r1;alpha=alpha1;theta=theta1;}
			else
				{O3=O3_2.makeClone();r=r2;alpha=alpha2;theta=theta2;}	
		
		}
	else
		{
			if(O3_1.disTo(D)>O3_2.disTo(D))
//			if(dot(O1O2,O3_1.makeVecTo(O2))>dot(O1O2,O3_2.makeVecTo(O2) ) )
				{O3=O3_1.makeClone();r=r1;alpha=alpha1;theta=theta1;}
			else
				{O3=O3_2.makeClone();r=r2;alpha=alpha2;theta=theta2;}	
		
		}
//	O3.show(5);
//	O3.showLabel("O3");
	
	P[0]=new pt(O3.x+r*cos(-alpha+theta),O3.y+r*sin(-alpha+theta));
	P[1]=new pt(O3.x+r*cos(alpha+theta),O3.y+r*sin(alpha+theta));	
	P[2]=new pt(O3.x+r*cos(-alpha-theta),O3.y+r*sin(-alpha-theta));
	P[3]=new pt(O3.x+r*cos(alpha-theta),O3.y+r*sin(alpha-theta));	
	
	int maxi=0;
	float maxdot=-1000000;
	for(int i=0;i<4;i++)
	{
		float curdot=dot(C.makeVecTo(D),D.makeVecTo(P[i]));
//		vec V=D.makeVecTo(A);
//		float curdot=dot(V,D.makeVecTo(P[i]));
		//if((acos(dot(B.makeVecTo(A).makeUnit(),B.makeVecTo(C).makeUnit()))+acos(dot(D.makeVecTo(C).makeUnit(),D.makeVecTo(A).makeUnit())) < 3.14f))
		{
//				V=A.makeVecTo(D);
	//			curdot=dot(V,D.makeVecTo(P[i]));
//			curdot=dot(D.makeVecTo(C),D.makeVecTo(P[i]));

		}
	//	println(i+":"+curdot+",,,"+P[i].x+"\n");
		if(abs(P[i].disTo(D)-L)<0.5f  &&  curdot>maxdot)
		{
			maxi=i;
			maxdot=curdot;
		}
	}

	
	return P[maxi];

//	if(dot(C.makeVecTo(D),D.makeVecTo(P1))>dot(C.makeVecTo(D),D.makeVecTo(P2)))
	//	return P2;
	//else return P1;
}