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

public class TorsoScript extends Script
{
	protected MFNode torsorange;
	protected float armLength, shoulderLength, torsoLength;

	protected SFBool enableMe, disableMe;
	protected SFVec3f arm_changed, armandjoint_changed;
	protected SFRotation range_changed, shoulder_changed, torso_changed;
	protected MFNode addRange, removeRange;

	protected boolean isactive;

	public void initialize ()
	{
		torsorange = (MFNode) getField ("torsorange");
		armLength = ((SFFloat) getField ("armLength")).getValue ();
		shoulderLength = ((SFFloat) getField ("shoulderLength")).getValue ();
		torsoLength = ((SFFloat) getField ("torsoLength")).getValue ();

		enableMe = (SFBool) getEventOut ("enableMe");
		disableMe = (SFBool) getEventOut ("disableMe");
		arm_changed = (SFVec3f) getEventOut ("arm_changed");
    range_changed = (SFRotation) getEventOut ("range_changed");
		shoulder_changed = (SFRotation) getEventOut ("shoulder_changed");
		torso_changed = (SFRotation) getEventOut ("torso_changed");
		addRange = (MFNode) getEventOut ("addRange");
		removeRange = (MFNode) getEventOut ("removeRange");

		armandjoint_changed = (SFVec3f) getEventOut ("armandjoint_changed");

		isactive = false;
	}

	public void processEvent (Event e)
	{
		String ename = e.getName ();

		if (ename.equals ("set_active"))
		{
			isactive = ((ConstSFBool) e.getValue ()).getValue ();
			if (isactive)
				addRange.setValue (torsorange);
			else
			{
				removeRange.setValue (torsorange);
				enableMe.setValue (true);
				disableMe.setValue (false);
			}
		}
		else if (ename.equals ("set_position"))
  	{
			set_position ((ConstSFVec3f) e.getValue ());
		}
	}

	public void set_position (ConstSFVec3f pos)
	{
		if (!isactive) return;

		float position[] = new float[3];
		float original[] = new float[3];

		pos.getValue (position);
		pos.getValue (original);

    double rangeangle = Math.atan2 (position[0], position[2]);
    range_changed.setValue (0.0f, 1.0f, 0.0f, (float) rangeangle);

		position[0] += shoulderLength;
		position[1] -= torsoLength;

		float temp = position[0];
		position[0] = position[2];
		position[2] = -temp;

    double length = Math.sqrt (Math.pow (position[0], 2) + Math.pow (position[1], 2) + Math.pow (position[2], 2));
    if (length <= armLength)
    {			
  		arm_changed.setValue (position);
      shoulder_changed.setValue (0.0f, 1.0f, 0.0f, 0.0f);
    }
    else if (length > armLength)
    {
      //  Look at projection into XZ plane
      double projlength = Math.sqrt (Math.pow (original[0], 2) + Math.pow (original[2], 2));
      double armproj = Math.sqrt (Math.pow (armLength, 2) - Math.pow (position[1], 2));

      if (projlength <= (armproj + Math.abs (shoulderLength)))
      {
        double jangle = Math.acos ((Math.pow (projlength, 2) - Math.pow (armproj, 2) - Math.pow (shoulderLength, 2))/(2*armproj*shoulderLength));
        double sanglea = Math.atan2 (Math.abs (original[0]), original[2]);
        double sangleb = Math.asin (armproj * Math.sin (jangle) / projlength);
        double sangle = (Math.PI/2) - (sanglea + sangleb);
				
				if (sangle > (Math.PI/3)) sangle = Math.PI/3;
				if (sangle < (-Math.PI/3)) sangle = -Math.PI/3;

				if (shoulderLength < 0) sangle = -sangle;
        shoulder_changed.setValue (0.0f, 1.0f, 0.0f, (float) (sangle));

        position[2] = (float) (original[2]*Math.cos (-sangle) - original[0]*Math.sin (-sangle));
        position[0] = (float) (original[2]*Math.sin (-sangle) + original[0]*Math.cos (-sangle)) + shoulderLength;
				temp = position[0];
				position[0] = position[2];
				position[2] = -temp;
						
        armandjoint_changed.setValue (position);
      }
    }
	}
}