/*
 * IK.java
 * This determines the jointvalues from gripperpositions found in the path-planning proces
 * Roelof de Vries  (0335843) (ravries)
 * Douwe Oosterhout (0612667) (doosterh)
 * Date: 30-06-06
 */

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

public class IK {

  // Class containing the Denavit-Hartenberg representation of the robot arm
  private static RobotJoints robotJoints;

  private static void handJointCalculation(GripperPosition pos, JointValues j) {
	double aanliggende = pythagoras(pos.normal.x, pos.normal.y);
	j.roll = 0; // the turning of the gripper does not effect anything, so it's best to just keep it a constant value
	j.pitch = -Math.toDegrees(Math.atan(pos.normal.z / aanliggende));
	j.grip = pos.grip;
  }

  static double pythagoras(double x, double y) {
	double z = Math.sqrt(x*x + y*y);	
	return z;
  }

  private static Point wristCoordinatesCalculation(GripperPosition pos) {

    Point c = new Point(pos.coords.x, 	  //does not change
			pos.coords.y,	  //as does this one
			pos.coords.z +
			robotJoints.get("roll").d +
			robotJoints.get("elbow").d +
			robotJoints.get("shoulder").d); // to get the top position, we have to add the height of the
							// roll, elbow and shoulder
    return(c);
  }

  private static void armJointCalculation(Point wristCoords, JointValues j) {

	// the following algorithm is the same as found in the syllabus

    double c, s, theta1, theta2, xsquare, ysquare, lelbow;
    xsquare = wristCoords.x * wristCoords.x;
    ysquare = wristCoords.y * wristCoords.y;	
    lelbow  = robotJoints.get("elbow").a; 		

    c  = (xsquare + ysquare - (lelbow * lelbow) - (lelbow * lelbow))  / (2 *(lelbow * lelbow)) ;

   if(wristCoords.x < 0)   	 // the elbow can be positioned to the left or the right
   	s = Math.sqrt(1 - (c*c));// if it crosses the line x=0 it changes it's position from left to right and vica versa
   else				 // if we want to use the arm to the right of this line, the elbow should be on the left
    		s = -Math.sqrt(1 - (c*c));	// because then most points can be reached

    theta1 = Math.atan2(wristCoords.y, wristCoords.x) - Math.atan2(lelbow * s, (lelbow*c) + lelbow);	
    theta2 = Math.atan2(s,c);
    j.zed = wristCoords.z; 
    j.shoulder = -Math.toDegrees(theta1)+90; 
    j.elbow = -Math.toDegrees(theta2);
    
  }

	/*
	 * jawCalculation determined the jaw angle which should be in the same direction as the boards normal vector
	 * the only problem is that the jaw cannot reach all points and is limited by the shoulder and elbow
	 * we haven't had time to solve this yet
	 */

  private static void jawCalculation(GripperPosition pos, JointValues j) {
    double newAngle = 90 - Math.toDegrees(Math.atan(pos.normal.y / pos.normal.x));
    double defaultAngle = -j.shoulder + (0.5 * -j.elbow); // from before we tilted the board
    j.yaw =  defaultAngle - newAngle;
  }

  private static JointValues jointCalculation(GripperPosition pos) {
    JointValues j = new JointValues();
    Point wristCoords;

    handJointCalculation(pos,j);
    wristCoords=wristCoordinatesCalculation(pos);
    armJointCalculation(wristCoords, j);
    jawCalculation(pos,j);
  
    return(j);
  }

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

    // initialize the Denavit-Hartenberg representation
    robotJoints = new RobotJoints();
  
    for (int i =0; i < p.size(); i++) {
      GripperPosition pos = (GripperPosition) p.elementAt(i);
      j.addElement(jointCalculation(pos));
    }

  }

  public static void main(String[] args) {
    Vector p = new Vector();
    Vector j = new Vector();

    System.out.println ("**** THIS IS THE STUDENT IK MODULE IN JAVA\n");

    // read the gripper positions as produced by PP.java
    GripperPosition.read(p);

    // correct for errors in the arm
    // (comment-out this correction if you are solving the
    // configuration-switching problem.) 
    RobotJoints.correctCartesian(p);
          
    inverseKinematics(p, j);
    
    for (int i =0; i < j.size(); i++)
              System.out.println((JointValues) j.get(i));

    JointValues.write(j);
  }
}