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

public class Robot extends Script
{
	//orientation
	final static int FORWARD = 0;
	final static int BACKWARD = 1;
	final static int LEFT = 2;
	final static int RIGHT = 3;
	final static float BOTROT = 0.157f;
	final static float BOTTRANS = 0.2f;
	final static int LIMIT = 100;
	final static float BOTROTBLOW = 0.05f;
	
	private int totalPoints = 0;
	private int orientation = FORWARD;

	//internal
	private float pathX[];
	private float pathY[];
	private int pathSize;
	private float convert[];
	private boolean weapon_avail[] = {true,true}; // {missle,laser}
	private int whichWeapon = 0;

	private int destx = 7;
	private int desty = 7;
	private int gridsMoved = 0;

	private float botRotTotal = 0f;
	private float botTransTotal = 0f;
	private float botRotUpper =0f;
	private float botRotLeft = 0f;
	private float botRotRight = 0f;

	private int searchGrid[];

	//fields
	private Node thisRobot;
	private MFNode otherRobot;
	private MFNode missleScript;
	private int grid[];
	private int gridXSize;
	private int gridYSize;
	private int currentX,currentY;
	private float range;
	private float missle_damage;
	private float laser_damage;

	//eventOut
	private SFVec3f missle1Fire;
	private SFVec3f missle2Fire;
	private SFVec3f laser1Fire;
	private SFVec3f laser2Fire;
	private SFFloat send_damage;

	/* Johnny's */
	// fields
	private String nodename;
	private float current_rotation[]={0,1,0,0};
	private float current_translation[]={0,0,0};

	// eventouts
	private SFRotation rotation_changed;
	private SFVec3f translation_changed;

	private SFRotation rightFirst_changed;
	private SFRotation rightSecond_changed;
	private SFRotation leftFirst_changed;
	private SFRotation leftSecond_changed;

	private SFRotation rightArmExplodeRotation;
	private SFRotation	leftArmExplodeRotation;
	private SFRotation upperExplodeRotation;
	private SFVec3f rightArmExplodeTranslation;
	private SFVec3f leftArmExplodeTranslation;
	private SFVec3f upperExplodeTranslation;

	private SFInt32 rightChoice_changed;
	private SFInt32 leftChoice_changed;
	private SFInt32 upperChoice_changed;
	
	// local vars
	private float generic[]={0,1,0,0};
	private float rightOne[]={0,1,0,0};
	private float rightTwo[]={0,1,0,0};
	private float leftOne[]={0,1,0,0};
	private float leftTwo[]={0,1,0,0};

	private float rightExplodeRot[]={0.85f,0.1f,0.15f,0};
	private float rightExplodeTrans[]={0,0,0};
	private float leftExplodeRot[]={0.85f,0.1f,0.15f,0};
	private float leftExplodeTrans[]={0,0,0};
	private float upperExplodeRot[]={0.5f,0.2f,0.3f,0};
	private float upperExplodeTrans[]={0,0,0};

	private boolean rightBlows=false;
	private boolean leftBlows=false;
	private boolean upperBlows=false;
	private boolean rightDoneBlowing=true;
	private boolean leftDoneBlowing=true;
	private boolean upperDoneBlowing=true;

	private boolean newFrameBegan = false;
	private float lastFrac = 0f;

	private boolean legsMoving = true;
	private boolean legsDoneMoving = false;

	private boolean moving = true;
	private boolean doneRotating = false;
	private boolean doneMoving = true;

	private int	rightHealth = 100;
	private int leftHealth = 100;
	private int mainHealth = 100;

	private Random arand;


