import java.lang.*;
import java.util.Vector;

public class IK {

    private static RobotJoints robotJoints;
    private static double shoulderLength;
    private static double elbowLength;
	private static int SAFE_HEIGHT = 100;

    private static void handJointCalculation(GripperPosition pos,
                                             JointValues j){
	j.roll 	= pos.theta; 	
	j.pitch	= 0; 	
	j.yaw 	= -(j.shoulder + j.elbow/2);
	j.grip 	= pos.grip; 	
    }

    private static Point wristCoordinatesCalculation(GripperPosition pos){

	Point c = new Point(pos.coords.x,pos.coords.y,pos.coords.z); 
	return(c);
    }

    private static void armJointCalculation(Point wristCoords, JointValues j, boolean positive){
	double theta1, theta2,c2,s2;
	c2 =  ((Math.pow(wristCoords.y,2) + Math.pow(wristCoords.x,2) - Math.pow(shoulderLength,2) -Math.pow(elbowLength,2))/(2*shoulderLength*elbowLength));
	
	if(positive)
		s2 = +Math.sqrt(1-Math.pow(c2,2)); 
	else
		s2 = -Math.sqrt(1-Math.pow(c2,2)); 
	
	theta2 = Math.atan2(s2,c2);
	theta1 = Math.atan2(wristCoords.x,wristCoords.y) - Math.atan2(elbowLength*s2,shoulderLength+elbowLength*c2);
		    
	j.zed = wristCoords.z; 
	j.shoulder = Math.toDegrees(theta1); 
	j.elbow = Math.toDegrees(theta2); 
    }

    private static JointValues jointCalculation(GripperPosition pos, boolean positive){
	JointValues j = new JointValues();
	Point wristCoords;
	
	wristCoords=wristCoordinatesCalculation(pos);
	armJointCalculation(wristCoords, j, positive);
	handJointCalculation(pos,j);
	
	return(j);
    }

    private static void inverseKinematics(Vector p, Vector j, boolean positive) {

        robotJoints = new RobotJoints();
	
	
	
	for (int i =0; i < p.size(); i++){
	    GripperPosition pos = (GripperPosition) p.elementAt(i);
	    JointValues jv = jointCalculation(pos,positive);
	    if(j.size() > 0) {
		if(rangeCheck(jv)) {
			if(!legalTransition(((JointValues)j.lastElement()), jv)) {
				JointValues jv2 = jointCalculation(pos,!positive);
				if(rangeCheck(jv2)) {
					if(legalTransition(((JointValues)j.lastElement()), jv2)){
						positive = !positive;
						j.addElement(jv2);
					}
					else {
						JointValues last = (JointValues) j.lastElement();
						JointValues last2 = new JointValues(430, last.shoulder, last.elbow, last.yaw, last.pitch, last.roll, last.grip);
						j.addElement(last2);
						JointValues jv3 = new JointValues(430, jv2.shoulder, jv2.elbow, jv2.yaw, jv2.pitch, jv2.roll, jv2.grip);
						j.addElement(jv3);
						j.addElement(jv2);			
					}
				}
			}
			else {
				j.addElement(jv);
			}
		}
		else {
			positive = !positive;
			JointValues jv2 = jointCalculation(pos,positive);
			if(rangeCheck(jv2)) {
				if(legalTransition(((JointValues)j.lastElement()), jv2)){
					j.addElement(jv2);
				}
				else {
					JointValues last = (JointValues) j.lastElement();
					JointValues last2 = new JointValues(SAFE_HEIGHT, last.shoulder, last.elbow, last.yaw, last.pitch, last.roll, last.grip);
					j.addElement(last2);
					JointValues jv3 = new JointValues(SAFE_HEIGHT, jv2.shoulder, jv2.elbow, jv2.yaw, jv2.pitch, jv2.roll, jv2.grip);
					j.addElement(jv3);
					j.addElement(jv2);			
				}
			}
			else{
				// illegal move
			}
		}
	    } else {
		j.addElement(jv);
	    }
	}
    }
    
    private static boolean rangeCheck(JointValues current) {
	if(current.shoulder < -90 || current.shoulder > 90) return false;
	if(current.elbow < -180 || current.elbow > 151) return false;
	return true;
    }

    private static boolean legalTransition(JointValues last, JointValues current) {
	if((last.shoulder < 0) != (current.shoulder < 0) && current.zed < 300) return false;
	return true;
    }
    
    private static Vector bestList(Vector pos, Vector neg) {
		int iPos = 0, iNeg = 0;
	
		for(int a = 1; a < pos.size(); a++) {
			if(  legalTransition(((JointValues)pos.get(a-1)), ((JointValues)pos.get(a))))
				iPos++;
		}
		for(int b = 1; b < neg.size(); b++) {
			if(legalTransition(((JointValues)neg.get(b-1)), ((JointValues)neg.get(b))))
				iNeg++;
		}
	
			
		if(iPos < iNeg) {
			return pos;
		}
		else {
			return neg;
		}
    }
    
    public static void cartesianToJoints(Vector p) {
		Vector positive = new Vector();
		Vector negative = new Vector();
		Vector finalpos	= new Vector();
		
	// 	RobotJoints.correctCartesian(p);
		robotJoints = new RobotJoints();
		shoulderLength = robotJoints.get("shoulder").a;
			elbowLength = robotJoints.get("elbow").a;
		
		inverseKinematics(p, negative, false);
		inverseKinematics(p, positive, true);
	
		Vector bestList = bestList(negative, positive);
		
		for(int i=0; i < bestList.size(); i++)
			System.out.println(bestList.get(i).toString());
		
		JointValues.write(bestList);
    }
}