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

public class FollowFish extends Script
{
	// Define all the eventOuts and fields as class variables.
	//private SFFloat initialOffsetDistance;
	//private SFFloat initialOffsetDepth;
	//private SFInt32 decisionTick;
	private SFNode follower;
	private SFNode followee;
	//private SFFloat distanceBegin;
	//private SFFloat distanceEnd;
	//private SFFloat velocityRange;
	//private SFFloat velocity;
	
	private SFVec3f translationFollowee;
	private SFVec3f translationFollower;

	// Event Out
	private SFVec3f translation_changed;
	private SFRotation rotation_changed;

	// Internal Variables
	private int tick;
	private int decisionTick;
	private float distanceBegin,distanceEnd,velocityDelta,velocityChange,velocity,radius,velocityMax;
	private double lastTime;

	// States
	final static int IDLE = 0;
	final static int SPEED_UP = 1;
	final static int SPEED_DOWN = 2;

	private int state;

	// This method is called when the script is loaded
	public void initialize()
	{
		float initialOffsetDistance,initialOffsetDepth;

		// Get handles to eventOuts and fields from the VRML environment
		state = IDLE;
		tick = 0;
		initialOffsetDistance = ((SFFloat) getField("initialOffsetDistance")).getValue();
		initialOffsetDepth = ((SFFloat) getField("initialOffsetDepth")).getValue();
		decisionTick = ((SFInt32) getField("decisionTick")).getValue();
		follower = (SFNode) getField("follower");
		followee = (SFNode) getField("followee");
		distanceBegin = ((SFFloat) getField("distanceBegin")).getValue();
		distanceEnd = ((SFFloat) getField("distanceEnd")).getValue();
		velocityDelta = ((SFFloat) getField("velocityDelta")).getValue();
		velocity = ((SFFloat) getField("velocity")).getValue();
		radius = ((SFFloat) getField("radius")).getValue();
		lastTime = 0.0f;
		velocityChange = 0.0f;
		velocityMax = velocity;

		translation_changed = (SFVec3f) getEventOut("translation_changed");
		rotation_changed = (SFRotation) getEventOut("rotation_changed");

		translationFollowee = (SFVec3f)((Node) followee.getValue()).getExposedField("translation");
		translationFollower = (SFVec3f)((Node) follower.getValue()).getExposedField("translation");
		float x,y,z;
		x = radius*((float)Math.sin(initialOffsetDistance));
		y = translationFollowee.getY()+initialOffsetDepth;
		z = radius*((float)Math.cos(initialOffsetDistance));
		translation_changed.setValue(x,y,z);
	}

	// Declare eventIn handlers as private methods
	private void set_time(double currentTime)
	{
		if (lastTime == 0.0)
			lastTime = currentTime;
		else
		{
			float dt = (float)Math.abs(currentTime-lastTime);
			float x0,y0,z0,x1,y1,z1,dx,dy,dz,length,degree;
			
			x0 = translationFollower.getX();
			y0 = translationFollower.getY();
			z0 = translationFollower.getZ();
			x1 = translationFollowee.getX();
			y1 = translationFollowee.getY();
			z1 = translationFollowee.getZ();

			dx = x1-x0;
			dy = y1-y0;
			dz = z1-z0;
			length = (float)Math.sqrt(dx*dx+dy*dy+dz*dz);


			if (length < 4.0)
			{
				velocity = 0;
				state = IDLE;
			}
			else
			if (length > distanceEnd)
			{
				if (state == IDLE)
				{
					velocity = velocity+velocityDelta;
				}
				else
				if (state == SPEED_UP)
				{
					velocity = velocity+velocityDelta;
				}
				else
				if (state == SPEED_DOWN)
				{
					velocity = velocity+velocityDelta;
				}
				state = SPEED_UP;
			}
			else
			if (length < distanceBegin)
			{
				if (state == IDLE)
				{
					velocity = velocity-velocityDelta;
				}
				else
				if (state == SPEED_UP)
				{
					velocity = 0.5f;//velocity-velocityDelta;
				}
				else
				if (state == SPEED_DOWN)
				{
					velocity = velocity-velocityDelta;
				}
				state = SPEED_DOWN;
			}
			else
			{
				/*
				if (state == IDLE)
				{
					velocityChange = 0;
				}
				else
				if (state == SPEED_UP)
				{
					velocity = velocity-velocityDelta;
				}
				else
				if (state == SPEED_DOWN)
				{
					velocity = velocity-velocityDelta;
				}
				*/
				state = IDLE;
			}			

			if (velocity < 0)
				velocity = 0;
			if (velocity > velocityMax)
				velocity = velocityMax;

			degree = findDegree(x1,z1,x0,z0);
			rotation_changed.setValue(0.0f,1.0f,0.0f,degree*((float)Math.PI)/180.0f);
			translation_changed.setValue(x0+dx*velocity*dt/length,y0+dy*velocity*dt/length,z0+dz*velocity*dt/length);
			lastTime = currentTime;
		}
	}
	
	// useful function to find the rotation of the fish
    public static float findDegree
    (float startX,float startY,float endX,float endY)
    {
	    float dX,dY,tAngle;

	    dX = startX - endX;
	    dY = startY - endY;


	    if (dX == 0.0f)
	    {
	        if (dY > 0.0f)
	            tAngle = 90.0f;
	        else
	            tAngle = 270.0f;
	    }
	    else
	    if (dY == 0.0f)
	    {
	        if (dX > 0.0f)
	            tAngle = 180.0f;
	        else
	            tAngle = 0.0f;
	    }
	    else
	    {
	        tAngle = (float)Math.atan(Math.abs(dY/dX))*180.0f/((float)Math.PI);

	        if ( (dX < 0.0f) && (dY < 0.0f) )
	        {
	            tAngle = 360.0f-tAngle;
	        }
	        else
	        if ( (dX > 0.0f) && (dY < 0.0f) )
	        {
	            tAngle = 180.0f+tAngle;
	        }
	        else
	        if ( (dX > 0.0f) && (dY > 0.0f) )
	        {
	            tAngle = 180.0f-tAngle;
	        }
	    }

	    return tAngle;
    }

	// In Java, you have to explicitly handle each event using the
	// processEvent method. Use it to call a private method for each event. 
	public void processEvent (Event e) 
	{
		String EventName = e.getName();
	
		if (EventName.equals("set_time"))
			set_time(((ConstSFTime)e.getValue()).getValue());
	}
}