	// This method is called when the script is loaded
	public void initialize()
	{
		System.out.println("Start init");

		float temp;
		int i;

		arand = new Random(System.currentTimeMillis()+293284);

		gridXSize = ((SFInt32)getField("gridXSize")).getValue();
		gridYSize = ((SFInt32)getField("gridYSize")).getValue();
	
		//internals
		pathX = new float[gridXSize*gridYSize];
		pathY = new float[gridXSize*gridYSize];
		grid = new int[gridXSize*gridYSize];
		searchGrid = new int[gridXSize*gridYSize];
		pathSize = 0;
		convert = new float[gridXSize];

		temp = (float)(1-gridXSize);
		for (i=0; i<gridXSize; i++)
		{
			convert[i] = temp;
			temp = temp+2.0f;
		}

		//fields
		thisRobot = (Node)((SFNode) getField("thisRobot")).getValue();
		otherRobot = (MFNode) getField("otherRobot");
		orientation = ((SFInt32) getField("orientation")).getValue();
		range = ((SFFloat) getField("range")).getValue();
		rightHealth = ((SFInt32) getField("rightHealth")).getValue();
		leftHealth = ((SFInt32) getField("leftHealth")).getValue();
		mainHealth = ((SFInt32) getField("mainHealth")).getValue();
		missle_damage = ((SFFloat) getField("missle_damage")).getValue();
		laser_damage = ((SFFloat) getField("laser_damage")).getValue();
		nodename = ((SFString) getField("nodename")).getValue();
		((MFInt32) getField("grid")).getValue(grid);
		((SFVec3f) thisRobot.getExposedField("translation")).getValue(current_translation);
		((SFRotation) thisRobot.getExposedField("rotation")).getValue(current_rotation);
		
		currentX = ((SFInt32) getField("currentX")).getValue();
		currentY = ((SFInt32) getField("currentY")).getValue();

		//eventouts
		missle1Fire = (SFVec3f) getEventOut("missle1Fire");
		missle2Fire = (SFVec3f) getEventOut("missle2Fire");
		laser1Fire = (SFVec3f) getEventOut("laser1Fire");
		laser2Fire = (SFVec3f) getEventOut("laser2Fire");
		send_damage = (SFFloat) getEventOut("send_damage");

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

		rightFirst_changed = (SFRotation) getEventOut("rightFirst_changed");
		rightSecond_changed = (SFRotation) getEventOut("rightSecond_changed");
		leftSecond_changed = (SFRotation) getEventOut("leftSecond_changed");
		leftFirst_changed = (SFRotation) getEventOut("leftFirst_changed");

		rightArmExplodeRotation = (SFRotation) getEventOut("rightArmExplodeRotation");
		leftArmExplodeRotation = (SFRotation) getEventOut("leftArmExplodeRotation");
		upperExplodeRotation = (SFRotation) getEventOut("upperExplodeRotation");
		rightArmExplodeTranslation = (SFVec3f) getEventOut("rightArmExplodeTranslation");
	    leftArmExplodeTranslation = (SFVec3f) getEventOut("leftArmExplodeTranslation");
		upperExplodeTranslation = (SFVec3f) getEventOut("upperExplodeTranslation");

		rightChoice_changed = (SFInt32) getEventOut("rightChoice_changed");
		leftChoice_changed = (SFInt32) getEventOut("leftChoice_changed");
		upperChoice_changed = (SFInt32) getEventOut("upperChoice_changed");

		startMovement();

		for(i=0; i<searchGrid.length; i++)
		{
			searchGrid[i] = grid[i];
		}
		searchGrid[currentX+currentY*gridXSize] = 0;

		destx = arand.nextInt()%8;
		desty = arand.nextInt()%8;
		
		while(!checkPath(searchGrid,destx,desty) || ((destx == currentX) && (desty == currentY)) )
		{
			destx = arand.nextInt()%8;
			desty = arand.nextInt()%8;
		}
		
		findPath(currentX,currentY,destx,desty,0);

		
		System.out.println("Finished init");
	}

	private void stopMovement()
	{
		moving = false;
		legsMoving = false;
		doneRotating = true;
	}

	private void startMovement()
	{
		moving = true;
		legsMoving = true;
		doneRotating = false;
	}

	private boolean checkPath(int[] g,int x,int y)
	{
		if ( (x < 0) || (y < 0) || (x >= gridXSize) || (y >= gridYSize))
			return false;
		if ( g[x+y*gridYSize] == 0)
			return false;
		return true;
	}

