//
// Inverse Kinematics
//
// Fred Chen & Loren Tsai
// CS184 Fall 98

import vrml.*;
import vrml.node.*;
import vrml.field.*;
import java.io.*;
import java.util.*;

public class legInvKin extends Script {

	// fields
	private SFNode Leg;
	private SFFloat ThighLength;
	private SFFloat CalfLength;

	// eventOuts
	private SFVec3f position_changed;
	private SFRotation Up;
	private SFRotation Side;
	private SFRotation Out;
	private SFRotation Calf;

	// internal variables		
	private SFRotation init_up;
	private SFRotation init_side;
	private SFRotation init_out;
	private SFRotation init_calf;

	private boolean first_val = true;
	private float init_position[] = new float[3];
	private float last_position[] = new float[3];
	private float next_position[] = new float[3];
	private float l1;
	private float l2;
	private float l3;
	private double alpha;
	private double beta;
	private double phi;

	private double uprange[] = new double[2];
	private double siderange[] = new double[2];
	private double outrange[] = new double[2];
	private double calfrange[] = new double[2];

	private SFVec3f Path[];
	private int steps = 20;
	private int iter = 0;

	private float x;
	private float y;
	private float z;

	// This method is called when the script is loaded
	public void initialize()
	{

		// Get handles to fields from the VRML environment
		Leg = (SFNode) getField("Leg");
		ThighLength = (SFFloat) getField("ThighLength");
		CalfLength = (SFFloat) getField("CalfLength");

		// Get handles to eventOuts from the VRML environment
		position_changed = (SFVec3f) getEventOut("position_changed");
		Up = (SFRotation) getEventOut("Up");
		Side = (SFRotation) getEventOut("Side");
		Out = (SFRotation) getEventOut("Out");
		Calf = (SFRotation) getEventOut("Calf");

		// Initialize internal variables
		init_up = (SFRotation) ((Node) Leg.getValue()).getExposedField("ThighUp");
		init_side = (SFRotation) ((Node) Leg.getValue()).getExposedField("ThighSide");
		init_out = (SFRotation) ((Node) Leg.getValue()).getExposedField("ThighOut");
		init_calf = (SFRotation) ((Node) Leg.getValue()).getExposedField("CalfUp");
		
		l1 = ThighLength.getValue();
		l2 = CalfLength.getValue();
		l3 = l1+l2;

		for(int i=0;i<3;i++) {
			init_position[i] = 0;
		}

		for(int i=0;i<2;i++) {

			if (i==0) {
				uprange[i] = -1.57;
				siderange[i] = -1.3;
				outrange[i] = -0.78;
				calfrange[i] = 0;
			} 
			else if (i==1) {
				uprange[i] = 1.57;
				siderange[i] = 1.3;
				outrange[i] = 1.57;
				calfrange[i] = 2.6;
			}

		}

		// Initialize Path to walk
		/*Path = new SFVec3f[steps];
		float temp[] = new float[3];
		double angle = 2;
		double netHeight = 3;
		for(int i=0;i<steps;i++) {
		
			Path[i] = new SFVec3f();
			if (i<steps/4) {	
				temp[0] = 0;
				temp[1] = (float) (i*netHeight*4/steps);
				temp[2] = (float) (-(l1+l2)*Math.sin(4*angle*i/steps));
			}
			else if (i< (3*steps/4)) {
				temp[0] = 0;
				temp[1] = (float) netHeight;
				temp[2] = (float) (4*i*(l1+l2)*Math.sin(angle)/steps);
			}
			else if (i<steps) {
				temp[0] =0;
				temp[1] = (float) netHeight*(1 - ((4*i-3*steps)/steps));
				temp[2] = (float) (-(l1+l2)*Math.sin(4*angle*(1 - (4*i-3*steps)/steps)));
			}
			Path[i].setValue(temp);
		}*/

	}

	//eventIns
	private void set_position(ConstSFVec3f newpos) {
		if(first_val == true) {
			newpos.getValue(init_position);
			newpos.getValue(last_position);
			first_val = false;
		}
		else {
			newpos.getValue(next_position);
			for(int i=0;i<3;i++)
				last_position[i] = last_position[i] + next_position[i];
			x = next_position[0] - init_position[0];
			y = next_position[1] - init_position[1];
			z = next_position[2] - init_position[2];
			x = x/5;  // scale factor of Foot object
			y = y/5;
			z = z/5;
			l3 = (float) Math.sqrt((l1+l2-y)*(l1+l2-y) + x*x + z*z);
			//l3 = (float) Math.sqrt((l1+l2-y)*(l1+l2-y) + z*z);
			//l3 = (float) Math.sqrt(y*y + z*z);
			if ( (Math.abs(y) > l1+l2) ) {
				}
			else {
				beta = Math.acos((l3*l3 - l1*l1 - l2*l2)/(2*l1*l2));
				alpha = Math.atan((z+l2*Math.sin(beta))/(l1+l2*Math.cos(beta)));
				//if (z < 0)
					//alpha = -alpha;
				position_changed.setValue(next_position);
			}
		}
	}

	private void set_thighup(ConstSFVec3f newpos) {
		
		float temp[] = new float[4];
		
		if (alpha < uprange[0])
			alpha = uprange[0];
		else if (alpha > uprange[1])
			alpha = uprange[1];
		init_up.getValue(temp);
		temp[3] = (float) alpha;
		Up.setValue(temp);

	}

	private void set_thighout(ConstSFVec3f newpos) {
		
		l3 = (float) Math.sqrt(l1*l1 + l2*l2 + 2*l1*l2*Math.cos(beta));

		
	}

	private void set_thighside(ConstSFVec3f newpos) {

		float temp[] = new float[4];
		l3 = (float) Math.sqrt(l1*l1 + l2*l2 + 2*l1*l2*Math.cos(beta));

		phi = Math.atan(x/(l1+l2-y));
		if (phi < siderange[0])
			phi = siderange[0];
		else if (phi > siderange[1])
			phi = siderange[1];
		init_side.getValue(temp);
		temp[3] = (float) phi;
		Side.setValue(temp);	
	}
	
	private void set_calf(ConstSFVec3f newpos) {

		float temp[] = new float[4];		

		if (beta < calfrange[0])
			beta = calfrange[0];
		else if (beta > calfrange[1])
			beta = calfrange[1];
		init_calf.getValue(temp);
		temp[3] = (float) beta;
		Calf.setValue(temp);
	
	}

	private void nextStep(float frac) {
		
		float temp[] = new float[3];
		Path[iter].getValue(next_position);
		iter = iter+1;
		if (iter >= steps)
			iter = 0;
		position_changed.setValue(temp);
		

	}

	public void processEvent (Event e) {
		String EventName = e.getName();
		if (EventName.equals("set_position"))
			set_position((ConstSFVec3f)e.getValue());
		
		else if (EventName.equals("set_thighup"))
			set_thighup((ConstSFVec3f)e.getValue());

		else if (EventName.equals("set_thighout"))
			set_thighout((ConstSFVec3f)e.getValue());

		else if (EventName.equals("set_thighside"))
			set_thighside((ConstSFVec3f)e.getValue());

		else if (EventName.equals("set_calf"))
			set_calf((ConstSFVec3f)e.getValue());
		
		else if (EventName.equals("nextStep"))
			nextStep(((ConstSFFloat)e.getValue()).getValue());
		
	}

}