import vrml.*;
import vrml.node.*;
import vrml.field.*;
import java.math.*;
import blueprint;
					 
/*** Name: Conrad Chu and Daniel Lim
 *   Login: cs184-cc      cs184-bu
 *	 PROJECT 1
 */ 

public class project1 extends Script {
	/*** FIELDS ***/
	private SFRotation shoulder_CurrentRotation;				//The current rotation of the shoulder
	private SFRotation bicep_CurrentRotation;					//The current rotation of the bicep
	private SFRotation forearm_CurrentRotation;					//The current rotation of the forearm
	
	/*** EVENT OUTS ***/
	private SFRotation shoulder_RotationChanged;				//The rotation applied to the shoulder
	private SFRotation bicep_RotationChanged;					//The rotation applied to the bicep
	private SFRotation forearm_RotationChanged;					//The rotation applied to the forearm
	private SFRotation hand_RotationChanged;					//The rotation applied to the hand
		
	/** EVENT INS ***/
	private SFBool reset;								//Reset system
	private SFBool map1;
	private SFBool map2;
	private SFBool map3;
	
	/** VARIABLES ***/
	private blueprint map = new blueprint();			//Map configurations imported in	
	private SFVec3f [] objects;							//Array of object translations
	private SFVec3f [] objects_EventOuts;				//Array of the evenouts for the objects
		
	private float shoulder_NewRotation;					//The rotation float that is sent to shoulder_RotationChanged
	private float bicep_NewRotation;					//The rotation float that is sent to bicep_RotationChanged
	private float forearm_NewRotation;					//The rotation float that is sent to forearm_RotationChanged
	private float hand_NewRotation;						//The rotation float that is sent to hand_RotationChanged
	private float object_NewTranslation;				//The translation of the object
	
	private float shoulder_CurrentRotationKeyframe;		//The rotation float for the current shoulder keyframe
	private float bicep_CurrentRotationKeyframe;		//The rotation float for the current bicep keyframe
	private float forearm_CurrentRotationKeyframe;		//The rotation float for the current forearm keyframe
	
	private float shoulder_NextRotationKeyframe;		//The rotation float for the next shoulder keyframe
	private float bicep_NextRotationKeyframe;			//The rotation float for the next bicep keyframe
	private float forearm_NextRotationKeyframe;			//The rotation float for the next forearm keyframe
	
	private boolean inbetweening=true;					//If robot is middle of interpolating
	private boolean isGrabbed=false;					//Boolean if object is being grabbed or not
	private boolean getNextKeyframe=true;				//Boolean of getting the next keyframe
	private int obj_count=0;							//Number of object being handled, 0... 100
	
	private float frame_num = 0.0f;						//Current frame number in interpolation
	private float r;									//distance from (0,0) to object
	private int map_num=0;								//Map number
	
	/** CONSTANTS **/	
	private final float FRAME_MAX = 50.0f;					//Resolution of interpolation	
	private final float bicep_length = 3.1f;
	private final float forearm_length = 1.7f;

	
  public void initialize() {
    try { 
	  //Initalize variables	  		
	  objects = new SFVec3f[map.NUM_OBJECTS];				//Define array of object translations 	  
	  objects_EventOuts = new SFVec3f[map.NUM_OBJECTS];
	  
	  //Initalize event outs
	  shoulder_RotationChanged = (SFRotation)(getEventOut("shoulder_RotationChanged"));
	  bicep_RotationChanged = (SFRotation)(getEventOut("bicep_RotationChanged"));
	  forearm_RotationChanged = (SFRotation)(getEventOut("forearm_RotationChanged"));
	  hand_RotationChanged = (SFRotation)(getEventOut("hand_RotationChanged"));
	  
	  for(int i=0; i<map.NUM_OBJECTS;i++){		  
		  objects_EventOuts[i] = (SFVec3f)(getEventOut("object" + i + "_translation"));   //init eventouts		  
		  objects[i] = (SFVec3f)(getField("object" + i));			//init fields
	  }
	  
      //Initalize fields
	  shoulder_CurrentRotation = (SFRotation)(getField("shoulder_CurrentRotation"));
	  bicep_CurrentRotation = (SFRotation)(getField("bicep_CurrentRotation"));
	  forearm_CurrentRotation = (SFRotation)(getField("forearm_CurrentRotation"));
	  
	  //Initialize rotations
	  float temp[] = new float[4];
	  shoulder_CurrentRotation.getValue(temp);	//Set the current rotation of the shoulder to shoulder_CurrentRotationKeyframe
	  shoulder_CurrentRotationKeyframe = temp[3];
	  
	  bicep_CurrentRotation.getValue(temp);		//Set the current rotation of the bicep to bicep_CurrentRotationKeyframe
	  bicep_CurrentRotationKeyframe = temp[3];
	  
	  forearm_CurrentRotation.getValue(temp);	//Set the current rotation of the forearm to forearm_CurrentRotationKeyframe
	  forearm_CurrentRotationKeyframe = temp[3];	  
    }
    catch(InvalidFieldException e) {
      System.out.println("invalid initialization");
      System.exit(1);
    }
  }