	private boolean findPath(int x, int y, int dX,int dY, int size)
	{
		if (x == dX && y == dY)
		{
			System.out.println("pathSize is "+pathSize);
			pathSize = size;
			return true;
		}
		else
		{
			//4 cases
			if (checkPath(searchGrid,x-1,y))
			{
				pathX[size] = convert[x-1];
				pathY[size] = convert[y];
				searchGrid[x-1+y*gridXSize] = 0;
				if (findPath(x-1,y,dX,dY,size+1))
					return true;
			}
			
			if (checkPath(searchGrid,x,y-1))
			{
				pathX[size] = convert[x];
				pathY[size] = convert[y-1];
				searchGrid[x+(y-1)*gridXSize] = 0;
				if (findPath(x,y-1,dX,dY,size+1))
					return true;
			}

			if (checkPath(searchGrid,x+1,y))
			{
				pathX[size] = convert[x+1];
				pathY[size] = convert[y];
				searchGrid[x+1+y*gridXSize] = 0;
				if (findPath(x+1,y,dX,dY,size+1))
					return true;
			}

			if (checkPath(searchGrid,x,y+1))
			{
				pathX[size] = convert[x];
				pathY[size] = convert[y+1];
				searchGrid[x+(y+1)*gridXSize] = 0;
				if (findPath(x,y+1,dX,dY,size+1))
					return true;
			}
		}
		return false;
	}

	private void missle_hit(ConstSFInt32 i)
	{
		startMovement();
		weapon_avail[0] = true;
		if (i.getValue() != -1)
		{
			send_damage.setValue(missle_damage);
		}
	}

	private void laser_hit(ConstSFInt32 i)
	{
		startMovement();
		weapon_avail[1] = true;
		if (i.getValue() != -1)
		{
			send_damage.setValue(laser_damage);
		}
	}

	private void receive_damage(ConstSFFloat f)
	{
		if( (arand.nextInt()%500) == 126)
			upperBlows = true;
	}

	private synchronized void set_fraction(ConstSFFloat temp)
	{
		float frac = temp.getValue();
		
		if( (legsMoving) || !(legsDoneMoving))
		{
			moveLegs(frac);
		}
		
		if(upperBlows)
		{
			rightBlows = true;
			leftBlows = true;
		}

		if((upperBlows) || !(upperDoneBlowing) )
		{
			upperDoneBlowing = false;
			blowUpupper();		// upper arm gone!
		}

		if((rightBlows) || !(rightDoneBlowing) )
		{
			rightDoneBlowing = false;
			blowUpRight();		// right arm gone!
		}

		if( (leftBlows) || !(leftDoneBlowing) )
		{
			leftDoneBlowing = false;
			blowUpLeft(); // left arm gone!
		}

	//	System.out.println(gridsMoved+" "+pathSize+" "+moving);
		if( (gridsMoved != pathSize) && (moving))
		{
			marchMech(frac);
		}
		else
		if (weapon_avail[0] && weapon_avail[1])
		{
			startMovement();
		}


		if(lastFrac > frac)
			newFrameBegan = true;
		else
			newFrameBegan = false;

		lastFrac = frac;
	}

