import vrml.*;
import vrml.node.*;
import vrml.field.*;
import java.lang.Math;
import java.util.*;

// 0 turn angle = +y direction (actually +z in VRML coords)

public class bug extends Script
{
    private double lasttime;

    private SFVec3f target;
    private SFRotation legs[];
    private SFVec3f translation;
    private SFRotation bugrotation;

    private float targetpos[] = { 0, 0, 0 };
    private float position[] = { -12, 0, -12 };
    private float velocity[] = { 0, 0, 1 };
    private float acceleration[] = { 0, 0, 0 };

    private boolean isOdd;
    private float movingFeet, stillFeet;

    public void initialize( )
    {
	lasttime = 0;

	target = (SFVec3f)getEventOut( "target" );
	translation = (SFVec3f)getEventOut( "translation" );
	bugrotation = (SFRotation)getEventOut( "bugrotation" );
	
	legs = new SFRotation[6];
	legs[0] = (SFRotation)getEventOut( "lega" );
	legs[1] = (SFRotation)getEventOut( "legb" );
	legs[2] = (SFRotation)getEventOut( "legc" );
	legs[3] = (SFRotation)getEventOut( "legd" );
	legs[4] = (SFRotation)getEventOut( "lege" );
	legs[5] = (SFRotation)getEventOut( "legf" );
	
	movingFeet = 0;
	stillFeet = 0;
	isOdd = true;
    }


    // save as getElevation but for an arbitrary point
    private float getElevationPt( float xpt, float ypt )
    {
	int i1x, i1y, i2x, i2y, i3x, i3y;
	i1x = (int)Math.floor( xpt + 16 );
	i1y = (int)Math.floor( ypt + 16 );
	i2x = i1x + 1;
	i2y = i1y + 1;

	float xleft = xpt + 16 - i1x;
	float yleft = ypt + 16- i1y;

	if( xleft > yleft )
	    {   // upper right
		i3x = i2x;
		i3y = i1y;

		float vchg = terrain[i2x][i2y] - terrain[i3x][i3y];
		float xchg = terrain[i1x][i1y] - terrain[i3x][i3y];

		return terrain[i3x][i3y] + vchg*yleft + xchg*(1-xleft);
	    }
	else
	    {   // lower left
		i3x = i1x;
		i3y = i2y;

		float vchg = terrain[i1x][i1y] - terrain[i3x][i3y];
		float xchg = terrain[i2x][i2y] - terrain[i3x][i3y];

		return terrain[i3x][i3y] + vchg*(1-yleft) + xchg*xleft;
	    }
    }


    private void printpoint( float pt[] )
    {
	System.out.println( "point:("+pt[0]+","+pt[2]+","+pt[1] + ")" );
    }


    private void locateBody( float t )
    {
	float scale = 1f;
	float pull[] = { scale * (position[0]-targetpos[0]), 0,
			 scale * (position[2]-targetpos[2]) };
	float pullamount= (float)Math.sqrt( pull[0]*pull[0] + pull[2]*pull[2] );

	// make it accelerate & damp it
		acceleration[0] -= pull[0];
	acceleration[2] -= pull[2];
	acceleration[0] *= 0.7;
	acceleration[2] *= 0.7;

	//System.out.println( "-----" );
	//printpoint( acceleration );

	velocity[0] = acceleration[0] * t;
	velocity[2] = acceleration[2] * t;

	//printpoint( velocity );

	position[0] += velocity[0] * t;
	position[1] = getElevationPt( position[0], position[2] ) + 0.4f;
	position[2] += velocity[2] * t;

	//printpoint( position );
	
	//	npos[1] = position[1];
        translation.setValue( position );

	//float rot[] = { 0, 1, 0, -(float)Math.atan( velocity[2]/velocity[0] )+3.14f };
	//	if( velocity[0] == 0f )
	//	    rot[2] *= -1;
	float id[] = { 1, 0, 0 };
	float rot[] = { 0, 1, 0, -anglebetween( id, velocity ) + 3.14f };

	bugrotation.setValue( rot );
    }