  public void shoulder_SetRotation(float vals[]) {
    shoulder_CurrentRotation.setValue(vals);
    shoulder_RotationChanged.setValue(vals);
  }
 
  public void bicep_SetRotation(float vals[]) {
    bicep_CurrentRotation.setValue(vals);
    bicep_RotationChanged.setValue(vals);
  }
  
  public void forearm_SetRotation(float vals[]) {
    forearm_CurrentRotation.setValue(vals);
    forearm_RotationChanged.setValue(vals);
  }    
   
  private void kinematics(float set_fraction){
  	    Interpolate();				//Interpolate new rotations for shoulder, bicep, and forearm
	  	
		shoulder_RotationChanged.setValue(0.0f, 1.0f, 0.0f, shoulder_NewRotation);
		bicep_RotationChanged.setValue(0.0f, 1.0f, 0.0f, -bicep_NewRotation);
		forearm_RotationChanged.setValue(0.0f, 1.0f, 0.0f, -forearm_NewRotation);
		hand_RotationChanged.setValue(0.0f, 1.0f, 0.0f, hand_NewRotation);
					
		if(isGrabbed){
			float [] temp = new float[3];
			temp = getEndEffector();
			objects_EventOuts[obj_count].setValue(getEndEffector());
		}
  }    
  
  private void reset(boolean bval) {			//Reset all values to initial conditions
	  if(bval){
		for(int i=0; i<map.NUM_OBJECTS;i++){
			  objects_EventOuts[i].setValue(objects[i]);
		}
		getNextKeyframe = true;
		obj_count = 0;
		isGrabbed = false;
		frame_num = 0.0f;
		inbetweening=true;					//If robot is middle of interpolating		
	  }
	  
  }
  
  private void map1(boolean bval) {			//Use the first map configuration
	  if(bval){	
		  reset(bval);
		  map_num = 0;
	  }
  }
  
  private void map2(boolean bval) {			//Use the second map configuration
	  if(bval){	
		  reset(bval);
		  map_num = 1;
	  }
  }
  
  private void map3(boolean bval) {			//Use the third map configuration
	  if(bval){	
		  reset(bval);
		  map_num = 2;
	  }  
  }

  /** Postcondition: Set the new rotations for all the joints */
  private void Interpolate(){												
	  if(inbetweening){	
		if(getNextKeyframe){				//If need the next keyframe			
			shoulder_NextRotationKeyframe = getShoulderRotation();			//Assign a new shoulder rotation keyframe
			bicep_NextRotationKeyframe = getBicepRotation();				//Assign a new bicep rotation keyframe
			forearm_NextRotationKeyframe = getForearmRotation();			//Assign a new forearm rotation keyframe
			getNextKeyframe = false;
		}		
			
		//Shoulder interpolation
		shoulder_NewRotation = shoulder_CurrentRotationKeyframe + (frame_num/FRAME_MAX) * (shoulder_NextRotationKeyframe - shoulder_CurrentRotationKeyframe);    

		//Forearm interpolation
		bicep_NewRotation = bicep_CurrentRotationKeyframe + (frame_num/FRAME_MAX) * (bicep_NextRotationKeyframe - bicep_CurrentRotationKeyframe);    
		
		//Bicep interpolation
		forearm_NewRotation = forearm_CurrentRotationKeyframe + (frame_num/FRAME_MAX) * (forearm_NextRotationKeyframe - forearm_CurrentRotationKeyframe);    
		
		if((frame_num > FRAME_MAX && isGrabbed) || (frame_num > FRAME_MAX-1 && !isGrabbed)) {					//Robot has reached destination		
			inbetweening = false;						//Reached destination, so stop interpolating joint movement until grab/ungrab is complete			
			frame_num = 0.0f;          					//Reset frame number
			getNextKeyframe = true;						//Next time, compute the next keyframe
			isGrabbed = !isGrabbed;
			inbetweening = false;
			if(!isGrabbed){			
				obj_count++;								//Proceed to the next object			  
			}
			
			//Set current rotation keyframe to the next
			shoulder_CurrentRotationKeyframe = shoulder_NextRotationKeyframe;
			bicep_CurrentRotationKeyframe = bicep_NextRotationKeyframe;
			forearm_CurrentRotationKeyframe = forearm_NextRotationKeyframe;
		} else frame_num++;									//Increment frame number		  
		
		
	}   	
   else{
	if(frame_num > FRAME_MAX-40.0f){						//If reached destination, hold there for a while and then continue interpolating
		frame_num = 0.0f;
		inbetweening = true;
	}else frame_num++;
   }
  }
  
	
	/** Postcondition: Return the rotation float of the next shoulder keyframe **/
	public float getShoulderRotation(){	  
		float x,z;
		if(!isGrabbed){			//If joint has not reached object and in not grab mode yet...
			x = objects[obj_count].getX();
			z = objects[obj_count].getZ();
		} else {				//Use map.destination_Translation's x value
			x = map.destination_Translation[map_num][obj_count][0];
			z = map.destination_Translation[map_num][obj_count][2];		
		}	  	  
		return -1.0f * (float)Math.atan2((double)z, (double)x);
	}									 							 								 	  
  	
