import vrml.*;
import vrml.node.*;
import vrml.field.*;

class ArmKinematics extends Script {
  private float ANGLE_INCR = 0.0001f;
	
	private MFNode armrange;
	private float minAngle, maxAngle;
  private float upperArmLength;
  private float lowerArmLength;
  private float handLength;
	private SFRotation joint_changed;
  private SFRotation upperArmRotation_changed;
  private SFRotation lowerArmRotation_changed;
  private SFRotation handRotation_changed;
	private MFNode addRange, removeRange;
	
	private boolean isactive, setjoint;
	private double jointangle;
  private float[] joint; // joint between upper and lower arms
  private float[] joint2; // joint between lower arm and hand
	
  public void initialize() {
		armrange = (MFNode) getField ("armrange");
    upperArmLength = ((SFFloat) getField("upperArmLength")).getValue();
    lowerArmLength = ((SFFloat) getField("lowerArmLength")).getValue();
    handLength = ((SFFloat) getField("handLength")).getValue();
		minAngle = ((SFFloat) getField ("minAngle")).getValue ();
		maxAngle = ((SFFloat) getField ("maxAngle")).getValue ();
		
		joint_changed = (SFRotation) getEventOut ("joint_changed");
    upperArmRotation_changed = (SFRotation) getEventOut("upperArmRotation_changed");
    lowerArmRotation_changed = (SFRotation) getEventOut("lowerArmRotation_changed");
    handRotation_changed = (SFRotation) getEventOut("handRotation_changed");
		
		addRange = (MFNode) getEventOut ("addRange");
		removeRange = (MFNode) getEventOut ("removeRange");
		
		isactive = false;
		setjoint = false;
		
    joint = new float[2];
    joint[0] = 0; // z-coord
    joint[1] = -upperArmLength; // y-coord
    joint2 = new float[2];
    joint2[0] = 0;
    joint2[1] = -upperArmLength - lowerArmLength;
	}
	
  public void processEvent(Event e) {
		
		String eventname = e.getName ();
		if (eventname.equals ("set_active"))
		{
			isactive = ((ConstSFBool) e.getValue ()).getValue ();
			if (isactive)
				addRange.setValue (armrange);
			else
				removeRange.setValue (armrange);
		}
		else if (eventname.equals ("set_posandjoint"))
		{
			setjoint = true;
		}
		else
		{
			setjoint = false;
		}
		
		if (!isactive) return;
		
    float[] position = new float[3];
    double length;
    double temp, angle1, angle2, angle3, alpha, beta;
    float[] tempJoint = new float[2];
    boolean recompute;
		
    recompute = true;
    angle1 = angle2 = angle3 = 0.0f;
    ((ConstSFVec3f) e.getValue()).getValue(position);
		
		if ((Math.abs (position[1]) < 0.01) || setjoint)
		{
			jointangle = -Math.atan2 (position[2], position[0]);
      if (jointangle > maxAngle) jointangle = maxAngle;
      if (jointangle < minAngle) jointangle = minAngle;
			
			joint_changed.setValue (0.0f, 1.0f, 0.0f, (float) jointangle);			
    }
		position[0] = (float) (position[0]*Math.cos (jointangle) - position[2]*Math.sin(jointangle));
		
    length = Math.sqrt(Math.pow(position[1]-joint2[1],2) + Math.pow(position[0]-joint2[0],2));
    if (length <= handLength) {
      angle2 = -(Math.atan2(position[0]-joint2[0], -(position[1]-joint2[1])));
      angle1 = 0.0;
      angle3 = 0.0;
			
      if (Math.abs(angle2) <= 0.785f)
        recompute = false;
		}
		
    if (recompute) {
      length = Math.sqrt(Math.pow(position[1]-joint[1],2) + Math.pow(position[0]-joint[0],2));
      if (length <= (lowerArmLength + handLength)) {
        temp = Math.acos((Math.pow(length,2) - Math.pow(lowerArmLength,2) - Math.pow(handLength,2))/
          (2 * lowerArmLength * handLength));
        angle2 = -temp;
				
        alpha = Math.atan2(position[1]-joint[1], position[0]-joint[0]);
        beta = Math.asin(-(Math.sin(temp) * handLength)/length);
        angle1 = -(alpha + beta + Math.PI/2);
				
        angle3 = 0.0;
				
        if (angle1 >= -2.791f && angle1 <= 0.0f && Math.abs(angle2) <= 0.785f)
          recompute = false;
			}
      if (recompute) {
        length = Math.sqrt(Math.pow(position[1],2) + Math.pow(position[0],2));
        if (length <= (upperArmLength + lowerArmLength + handLength)) {
          temp = Math.acos((Math.pow(length,2) - Math.pow(upperArmLength,2) - Math.pow(lowerArmLength+handLength,2))/
            (2 * upperArmLength * (lowerArmLength+handLength)));
          angle1 = -temp;
					
          alpha = Math.atan2(position[1], position[0]);
          beta = Math.asin(-(Math.sin(temp) * (lowerArmLength+handLength))/length);
          angle3 = -(alpha + beta + Math.PI/2);
					
          angle2 = 0.0;
					
          if (angle1 >= -2.791f && angle1 <= 0.0f && angle3 <= 0.785f && angle3 >= -3.14f)
            recompute = false;
				}
        if (recompute) {
          angle3 = - (Math.atan2(position[0], -position[1]));
          angle1 = 0.0;
          angle2 = 0.0;

          if (angle3 <= 0.785f && angle3 >= -3.14f && length > (upperArmLength+lowerArmLength+handLength))
            recompute = false;
					
				}
			}
		}
		
    if (!recompute) {
      upperArmRotation_changed.setValue(1.0f, 0.0f, 0.0f, (float) angle3);
      lowerArmRotation_changed.setValue(1.0f, 0.0f, 0.0f, (float) angle1);
      handRotation_changed.setValue(1.0f, 0.0f, 0.0f, (float) angle2);
		}
	}
}