import java.util.*;

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

public class LegKinematics extends Script
{
  protected MFNode legrange;
  protected float trackpoint[], thigh_length, shin_length, foot_length;
  protected SFRotation joint_changed, thigh_changed, shin_changed, foot_changed;
  protected MFNode addRange, removeRange;

  protected double jointangle;
	protected boolean isactive;

  protected void set_translation (ConstSFVec3f sftrans)
  {
    if (!isactive) return;

    float trans[] = new float[3];
    sftrans.getValue (trans);

    if (Math.abs(trans[1]) < 0.1f)
    {
      jointangle = Math.atan2 (trans[2], -trans[0]);
      if (jointangle > (Math.PI/3)) jointangle = Math.PI/3;
      if (jointangle < -(Math.PI/3)) jointangle = -Math.PI/3;
      joint_changed.setValue (0.0f, 1.0f, 0.0f, (float) jointangle);
    }
    else
    {
      float temp = trans[0];
      trans[0] = -trans[1];
      trans[1] = (float) (temp*Math.cos (jointangle) - trans[2]*Math.sin(jointangle));

      double zlength = Math.abs (trans[1]) * Math.tan (jointangle);

      float length = (float) Math.sqrt (trans[0] * trans[0] + trans[1] * trans[1] + trans[2]*trans[2]);

      double thighangle=0;

      if (length <= (thigh_length + Math.sqrt (shin_length*shin_length + foot_length*foot_length)))
      {
        foot_changed.setValue (0.0f, 1.0f, 0.0f, 0.0f);

        double comblength = Math.sqrt (shin_length*shin_length + foot_length*foot_length);
        double combangle = Math.acos ((length*length - thigh_length*thigh_length - comblength*comblength) / (2 * thigh_length * comblength));

        double shinangle = combangle + Math.atan2 (foot_length, shin_length);
        shin_changed.setValue (0.0f, 1.0f, 0.0f, (float) shinangle);

        double thighanglea = -Math.atan2 (trans[0], -trans[1]);
        double thighangleb = -Math.asin (comblength * Math.sin (combangle) / length);

        thighangle = thighanglea + thighangleb;
      }
      else if (length <= (thigh_length + shin_length + foot_length))
      {
        thighangle = -Math.atan2 (trans[0], -trans[1]);
        length -= thigh_length;

        double footangle = Math.acos ((length*length - shin_length*shin_length - foot_length*foot_length) / (2 * shin_length * foot_length));
        double shinangle = Math.asin (foot_length * Math.sin (footangle) / length);

        shin_changed.setValue (0.0f, 1.0f, 0.0f, (float) shinangle);

        footangle = Math.PI/2 - footangle;
        foot_changed.setValue (0.0f, 1.0f, 0.0f, (float) footangle);
      }
      else
      {
        thighangle = -Math.atan2 (trans[0], -trans[1]);
        shin_changed.setValue (0.0f, 1.0f, 0.0f, 0.0f);
        foot_changed.setValue (0.0f, 1.0f, 0.0f, (float) Math.PI/2);
      }

      if (thighangle > (Math.PI / 3))
        thighangle = Math.PI/3;
      else if (thighangle < (-2*Math.PI/3))
        thighangle = -2*Math.PI/3;

      thigh_changed.setValue (0.0f, 1.0f, 0.0f, (float) thighangle);
    }
  }

  public void initialize ()
  {
    trackpoint = new float[3];
    jointangle = 0;

    legrange = (MFNode) getField ("legrange");

    thigh_length = ((SFFloat) getField ("thigh_length")).getValue ();
    shin_length = ((SFFloat) getField ("shin_length")).getValue ();
    foot_length = ((SFFloat) getField ("foot_length")).getValue ();

    joint_changed = (SFRotation) getEventOut ("joint_changed");
    thigh_changed = (SFRotation) getEventOut ("thigh_changed");
    shin_changed = (SFRotation) getEventOut ("shin_changed");
    foot_changed = (SFRotation) getEventOut ("foot_changed");

    addRange = (MFNode) getEventOut ("addRange");
    removeRange = (MFNode) getEventOut ("removeRange");
  }

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

		if (eventname.equals ("set_trackpoint"))
    {
      set_translation ((ConstSFVec3f) e.getValue ());
      ((ConstSFVec3f) e.getValue ()).getValue (trackpoint);
    }
    else if (eventname.equals ("set_active"))
    {
      isactive = ((ConstSFBool) e.getValue ()).getValue ();

      if (isactive)
      {
        addRange.setValue (legrange);
      }
      else
      {
        removeRange.setValue (legrange);
      }
    }
  }
}