	private boolean checkProximity()
	{
		SFVec3f t = (SFVec3f) thisRobot.getExposedField("translation");
		SFVec3f o = (SFVec3f) ((Node)otherRobot.get1Value(0)).getExposedField("translation");
		float x1 = t.getX();
		float y1 = t.getZ();
		float x2 = o.getX();
		float y2 = o.getZ();

		if( Math.sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)) < 1)
		{
			// BLOW UP!
			upperBlows=true;
			return true;
		}
		else
			return false;
	}

	private void checkToFire()
	{
		SFVec3f o = (SFVec3f) ((Node)otherRobot.get1Value(0)).getExposedField("translation");
		float x1 = current_translation[0];
		float y1 = current_translation[2];
		float x2 = o.getX();
		float y2 = o.getZ();

		if ( ((Math.abs(x1-x2) < 0.1) && (orientation == FORWARD) && ((y2-y1) > 0.1) && ((y2-y1) <= range)) || 
			 ((Math.abs(x1-x2) < 0.1) && (orientation == BACKWARD) && ((y1-y2) > 0.1) && ((y1-y2) <= range)) ||
			 ((Math.abs(y1-y2) < 0.1) && (orientation == LEFT) && ((x2-x1) > 0.1) && ((x2-x1) <= range)) ||
			 ((Math.abs(y1-y2) < 0.1) && (orientation == RIGHT) && ((x1-x2) > 0.1) && ((x1-x2) <= range)))
		{	
			if ( weapon_avail[0] && weapon_avail[1])
			{
				weapon_avail[0] = false;
				weapon_avail[1] = false;
				stopMovement();
				
				missle1Fire.setValue(o.getX(),o.getY(),o.getZ());
				laser2Fire.setValue(o.getX(),o.getY(),o.getZ());
				missle2Fire.setValue(o.getX(),o.getY(),o.getZ());
				laser1Fire.setValue(o.getX(),o.getY(),o.getZ());
			}
		}
	}

	private void marchMech(float frac)
	{
		if(checkProximity())
			return;

		float tempTrans[] = {0,0,0};
		int randX, randY;
		
		if(!doneRotating)
			rotateToDest();	
		
		if(doneRotating)
			doneMoving = false;

		if(!doneMoving)
		{
			tempTrans[0] = current_translation[0];
			tempTrans[2] = current_translation[2];

			float dx,dy,l;
			SFVec3f r = (SFVec3f) thisRobot.getExposedField("translation");
			dx = r.getX() - pathX[gridsMoved];
			dy = r.getZ() - pathY[gridsMoved];
			l = (float)Math.sqrt(dx*dx+dy*dy);

			if((pathX[gridsMoved] == convert[currentX]) && (pathY[gridsMoved] < convert[currentY]))
			{  
				botTransTotal -= BOTTRANS;
				tempTrans[2] = current_translation[2] + botTransTotal;

				//if(botTransTotal <= -2.0f)
				if (l < 0.1)
				{
					current_translation[2] -= 2f;
					tempTrans[2] = current_translation[2];
					currentY--;
				}
			}
			else
			if((pathX[gridsMoved] == convert[currentX]) && (pathY[gridsMoved] > convert[currentY]))
			{
			
				botTransTotal += BOTTRANS;
				tempTrans[2] = current_translation[2] + botTransTotal;

				//if(botTransTotal >= 2.0f)
				if (l < 0.1)				
				{
					current_translation[2] += 2f;
					tempTrans[2] = current_translation[2];
					currentY++;
				}

			}
			else
			if((pathY[gridsMoved] == convert[currentY]) && (pathX[gridsMoved] < convert[currentX]))
			{
			
				botTransTotal -= BOTTRANS;
				tempTrans[0] = current_translation[0] + botTransTotal;

				if (l < 0.1)
				//if(botTransTotal <= -2.0f)
				{
					current_translation[0] -= 2f;
					tempTrans[0] = current_translation[0];
					currentX--;
				}
			}
			else
			if((pathY[gridsMoved] == convert[currentY]) && (pathX[gridsMoved] > convert[currentX]))
			{
			
				botTransTotal += BOTTRANS;
				tempTrans[0] = current_translation[0] + botTransTotal;

				//if(botTransTotal >= 2.0f)
				if (l < 0.1)
				{
					current_translation[0] += 2f;
					tempTrans[0] = current_translation[0];
					currentX++;
				}
			}
			
			//if((botTransTotal >= 2.0f) || (botTransTotal <= -2.0f))
			if (l < 0.1)
			{


				doneMoving = true;
				gridsMoved++;
				doneRotating = false;
				botTransTotal = 0;
			//	System.out.println(nodename+": X:"+currentX+" Y:"+currentY+" num:"+gridsMoved+"/"+pathSize);

				checkToFire();

				if(gridsMoved == pathSize)
				{
					randX = arand.nextInt()%8;
					randY = arand.nextInt()%8;
					
					while(!checkPath(grid,randX,randY) || ((randX == currentX) && (randY == currentY)) )
					{
						randX = arand.nextInt()%8;
						randY = arand.nextInt()%8;
					}
				

				//	System.out.println(nodename+": finding path...X:"+randX+" Y:"+randY);

					for(int i=0; i<searchGrid.length; i++)
					{
						searchGrid[i] = grid[i];
					}
					searchGrid[currentX+currentY*gridXSize] = 0;
					findPath(currentX,currentY,randX,randY,0);
				//	System.out.println(nodename+": found path...");

					gridsMoved = 0;
					totalPoints++;
					destx = randX;
					desty = randY;
				//	System.out.println(nodename+": changing path... new path X:"+randX+" Y:"+randY+" size:"+pathSize);

					if(totalPoints == LIMIT)
					{
						stopMovement();
						upperBlows = true;
					}

				}
			}
			
			translation_changed.setValue(tempTrans);
		}
	}

	private void rotateToDest()
	{
		int newOrientation = orientation;
		float tempRotation[] = {0,1,0,0};

		tempRotation[3] = current_rotation[3];

		if((pathX[gridsMoved] == convert[currentX]) && (pathY[gridsMoved] < convert[currentY]))
		{  // Means need to rotate to face Backward
			newOrientation = BACKWARD;
		}
		else
		if((pathX[gridsMoved] == convert[currentX]) && (pathY[gridsMoved] > convert[currentY]))
		{
			newOrientation = FORWARD;
		}
		else
		if((pathY[gridsMoved] == convert[currentY]) && (pathX[gridsMoved] < convert[currentX]))
		{
			newOrientation = RIGHT;
		}
		else
		if((pathY[gridsMoved] == convert[currentY]) && (pathX[gridsMoved] > convert[currentX]))
		{
			newOrientation = LEFT;
		}

		if(orientation != newOrientation)
		{
			if( ((orientation == FORWARD) && (newOrientation == RIGHT)) ||
				((orientation == RIGHT) && (newOrientation == BACKWARD)) ||
				((orientation == BACKWARD) && (newOrientation == LEFT)) ||
				((orientation == LEFT) && (newOrientation == FORWARD)) )

			{
				botRotTotal -= BOTROT;
				tempRotation[3] = current_rotation[3] + botRotTotal;

				if(botRotTotal <= -1.57f)
				{
					current_rotation[3] -= 1.57f;
					tempRotation[3] = current_rotation[3];
					orientation = newOrientation;
					doneRotating = true;
					botRotTotal = 0;
				}

			}
			else
			if( ((orientation == FORWARD) && (newOrientation == BACKWARD)) ||
				((orientation == RIGHT) && (newOrientation == LEFT)) ||
				((orientation == BACKWARD) && (newOrientation == FORWARD)) ||
				((orientation == LEFT) && (newOrientation == RIGHT)) )
			{
			
				botRotTotal += BOTROT;
				tempRotation[3] = current_rotation[3] + botRotTotal;

				if(botRotTotal >= 3.14f)
				{
					current_rotation[3] += 3.14f;
					tempRotation[3] = current_rotation[3];
					orientation = newOrientation;
					doneRotating = true;
					botRotTotal = 0;
				}
			}
			else
			{
			
				botRotTotal += BOTROT;
				tempRotation[3] = current_rotation[3] + botRotTotal;

				if(botRotTotal >= 1.57f)
				{
					current_rotation[3] += 1.57f;
					tempRotation[3] = current_rotation[3];
					orientation = newOrientation;
					doneRotating = true;
					botRotTotal = 0;
				}
			
			}

			rotation_changed.setValue(tempRotation);
		}
		else
		{
			doneRotating = true;
		}
	//	System.out.println("Orientation = "+orientation+" doneRotation = "+doneRotating);
	}
			

	private void blowUpRight()
	{
		rightExplodeRot[3] = (float)Math.sin(botRotRight*1.57)*4.71f;
		rightArmExplodeRotation.setValue(rightExplodeRot);

		rightExplodeTrans[0] = botRotRight*1.75f;

		if(botRotRight <= 0.5f)
		{
			rightExplodeTrans[1] = -4.5f*( (botRotRight-.5f)/.5f)*((botRotRight-.5f)/.5f)+4.5f;
		}
		else
		{
			rightExplodeTrans[1] = -4.5f*( (botRotRight-.5f)/.5f)*((botRotRight-.5f)/.5f)+4.5f - (botRotRight-0.5f)*2.0f;
		}
		rightArmExplodeTranslation.setValue(rightExplodeTrans);

		if(botRotRight >= 1f)
		{
			rightBlows = false;
			rightDoneBlowing = true;
			rightChoice_changed.setValue(-1);

			botRotRight = 0;
			return;
		}

		botRotRight+=BOTROTBLOW;
	}

	private void blowUpLeft()
	{
		leftExplodeRot[3] = (float)Math.sin(botRotLeft*1.57)*4.71f;
		leftArmExplodeRotation.setValue(leftExplodeRot);

		leftExplodeTrans[0] = -botRotLeft*1.35f;
		leftExplodeTrans[2] = botRotLeft*1.0f;

		if(botRotLeft <= 0.5f)
		{
			leftExplodeTrans[1] = -4.0f*( (botRotLeft-.5f)/.5f)*((botRotLeft-.5f)/.5f)+4.0f;
		}
		else
		{
			leftExplodeTrans[1] = -4.0f*( (botRotLeft-.5f)/.5f)*((botRotLeft-.5f)/.5f)+4.0f - (botRotLeft-0.5f)*2.0f;
		}
		leftArmExplodeTranslation.setValue(leftExplodeTrans);

		if(botRotLeft >= 1f)
		{
			leftBlows = false;
			leftDoneBlowing = true;
			leftChoice_changed.setValue(-1);
			botRotLeft = 0;
			return;
		}

		botRotLeft+=BOTROTBLOW;
	}

	private void blowUpupper()
	{
		upperExplodeRot[3] = (float)Math.sin(botRotUpper*1.57)*4.71f;
		upperExplodeRotation.setValue(upperExplodeRot);

		upperExplodeTrans[2] = -botRotUpper*1.2f;

		stopMovement();
		weapon_avail[0] = false;
		weapon_avail[1] = false;
		if(botRotUpper <= 0.5f)
		{
			upperExplodeTrans[1] = -6.0f*( (botRotUpper-.5f)/.5f)*((botRotUpper-.5f)/.5f)+6.0f;
		}
		else
		{
			upperExplodeTrans[1] = -6.0f*( (botRotUpper-.5f)/.5f)*((botRotUpper-.5f)/.5f)+6.0f - (botRotUpper-0.5f)*1.5f;
		}
		upperExplodeTranslation.setValue(upperExplodeTrans);

		if(botRotUpper >= 1f)
		{
			upperBlows = false;
			upperDoneBlowing = true;
			upperChoice_changed.setValue(-1);
			botRotUpper = 0;
			return;
		}
		botRotUpper += BOTROTBLOW;
	}

	private void moveLegs(float frac)
	{
		rightOne[3] = 0;
		rightTwo[3] = 0;
		leftTwo[3] = 0;
		leftOne[3] = 0;

		
		if( (frac > 0.95f) && !(legsMoving))
		{
			legsDoneMoving = true;
		}
		else if(frac <= .5f)
		{
			rightOne[3] = (float)Math.sin(frac*3.14*4)*0.8f - .1f;
			rightTwo[3] = -(float)Math.sin(frac*3.14*2)*0.5f;
		}
		else
		{
			frac -= .5f;

			leftOne[3] = (float)Math.sin(frac*3.14*4)*0.8f - .1f;
			leftTwo[3] = -(float)Math.sin(frac*3.14*2)*0.5f;
		}
		//rotation_changed.setValue(generic);

		

		rightFirst_changed.setValue(rightOne);
		rightSecond_changed.setValue(rightTwo);
		leftFirst_changed.setValue(leftOne);
		leftSecond_changed.setValue(leftTwo);
	}
	
	// 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_fraction"))
			set_fraction((ConstSFFloat)e.getValue());
		else
		if (EventName.equals("missle_hit"))
			missle_hit((ConstSFInt32)e.getValue());
		else
		if (EventName.equals("laser_hit"))
			laser_hit((ConstSFInt32)e.getValue());
		else
		if (EventName.equals("receive_damage"))
			receive_damage((ConstSFFloat)e.getValue());
		else
		if (EventName.equals("die"))
			upperBlows = true;
	}
}