    private float anglebetween( float v1[], float v2[] )
    {
	return (float)Math.acos(
	    ( v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2] ) /
	    ( Math.sqrt( v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2] ) *
	      Math.sqrt( v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2] ) ) );
    }

    // assumes budy is already located
    // leg is aligned on line connecting center of bug to foot target
    private void locateLeg( int num )
    {
	
    }


    private void timer( )
    {
	double curtime = (double)(System.currentTimeMillis() / 1000.0);
	double t;
	if( lasttime == 0 )
	    t = 0.01f;
	else
	    t = curtime - lasttime;
	lasttime = curtime;

	locateBody( (float)t );

	if( movingFeet > 0.5 )
	    {
		isOdd = !isOdd;
		movingFeet = -0.5f;
		stillFeet = 0.5f;
	    }

	float rotm[] = { 0, 1, 0, movingFeet };
	float rots[] = { 0, 1, 0, stillFeet };
	
	if( isOdd )
	    {
		legs[0].setValue( rots );
		legs[2].setValue( rots );
		legs[4].setValue( rots );
		legs[1].setValue( rotm );
		legs[3].setValue( rotm );
		legs[5].setValue( rotm );
	    }
	else
	    {
		legs[0].setValue( rotm );
		legs[2].setValue( rotm );
		legs[4].setValue( rotm );
		legs[1].setValue( rots );
		legs[3].setValue( rots );
		legs[5].setValue( rots );
	    }
	float speed = (float)Math.sqrt( velocity[0]*velocity[0] +
					velocity[1]*velocity[1] +
					velocity[2]*velocity[2] );
	movingFeet += speed*t*2;
	stillFeet -= speed*t;
    }


    public void processEvent( Event e ) 
    {
	String EventName = e.getName( );
     
	if( EventName.equals( "setfraction" ) )
	    timer( );
	else if( EventName.equals( "target" ) )
       	    ((ConstSFVec3f)e.getValue()).getValue( targetpos );
    }

    float terrain[][] =
    {
{ 0.30f, 0.74f, 0.65f, 0.77f, 0.34f, 0.94f, 0.60f, 0.80f, 0.23f, 0.66f, 0.71f, 0.77f, 0.29f, 0.87f, 0.49f, 0.60f, 0.17f, 0.41f, 0.36f, 0.61f, 0.44f, 0.78f, 0.73f, 0.91f, 0.32f, 0.43f, 0.49f, 0.55f, 0.19f, 0.71f, 0.43f, 0.55f, 0.00f },
{ 0.78f, 1.07f, 1.07f, 0.87f, 0.90f, 0.87f, 1.12f, 0.74f, 0.80f, 0.77f, 0.93f, 0.74f, 0.58f, 0.68f, 0.76f, 0.84f, 0.41f, 0.55f, 1.07f, 0.76f, 1.17f, 1.18f, 1.05f, 0.90f, 0.67f, 0.98f, 0.97f, 0.87f, 0.98f, 0.68f, 0.82f, 0.42f, 0.38f },
{ 0.52f, 1.13f, 0.85f, 0.89f, 0.87f, 1.32f, 0.92f, 1.15f, 0.64f, 0.66f, 0.59f, 0.76f, 0.81f, 0.81f, 0.77f, 1.26f, 0.27f, 0.69f, 0.82f, 0.77f, 0.93f, 1.36f, 0.91f, 1.22f, 0.88f, 1.22f, 0.77f, 0.65f, 0.78f, 0.76f, 0.45f, 0.78f, 0.35f },
{ 0.84f, 0.78f, 1.21f, 0.95f, 1.06f, 1.09f, 1.00f, 0.92f, 0.89f, 0.92f, 1.12f, 1.01f, 0.74f, 0.77f, 1.13f, 1.31f, 0.51f, 0.98f, 0.76f, 0.66f, 0.74f, 1.21f, 1.16f, 0.88f, 0.87f, 1.29f, 1.26f, 1.02f, 0.68f, 1.12f, 0.70f, 0.72f, 0.88f },
{ 0.61f, 0.70f, 0.75f, 0.91f, 0.59f, 0.99f, 0.83f, 1.10f, 0.64f, 0.86f, 0.61f, 1.01f, 0.54f, 0.90f, 0.95f, 1.17f, 0.37f, 0.96f, 0.85f, 0.81f, 0.50f, 0.85f, 0.71f, 1.19f, 0.83f, 1.18f, 1.10f, 0.86f, 0.50f, 1.07f, 0.64f, 0.76f, 0.62f },
    { 1.12f, 1.05f, 1.34f, 1.20f, 0.78f, 1.30f, 1.32f, 1.20f, 0.99f, 1.27f, 1.22f, 1.03f, 1.08f, 1.10f, 0.95f, 0.90f, 0.90f, 0.69f, 0.89f, 1.07f, 0.85f, 1.16f, 1.21f, 0.79f, 1.36f, 1.14f, 1.32f, 1.23f, 0.96f, 1.01f, 1.03f, 0.97f, 0.76f },
    { 0.69f, 0.99f, 1.11f, 1.31f, 0.83f, 1.41f, 1.00f, 1.31f, 0.91f, 1.12f, 0.95f, 0.95f, 0.99f, 1.10f, 0.80f, 1.30f, 0.46f, 1.14f, 0.85f, 1.08f, 0.81f, 0.86f, 0.77f, 1.14f, 0.89f, 1.22f, 1.17f, 1.32f, 1.15f, 1.16f, 0.70f, 0.98f, 0.89f },
    { 1.05f, 0.82f, 1.37f, 1.31f, 1.24f, 1.27f, 1.05f, 0.86f, 1.02f, 1.31f, 1.05f, 1.05f, 1.17f, 1.11f, 1.45f, 1.34f, 0.95f, 0.89f, 0.89f, 1.17f, 0.85f, 1.16f, 0.93f, 1.11f, 1.23f, 1.34f, 1.33f, 1.22f, 1.10f, 1.15f, 1.06f, 0.97f, 1.04f },
    { 0.65f, 1.27f, 0.96f, 1.09f, 0.71f, 1.19f, 1.00f, 1.31f, 0.69f, 1.03f, 1.16f, 1.31f, 1.06f, 1.28f, 1.04f, 0.94f, 0.49f, 0.88f, 0.98f, 0.95f, 0.71f, 1.18f, 0.74f, 0.75f, 0.59f, 1.13f, 0.72f, 0.99f, 0.61f, 0.87f, 0.96f, 0.86f, 0.63f },
{ 1.11f, 1.05f, 1.05f, 1.22f, 1.32f, 1.34f, 1.22f, 1.00f, 1.07f, 1.44f, 1.63f, 1.55f, 1.18f, 1.21f, 1.63f, 1.43f, 1.09f, 0.98f, 0.93f, 1.25f, 1.07f, 1.32f, 1.19f, 1.13f, 0.95f, 1.27f, 1.36f, 1.05f, 1.04f, 1.11f, 1.10f, 1.28f, 1.25f },
{ 1.05f, 1.38f, 0.96f, 1.12f, 1.24f, 1.21f, 1.02f, 1.31f, 0.99f, 1.42f, 1.19f, 1.58f, 1.15f, 1.41f, 1.28f, 1.22f, 0.78f, 0.85f, 0.80f, 1.14f, 1.16f, 1.60f, 1.12f, 1.21f, 1.05f, 1.26f, 1.15f, 1.13f, 0.94f, 1.08f, 1.21f, 1.50f, 1.29f },
{ 1.15f, 1.46f, 1.30f, 1.14f, 1.19f, 1.38f, 1.37f, 1.60f, 0.89f, 1.50f, 1.43f, 1.52f, 1.57f, 1.43f, 1.70f, 1.24f, 1.12f, 0.89f, 1.32f, 0.91f, 1.38f, 1.33f, 1.22f, 1.31f, 1.40f, 1.28f, 1.38f, 1.17f, 1.27f, 1.52f, 1.41f, 1.38f, 1.41f },
{ 1.10f, 1.62f, 1.21f, 1.43f, 1.01f, 1.61f, 1.30f, 1.14f, 0.74f, 1.28f, 1.19f, 1.36f, 1.10f, 1.69f, 1.38f, 1.43f, 0.89f, 1.14f, 1.01f, 1.19f, 0.75f, 1.08f, 1.13f, 1.26f, 1.04f, 1.09f, 1.00f, 1.27f, 0.81f, 1.10f, 1.10f, 1.23f, 0.99f },
{ 1.53f, 1.36f, 1.60f, 1.61f, 1.18f, 1.27f, 1.35f, 1.16f, 1.09f, 1.41f, 1.35f, 1.38f, 1.40f, 1.30f, 1.71f, 1.40f, 1.34f, 1.32f, 1.30f, 1.20f, 1.43f, 1.16f, 1.54f, 1.59f, 1.48f, 1.51f, 1.33f, 1.21f, 1.14f, 1.10f, 1.32f, 1.11f, 1.28f },
{ 0.98f, 1.25f, 1.26f, 1.30f, 0.89f, 0.94f, 0.92f, 1.23f, 1.14f, 1.55f, 1.12f, 1.68f, 1.05f, 1.69f, 1.36f, 1.45f, 0.97f, 1.03f, 1.05f, 1.21f, 1.23f, 1.49f, 1.25f, 1.78f, 1.27f, 1.48f, 1.16f, 1.16f, 0.96f, 1.42f, 1.02f, 1.40f, 0.94f },
{ 0.97f, 1.15f, 1.17f, 1.23f, 0.93f, 0.90f, 1.10f, 1.22f, 1.13f, 1.17f, 1.53f, 1.37f, 1.32f, 1.18f, 1.18f, 1.39f, 0.85f, 1.28f, 1.21f, 1.59f, 1.49f, 1.68f, 1.68f, 1.59f, 1.57f, 1.45f, 1.39f, 1.51f, 1.17f, 1.18f, 1.31f, 1.24f, 0.81f },
{ 0.60f, 1.08f, 1.26f, 1.46f, 1.16f, 1.33f, 1.45f, 1.46f, 0.87f, 1.21f, 1.27f, 1.28f, 1.13f, 1.37f, 1.06f, 1.07f, 0.57f, 0.78f, 0.91f, 1.41f, 0.97f, 1.18f, 1.00f, 1.40f, 0.92f, 1.34f, 1.13f, 1.45f, 0.90f, 1.51f, 1.12f, 1.29f, 0.48f },
{ 1.00f, 1.39f, 1.28f, 1.60f, 1.32f, 1.66f, 1.63f, 1.43f, 1.16f, 1.53f, 1.63f, 1.56f, 1.52f, 1.25f, 1.36f, 1.02f, 1.01f, 1.23f, 1.58f, 1.56f, 1.50f, 1.40f, 1.35f, 1.24f, 1.13f, 1.34f, 1.48f, 1.37f, 1.37f, 1.08f, 1.35f, 1.34f, 0.83f },
{ 1.00f, 1.06f, 1.12f, 1.54f, 1.40f, 1.83f, 1.35f, 1.46f, 1.32f, 1.39f, 1.24f, 1.63f, 1.18f, 1.61f, 1.18f, 1.61f, 1.18f, 1.65f, 1.33f, 1.26f, 1.38f, 1.41f, 1.23f, 1.66f, 1.31f, 1.29f, 1.06f, 1.48f, 1.21f, 1.42f, 0.91f, 1.37f, 0.84f },
{ 1.09f, 1.34f, 1.61f, 1.48f, 1.29f, 1.47f, 1.41f, 1.45f, 1.44f, 1.61f, 1.35f, 1.25f, 1.30f, 1.47f, 1.50f, 1.43f, 1.16f, 1.53f, 1.81f, 1.41f, 1.46f, 1.46f, 1.41f, 1.62f, 1.19f, 1.50f, 1.50f, 1.49f, 1.62f, 1.18f, 1.18f, 1.18f, 1.18f },
{ 0.97f, 1.59f, 1.38f, 1.22f, 0.89f, 1.38f, 1.12f, 1.47f, 0.89f, 1.16f, 1.11f, 1.36f, 0.92f, 1.23f, 1.00f, 1.30f, 0.90f, 1.45f, 1.32f, 1.41f, 1.21f, 1.44f, 1.21f, 1.38f, 0.95f, 1.04f, 1.09f, 1.16f, 1.05f, 1.17f, 0.95f, 1.28f, 0.68f },
{ 1.51f, 1.23f, 1.44f, 1.25f, 1.47f, 1.47f, 1.31f, 1.51f, 1.07f, 1.16f, 1.47f, 1.45f, 1.14f, 1.31f, 1.48f, 1.24f, 1.47f, 1.63f, 1.55f, 1.55f, 1.90f, 1.54f, 1.60f, 1.61f, 1.19f, 1.12f, 1.42f, 1.67f, 1.44f, 1.56f, 1.44f, 1.36f, 0.93f },
{ 1.08f, 1.23f, 1.10f, 1.45f, 1.33f, 1.36f, 1.24f, 1.45f, 1.23f, 1.49f, 1.15f, 1.34f, 1.24f, 1.61f, 1.29f, 1.49f, 1.26f, 1.53f, 1.23f, 1.46f, 1.62f, 1.78f, 1.34f, 1.37f, 1.02f, 1.50f, 1.09f, 1.72f, 1.37f, 1.48f, 1.20f, 1.40f, 0.93f },
{ 1.40f, 1.10f, 1.10f, 1.27f, 1.38f, 1.15f, 1.20f, 1.26f, 1.36f, 1.35f, 1.10f, 1.49f, 1.43f, 1.45f, 1.18f, 1.12f, 1.53f, 1.56f, 1.46f, 1.75f, 1.51f, 1.58f, 1.54f, 1.29f, 1.20f, 1.44f, 1.34f, 1.74f, 1.53f, 1.35f, 1.30f, 1.22f, 1.12f },
{ 0.75f, 1.00f, 0.93f, 1.20f, 0.85f, 1.22f, 1.04f, 1.21f, 0.72f, 1.46f, 1.27f, 1.55f, 1.22f, 1.36f, 1.33f, 1.31f, 0.92f, 1.63f, 1.38f, 1.29f, 0.92f, 1.56f, 1.21f, 1.26f, 0.84f, 1.27f, 0.94f, 0.98f, 0.92f, 1.33f, 0.86f, 1.13f, 0.45f },
{ 0.81f, 1.30f, 1.07f, 1.14f, 1.37f, 1.24f, 1.44f, 1.32f, 1.36f, 1.42f, 1.44f, 1.63f, 1.50f, 1.70f, 1.50f, 1.28f, 1.57f, 1.22f, 1.46f, 1.40f, 1.48f, 1.60f, 1.19f, 1.22f, 0.93f, 1.23f, 1.51f, 1.49f, 1.26f, 1.27f, 1.23f, 0.74f, 0.86f },
{ 0.75f, 0.97f, 1.07f, 1.36f, 1.04f, 1.34f, 0.96f, 0.95f, 1.00f, 1.15f, 1.26f, 1.88f, 1.55f, 1.52f, 1.44f, 1.77f, 1.38f, 1.63f, 1.04f, 1.22f, 1.30f, 1.58f, 1.02f, 1.48f, 1.01f, 1.45f, 1.17f, 1.20f, 1.33f, 1.46f, 1.05f, 1.01f, 0.46f },
{ 0.81f, 1.25f, 1.39f, 1.02f, 0.93f, 1.30f, 1.36f, 1.02f, 1.21f, 1.27f, 1.69f, 1.49f, 1.38f, 1.43f, 1.62f, 1.10f, 1.79f, 1.39f, 1.19f, 1.10f, 1.49f, 1.26f, 1.26f, 1.22f, 0.98f, 1.02f, 1.42f, 1.15f, 1.42f, 1.30f, 1.16f, 0.82f, 0.60f },
{ 0.71f, 1.09f, 0.97f, 1.15f, 0.65f, 1.02f, 0.97f, 1.03f, 0.89f, 1.40f, 1.24f, 1.54f, 1.07f, 1.49f, 1.06f, 1.20f, 1.25f, 1.68f, 1.22f, 1.22f, 0.80f, 1.14f, 0.95f, 1.07f, 0.91f, 1.01f, 1.02f, 1.10f, 0.92f, 0.87f, 0.75f, 0.67f, 0.42f },
{ 1.21f, 0.83f, 1.06f, 1.26f, 1.25f, 1.30f, 1.27f, 1.24f, 1.38f, 1.60f, 1.51f, 1.59f, 1.28f, 1.21f, 1.52f, 1.46f, 1.82f, 1.65f, 1.29f, 1.24f, 0.95f, 1.37f, 0.96f, 1.22f, 0.98f, 1.20f, 1.38f, 1.26f, 1.43f, 1.35f, 1.05f, 0.64f, 0.75f },
{ 0.93f, 1.12f, 0.71f, 1.26f, 0.96f, 1.34f, 1.21f, 1.39f, 1.12f, 1.52f, 1.20f, 1.55f, 1.23f, 1.22f, 1.14f, 1.31f, 1.39f, 1.46f, 1.32f, 1.53f, 0.96f, 1.22f, 0.88f, 1.27f, 0.84f, 1.28f, 1.23f, 1.42f, 1.06f, 1.37f, 0.77f, 1.09f, 0.61f },
{ 0.79f, 1.11f, 1.13f, 1.14f, 0.94f, 1.48f, 1.44f, 1.44f, 1.17f, 1.04f, 1.21f, 1.49f, 1.27f, 1.55f, 1.16f, 1.41f, 1.13f, 1.46f, 1.64f, 1.54f, 1.07f, 1.21f, 1.14f, 1.26f, 1.03f, 1.35f, 1.38f, 1.45f, 0.89f, 0.98f, 0.99f, 0.86f, 0.61f },
{ 0.21f, 0.55f, 0.71f, 1.27f, 0.91f, 1.55f, 1.23f, 0.94f, 0.64f, 0.88f, 0.88f, 1.27f, 0.84f, 1.00f, 1.07f, 1.18f, 0.68f, 1.41f, 1.27f, 1.35f, 0.92f, 1.51f, 1.14f, 1.17f, 0.73f, 0.87f, 0.94f, 0.96f, 0.63f, 1.04f, 0.50f, 0.59f, 0.21f }

};

}