	/** Postcondition: Return the rotation float of the next bicep keyframe **/
	public float getBicepRotation(){
	  float x,y,z;	  
		if(!isGrabbed){			//If joint has not reached destination and in grab mode yet...
		  x = objects[obj_count].getX();
		  y = objects[obj_count].getY();
		  z = objects[obj_count].getZ();	
	  } else{					//In grab mode, bring object to a destination]
		  x = map.destination_Translation[map_num][obj_count][0];
		  y = map.destination_Translation[map_num][obj_count][1];
		  z = map.destination_Translation[map_num][obj_count][2];
	  }		
	  r = (float)Math.sqrt((double)(x*x + y*y + z*z));									//Compute distance r		  		   
	  double phi = Math.atan2((double)y,Math.sqrt((double)(x*x + z*z)));	  
	  return (float)(phi + Math.acos(( (double)(bicep_length*bicep_length) - (double)(forearm_length*forearm_length) + (double)(r*r))/(2.0*bicep_length*r)));	  	  
	}			

	/** Postcondition: Return the rotation float of the next forearm keyframe **/
	public float getForearmRotation(){
		float x,y,z;
		if(!isGrabbed){			//If joint has not reached destination and in grab mode yet...
		  x = objects[obj_count].getX();
		  y = objects[obj_count].getY();
		  z = objects[obj_count].getZ();		  
	  } else{					//In grab mode, bring object to a destination
		  x = map.destination_Translation[map_num][obj_count][0];
		  y = map.destination_Translation[map_num][obj_count][1];
		  z = map.destination_Translation[map_num][obj_count][2];
	  }	  
	  r = (float)Math.sqrt((double)(x*x + + y*y + z*z));									//Compute distance r		  		   		  
	  return -(float)(Math.PI - Math.acos(( (double)(bicep_length*bicep_length) + (double)(forearm_length*forearm_length) - (double)(r*r))/(2.0*bicep_length*forearm_length)));	  
	}		
	
	/** Postcondition: Return the end effector translation values as a 1x3 float array */
	public float[] getEndEffector(){
		float [] temp = new float[3];
		r = (bicep_length * (float)Math.cos((double)bicep_NewRotation)) + (forearm_length * (float)Math.cos((double)(bicep_NewRotation + forearm_NewRotation)));

		//Calculate x, y, and z through forward kinematics
		temp[0] = r * (float)Math.cos((double)shoulder_NewRotation);
		temp[1] = (bicep_length * (float)Math.sin((double)bicep_NewRotation)) + (forearm_length * (float)Math.sin((double)(bicep_NewRotation + forearm_NewRotation)));
		temp[2] = -1.0f * r * (float)Math.sin((double)shoulder_NewRotation);		
		
		return temp;		
	}
	 
  public void processEvent(Event e) {
  	String name = e.getName();	  
   	
	if(name.equals("set_fraction"))
			kinematics(((ConstSFFloat)e.getValue()).getValue());	
	else if(name.equals("reset")) {
		reset(((ConstSFBool)e.getValue()).getValue());
	}
	else if(name.equals("map1")) {
		map1(((ConstSFBool)e.getValue()).getValue());
	}
	else if(name.equals("map2")) {
		map2(((ConstSFBool)e.getValue()).getValue());
	}
	else if(name.equals("map3")) {
		map3(((ConstSFBool)e.getValue()).getValue());
	}
    else {
      System.out.println("error: unknown event");
      System.exit(3);
    }
  }
}