package demo.helicopter;

import java.text.*;			// for class DecimalFormat
import java.util.*;			// for class Vector
import mil.navy.nps.dis.*;		// for class EntityStatePdu
import mil.navy.nps.testing.*;		// for MulticastPduSender
import mil.navy.nps.util.*;
import mil.navy.nps.disEnumerations.*;

/**
 * Coordinate frame
 * <PRE>
 *
 *                 -Z
 *                  |
 *                  |
 *                  |
 *                  |
 *                  |_________ X
 *                 /
 *                /
 *               /
 *              Y
 * </PRE>
**/

public class HeloActionInterpreter extends Thread {

   private float pduSendInterval,	// time delay between pdus
		speed,			// not velocity, no directional information
		torque, attitude;	// current power setting
   private DecimalFormat df;		// pretty print screen output

   private EntityStatePdu teamEspdu;	// Pdu to send entity state to VRML scene
   private EntityStatePdu espdu;				// Pdu to send entity state to VRML scene
   private FirePdu fpdu;		// Pdu to send fire info to VRML scene
   private CollisionPdu copdu;		// Pdu to announce
   private DetonationPdu dpdu;

   private CreateEntityPdu cepdu;
   private CommentPdu cpdu;
   private RemoveEntityPdu repdu;
   private DatumSpecification datum;
   private VariableDatum variableDatum;
   private String geometryURL;
   private byte geometryURLbyteArray[];
   private long geometryURLlongArray[];

   private BehaviorStreamBufferUDP behaviorStreamBuffer; // tie in to the network
   private String ipAddress;
   private int portNumber;
   private short idNum;

   private HeloControlPanel hcp;
   private boolean isCollided, flying;
   private boolean red;
   private static CollisionDetectionTerrainReader terrain;
   private static boolean terrainAlreadyRead = false;

   private double [] normalHeight;   //mls
   private double oldHeading;
   private double newHeading;
   private double oldAltitude;
   private double newAltitude;
   private double oldAboveGround;
   private boolean first;

   private int sleepInterval = 50;  // milliseconds per sleep
   private int sleepCounter  =  0;
   private boolean userChangeHeard = false;

//=============================================================================================
// Constructors & Start
//=============================================================================================


public HeloActionInterpreter(String ipAddr, int portNum, int timeToLive, short siteNum, short appNum,
			short id, String marking, boolean rtpHeaderEnabled, HeloControlPanel parent)
{
	setFlying(true);
	speed = torque = 0.0f;  // initially helo on ground with no fwd airspeed
	pduSendInterval = 5.0f;	// seconds between pdu sends
	df = new DecimalFormat("###.00");

	cepdu = new CreateEntityPdu();	// instantiate a cepdu for announcing the creation of an entity
	cepdu.setRtpHeaderEnabled(rtpHeaderEnabled);
	cpdu = new CommentPdu();	// instantiate a cpdu, used to store the URL address
	cpdu.setRtpHeaderEnabled(rtpHeaderEnabled);
	repdu = new RemoveEntityPdu();	// instantiate a repdu, used to remove an entity
	repdu.setRtpHeaderEnabled(rtpHeaderEnabled);

	cepdu.setRequestID((long) id);
	repdu.setRequestID((long) id);

	datum = new DatumSpecification();
	variableDatum = new VariableDatum();
	geometryURLbyteArray = new byte[120];
	geometryURLlongArray = new long[120];
	geometryURL = new String ("http://www.web3D.org/WorkingGroups/vrtp/dis-java-vrml/demo/helicopter/helo_main.wrl");	// place URL address here

	teamEspdu = new EntityStatePdu();
	teamEspdu.setRtpHeaderEnabled(rtpHeaderEnabled);
	espdu = new EntityStatePdu();	// instantiate an espdu, used as a data structure
	espdu.setRtpHeaderEnabled(rtpHeaderEnabled);
	fpdu  = new FirePdu();		// instantiate a fire Pdu, used as data structure
	fpdu.setRtpHeaderEnabled(rtpHeaderEnabled);
	copdu = new CollisionPdu();	// instantiate a collision Pdu for later use
	copdu.setRtpHeaderEnabled(rtpHeaderEnabled);
	dpdu  = new DetonationPdu();
	dpdu.setRtpHeaderEnabled(rtpHeaderEnabled);

	System.out.println ("marking=" + marking + ", timeToLive=" + timeToLive);
	espdu.setMarking    (marking);

	idNum = id;
	if (id < 30)	red = false;
	else		red = true;

	if (red == true)
	{
		short[] RedentityID = new short[3];	// create entity id triplet for espdu & fire pdu
		RedentityID[0] = 0;
		RedentityID[1] = 1;
		RedentityID[2] = 98;
		teamEspdu.setEntityID(RedentityID[0], RedentityID[1], RedentityID[2]);  // set entity id triplet into

		espdu.setEntityLocationX(50f);	// mls
		espdu.setEntityLocationY(2000f + id%30*50);// initial quantities are set to zero when the
		espdu.setEntityOrientationPsi((float)(Math.PI));  // mls // set starting heading
		if (id == 32) {
			espdu.setEntityLocationZ(37.0f);	// mls  // adjust shrike32 altitude to 37 below sea level
		}
		else {
			espdu.setEntityLocationZ(37.25f);	// mls
		} //endif
	}
	else {
		short[] BlueentityID = new short[3];	// create entity id triplet for espdu & fire pdu
		BlueentityID[0] = 0;
		BlueentityID[1] = 1;
		BlueentityID[2] = 99;
		teamEspdu.setEntityID(BlueentityID[0], BlueentityID[1], BlueentityID[2]);  // set entity id triplet into

		espdu.setEntityLocationX(-4950f); // mls	// set starting location into espdu, all other
		espdu.setEntityLocationY(2000f + id%22*50);  // initial quantities are set to zero when the
		espdu.setEntityOrientationPsi(0.0f);	// mls // set starting heading
		espdu.setEntityLocationZ(41.5f);			// mls
	}
	oldHeading = newHeading = espdu.getEntityOrientationPsi();
	oldAltitude = newAltitude = espdu.getEntityLocationZ();
	oldAboveGround = 0.0;
	first = true;
	short[] entityID = new short[3];		// create entity id triplet for espdu & fire pdu
	entityID[0] = siteNum;
	entityID[1] = appNum;
	entityID[2] = id;
	espdu.setEntityID(entityID[0], entityID[1], entityID[2]);  // set entity id triplet into
	fpdu.setFiringEntityID(espdu.getEntityID());		// the espdu and fire pdu
	copdu.setIssuingEntityID(espdu.getEntityID());

	geometryURLbyteArray = geometryURL.getBytes();
	variableDatum.setVariableDatumID(1);
	variableDatum.setVariableDatumLength(120);

	for (int counter = 0; counter < geometryURLbyteArray.length ; counter++)
	{
		geometryURLlongArray[counter] = (long) geometryURLbyteArray[counter];
	}
	variableDatum.setVariableDatumValue(geometryURLlongArray);
	datum.addVariableDatum(variableDatum);	// add URL address as variable datum value

	cpdu.setDatumSpecification(datum);
	cpdu.setOriginatingEntityID(espdu.getEntityID());
	cpdu.setReceivingEntityID(teamEspdu.getEntityID());
	cpdu.setExemplar(cpdu);			// set cpdu fields

	cepdu.setOriginatingEntityID(espdu.getEntityID());
	cepdu.setReceivingEntityID(teamEspdu.getEntityID());
	cepdu.setExemplar(cepdu);		// set cepdu fields
	
	repdu.setOriginatingEntityID(espdu.getEntityID());
	repdu.setReceivingEntityID(teamEspdu.getEntityID());
	repdu.setExemplar(repdu);		// set repdu fields

	// instantiate a BehaviorStreamBuffer to handle the network interface,
	// instantiate with the multicast IP address and port
	EntityID temp = espdu.getEntityID();
	hcp = parent;
	isCollided = false;
	ipAddress = ipAddr;
	portNumber = portNum;

	// read in Fort Irwin Terrain file
	if (!terrainAlreadyRead)
	{
		terrain = new CollisionDetectionTerrainReader();
		terrain.parseFile();
		terrainAlreadyRead = true;
	}
	normalHeight = new double[4];   //mls

	behaviorStreamBuffer =  new BehaviorStreamBufferUDP (ipAddress, portNumber);
	behaviorStreamBuffer.setTimeToLive (timeToLive);
	Thread pduReader = new Thread(behaviorStreamBuffer);
	pduReader.start();

	System.out.println("sending CreateEntity PDU...");
	behaviorStreamBuffer.sendPdu(cepdu.getExemplar(), ipAddress, portNumber); // send cepdu
	System.out.println("sending Comment PDU with URL...");
	behaviorStreamBuffer.sendPdu(cpdu.getExemplar(), ipAddress, portNumber);  // send cpdu

   } // end constructor

   public void run ()	// start sending espdus, called from TestFrame.java
   {
	startFlying();   // calls flying which is an infinite espdu sending loop
   }

//=============================================================================================
// Properties
//=============================================================================================

   public void setIsCollided (boolean setValue)
   {
	userChangeHeard = true;
	isCollided = setValue;
   }
   public boolean getIsCollided () { return isCollided; }

   public void setFlying (boolean setValue) {
	userChangeHeard = true;
	flying = setValue;
   }
   public boolean getFlying () { return flying; }
   
   /** setSpeed is called by TestFrame.java whenever the airspeed scrollbar on the control panel is adjusted
   * This method calculates new X & Y linear velocities and sets the new values in the entity
   * state Pdu. The new values are then sent as part of the espdu the next time the
   * startFlying() method loops.
   */

   public void setSpeed(float newSpeed)
   {
	userChangeHeard = true;
	// System.out.println("entering setSpeed() w/arg.. " + newSpeed + " knots");
	speed = .5144f * newSpeed;    // convert from knots to meters/second
	float angle;
	if   (newSpeed <= 90)	angle = -newSpeed/9;
	else			angle = -10 - (newSpeed-90)/3;

	attitude = (float)Math.PI*angle/180;
	espdu.setEntityOrientationTheta(attitude);
	espdu.setEntityLinearVelocityX((float)(speed * Math.cos(espdu.getEntityOrientationPsi())));
	espdu.setEntityLinearVelocityY((float)(speed * Math.sin(espdu.getEntityOrientationPsi())));
//	espdu.setEntityAngularVelocityZ(0.0f);
	hcp.updateNoseAttitude((int) newSpeed, (int) getAttitude());
	System.out.println(", setting speed to " + speed + " m/s");
   } // end setSpeed()

   public void raiseNose()
   {
	userChangeHeard = true;
	setAttitude(getAttitude() + 1);
   }

   public void lowerNose ()
   {
	userChangeHeard = true;
	setAttitude(getAttitude() - 1);
   }

   public void setAttitude(float angle)
   {
	userChangeHeard = true;
	System.out.println("entering setAttitude() w/arg.. " + angle);
	float spd;
	if      (angle < -20)	angle = -20;
	else if (angle > 20)	angle =  20;
	
	if      (angle >= -10 && angle < 0)
	{
		spd = (-9*angle);
	}
	else if (angle < 0)
	{
		spd = 90 - 3*(angle+10);
	}
	else
	{
		spd = 0;
	}
	if (spd > 0)
	{
		espdu.setEntityAngularVelocityZ(0.0f);
	}
	attitude = (float)Math.PI*angle/180;
	espdu.setEntityOrientationTheta(attitude);
	speed = .5144f * spd;    // convert from knots to meters/second
	espdu.setEntityLinearVelocityX((float)(spd * Math.cos(espdu.getEntityOrientationPsi())));
	espdu.setEntityLinearVelocityY((float)(spd * Math.sin(espdu.getEntityOrientationPsi())));
	System.out.println("Setting speed to " + speed + " m/s");
	hcp.updateNoseAttitude((int) spd, (int) getAttitude());
   } // end setAttitude()

   public double getSpeed() { return speed; }

   public float getAttitude() { return attitude*180/(float)Math.PI; }	// return in degrees

   public float getAttitudeRad () { return attitude; }			// return in radians


	/** called by TestFrame.java whenever the collective(power) scrollbar on the control panel is
	* adjusted. This method calculates new Z linear velocity and sets the new value in the
	* entity state Pdu. The new value is then sent as part of the espdu the next time the
	* startFlying() method loops.
	*/
   public void setTorque (float tor)
   {
	userChangeHeard = true;
	torque = tor;
	//System.out.println("Setting torque to ... " + torque);

	// torque = 64 is the baseline, values below 64% result in downward velocity and values
	// greater than 64 result in upward velocity. One pound of torque results in a
	// 100 ft/min rate of climb or descent. Converted to meters per second this equals .24m/s

	if (tor > 64)
	{
		espdu.setEntityLinearVelocityZ((float)(-.24 * (tor - 64.0)));
	}
	else if (tor == 64)
	{
		espdu.setEntityLinearVelocityZ(0.0f);
	}
	else
	{
		espdu.setEntityLinearVelocityZ((float)(.24 * (64 - tor)));
	}
	//System.out.println("Setting zDot to ... "
	//						+ df.format(espdu.getEntityLinearVelocityZ()) + " m/sec");
   } // end setTorque()

 public float getTorque() { return torque; }

   /** called by TestFrame.java whenever the bankAngle scrollbar on the control panel is
   * adjusted. This method calculates new Z angular velocity and sets the new value in the
   * entity state Pdu. The new bank angle (Phi) is also set into the espdu. The new values
   * are then sent as part of the espdu the next time the startFlying() method loops.
   */
   public void setBankAngle(float bankAngle)
   {
	userChangeHeard = true;

	// conversion to radians is embedded here, values received from TestFrame.java are in degrees
	espdu.setEntityOrientationPhi((float)(bankAngle * 3.1415 / 180.0));
	//System.out.println("Setting Bank Angle to ... "
	//	+ df.format(espdu.getEntityOrientationPhi()) + " radians");

	espdu.setEntityAngularVelocityZ((float)(.1058 * espdu.getEntityOrientationPhi()));
	//System.out.println("Setting psiDot to ... "
	//	+ df.format(espdu.getEntityAngularVelocityZ()) + " rad/sec");

	// because the bank angle and angular velocity have changed,
	// X & Y linear velocities must must be updated as well
	espdu.setEntityLinearVelocityX((float)(speed *
					Math.cos(espdu.getEntityOrientationPsi())));
	espdu.setEntityLinearVelocityY((float)(speed *
					Math.sin(espdu.getEntityOrientationPsi())));
	System.out.println(", LinearVelocityX = " + espdu.getEntityLinearVelocityX() +
			   ", LinearVelocityY = " + espdu.getEntityLinearVelocityY());
   } // end setBankAngle()

   public EntityStatePdu getEspdu () { return espdu; }

//=============================================================================================
// Utility Methods
//=============================================================================================

   /** startFlying loops indefinitely sending espdus in its own thread. */

   public void startFlying()
   {
     float deltaTime;

     while (flying)	// loops until mother thread exits thus killing this thread
     {	
//	System.out.println ("sleepCounter * sleepInterval = " + sleepCounter * sleepInterval);
//	System.out.println ("userChangeHeard              = " + userChangeHeard);
//	System.out.println ("pduSendInterval * 1000.0     = " + pduSendInterval * 1000.0);

	// send PDU prerequisites test
	if (	(sleepCounter * sleepInterval >= pduSendInterval * 1000.0) ||  // compare seconds, not msec; keepalive heartbeat
		((userChangeHeard == true) && (sleepCounter * sleepInterval > 50)))  // otherwise, send if userChangeHeard
	{
		System.out.println ("HeloActionInterpreter sending espdu");
		espdu.makeTimestampCurrent();
		// send espdu to the specified multicast address
		behaviorStreamBuffer.sendPdu(espdu, "224.2.181.145", 62040);

		sleepCounter=0;
		userChangeHeard = false;
	} 

	// simple kinematics to update X, Y, Z location in world coordinates
     	deltaTime = (float) sleepInterval / 1000.0f;
	float newXLocation =(float) (espdu.getEntityLocationX() +
					espdu.getEntityLinearVelocityX() * deltaTime);
	float newYLocation= (float)(espdu.getEntityLocationY() +
					espdu.getEntityLinearVelocityY() * deltaTime);
	espdu.setEntityLocationX(newXLocation);
	espdu.setEntityLocationY(newYLocation);
	espdu.setEntityLocationZ(espdu.getEntityLocationZ() +
		espdu.getEntityLinearVelocityZ() * deltaTime);

	// update linear velocities
	espdu.setEntityLinearVelocityX((float)(speed * Math.cos(espdu.getEntityOrientationPsi())));
	espdu.setEntityLinearVelocityY((float)(speed * Math.sin(espdu.getEntityOrientationPsi())));
	// update orientations
	espdu.setEntityOrientationPhi(espdu.getEntityOrientationPhi() +
		espdu.getEntityAngularVelocityX() * deltaTime);
	espdu.setEntityOrientationPsi(espdu.getEntityOrientationPsi() +
		espdu.getEntityAngularVelocityZ() * deltaTime);
	espdu.setEntityOrientationTheta(espdu.getEntityOrientationTheta() +
		espdu.getEntityAngularVelocityY() * deltaTime);
	
	// get normal and height at current terrain position
	terrain.getNormalHeight((double)newXLocation, (double)newYLocation, normalHeight);
	// for display in the helo control panel
	newHeading   = espdu.getEntityOrientationPsi();
	newAltitude = espdu.getEntityLocationZ();
	if (first) {
		hcp.updateHeading( espdu.getEntityOrientationPsi() );
		hcp.updateAltitude( espdu.getEntityLocationZ(), normalHeight[3] );
		first = false;
	}
	else if (Math.abs(oldAltitude - newAltitude) >= 1.0  ||
		Math.abs(oldAboveGround - normalHeight[3]) >= 1.0 ||
		Math.abs((oldHeading - newHeading) * 180 / 3.1416) >= 1.0)
	{
		hcp.updateHeading ( espdu.getEntityOrientationPsi() );
		hcp.updateAltitude( espdu.getEntityLocationZ(), normalHeight[3] );
	}
	checkForGroundCollision();

	checkForReload();

	collectFirePdus();

	sleepCounter++;
	try {
		Thread.sleep (sleepInterval);
	}
	catch (InterruptedException ite)
	{
		System.out.println ("sleep InterruptedException " + ite);
		ite.printStackTrace();
	}

     } // end while
   } // end startFlying()

   public void checkForGroundCollision()
   {
	double aboveGround = espdu.getEntityLocationZ() - normalHeight[3];	// mls

	if ( aboveGround >= -1.85 )						// mls
	{
		espdu.setEntityLocationZ (normalHeight[3]-1.85);		// mls
		espdu.setEntityLinearVelocityX(0.0f);
		espdu.setEntityLinearVelocityY(0.0f);
		espdu.setEntityLinearVelocityZ(0.0f);
		espdu.setEntityOrientationPhi(0.0f);
		espdu.setEntityAngularVelocityX(0.0f);
		espdu.setEntityAngularVelocityZ(0.0f);
		speed = 0.0f;
	} // end if
   } // end checkforGroundCollision

public void checkForReload()
{
	double xreload;
	double yreload;
	double xNought = espdu.getEntityLocationX();
	double yNought = espdu.getEntityLocationY();
	if (red == true)
	{
		xreload = 0;		// mls
		yreload = 2000;
	}
	else { // blue
		xreload = -5000;	// mls
		yreload = 2000;
	}
	double distanceFromHome = Math.sqrt((xreload-xNought)*(xreload-xNought) + (yreload-yNought)*(yreload-yNought));
	if (distanceFromHome < 100)
	{
		hcp.resetAmmo();
	}
}//end check for reload

public void selfDestruct()
{
	userChangeHeard = true;
	copdu.setCollisionType(new UnsignedByte(CollisionTypeField.INELASTIC));
	copdu.setLinearVelocity(0.0f, 0.0f, 0.0f);
	copdu.makeTimestampCurrent();
	behaviorStreamBuffer.sendPdu(copdu, ipAddress, portNumber);
	System.out.println("Sent CollisionPdu.");
	dpdu.setLocationInWorldCoordinate(espdu.getEntityLocationX(),espdu.getEntityLocationY(),espdu.getEntityLocationZ());
	dpdu.setTargetEntityID(espdu.getEntityID()  );
	dpdu.setVelocity(0.0f, 0.0f, 0.0f);
	dpdu.makeTimestampCurrent();
	behaviorStreamBuffer.sendPdu(dpdu, ipAddress, portNumber);
	System.out.println("Sent DetonationPdu.");
	// could drop more slowly to ground...
	espdu.setEntityLocationZ(normalHeight[3]);
	espdu.setEntityOrientationPhi(0.1f);
	espdu.setEntityLinearVelocityX(0.0f);
	espdu.setEntityLinearVelocityY(0.0f);
	espdu.setEntityLinearVelocityZ(0.0f);
	espdu.setEntityAngularVelocityX(0.0f);
	espdu.setEntityAngularVelocityY(0.0f);
	espdu.setEntityAngularVelocityZ(0.0f);
	speed = 0;
	torque = 0;
	setAttitude(0);
	behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber);
	hcp.updateAltitude( espdu.getEntityLocationZ(), normalHeight[3] );

	isCollided = true;
	try
	{
		Thread.sleep(4500);
	}	
	catch(InterruptedException ite)
	{
		System.out.println ("sleep InterruptedException" + ite);
		ite.printStackTrace();
	}
	hcp.reset();
	reset();	
} //end selfDestruct



public String getFrameHeaderInfo(){
	EntityID temp = espdu.getEntityID();
	return new String(espdu.getMarking () + "   R71-630" + temp.getEntityID() +
			"    (" + temp.getSiteID() + "/" +
			temp.getApplicationID() + "/" + temp.getEntityID() + ")");
} // end getFrameheaderInfo

public void stopRun()
{
	userChangeHeard = true;
	flying = false;
	behaviorStreamBuffer.shutdown ();
}

//=============================================================================================
// Button Methods - These methods are all called by TestFrame.java
//=============================================================================================

public void reset()
{	
	userChangeHeard = true;
	if (red == true)
	{
		espdu.setEntityLocationX(50f);				// mls
		espdu.setEntityLocationY(2000f + idNum%30*50);;// state variables (other than location) zeroed
		espdu.setEntityOrientationPsi((float)(Math.PI));	// mls
		if (idNum == 32)
		{
			// adjust shrike32 altitude to 37 below sea level
			espdu.setEntityLocationZ(37.0f);	// mls  
		}
		else {
			espdu.setEntityLocationZ(37.25f);	// mls
		} //endif
	}
	else {
		espdu.setEntityLocationX(-4950f);	// mls
		// state variables (other than location) zeroed
		espdu.setEntityLocationY(2000f + idNum%22*50);
		espdu.setEntityOrientationPsi(0.0f);	// mls
		if (idNum == 22)			// mls
		{
			// adjust hawk22 altitude to 41.15 below sea level
			espdu.setEntityLocationZ(41.15f);	// mls  
		}						// mls
		else
		{						// mls
			espdu.setEntityLocationZ(41.5f);	// mls
		} //endif					// mls
	}
	espdu.setEntityOrientationPhi(0.0f);
	espdu.setEntityOrientationTheta(0.0f);
	espdu.setEntityLinearVelocityX(0.0f);
	espdu.setEntityLinearVelocityY(0.0f);
	espdu.setEntityLinearVelocityZ(0.0f);
	espdu.setEntityAngularVelocityX(0.0f);
	espdu.setEntityAngularVelocityY(0.0f);
	espdu.setEntityAngularVelocityZ(0.0f);
	espdu.setEntityLinearAccelerationX(0.0f);
	espdu.setEntityLinearAccelerationY(0.0f);
	espdu.setEntityLinearAccelerationZ(0.0f);
	speed = 0;
	torque = 0;
	isCollided = false;
	//System.out.println("isCollided = " + isCollided);

	System.out.println("reset, sending RemoveEntity PDU...");	
	// send notification to remove entity
	behaviorStreamBuffer.sendPdu(repdu.getExemplar(), ipAddress, portNumber);
} // end reset()

public void sendFirePdu()
{
	userChangeHeard = true;
	// much more work needs to be done here (overall)
	fpdu.setLocationInWorldCoordinate(espdu.getEntityLocationX() + 2.35,
					espdu.getEntityLocationY() + 0.5,
					espdu.getEntityLocationZ() + 0.6);
	
	fpdu.setVelocity((float)(espdu.getEntityLinearVelocityX() +
			1000 * Math.cos(espdu.getEntityOrientationPsi())),
			(float)(espdu.getEntityLinearVelocityY() +
			1000 * Math.sin(espdu.getEntityOrientationPsi())),
			(float)(espdu.getEntityLinearVelocityZ() -
			1000 * Math.sin(espdu.getEntityOrientationTheta())));
	fpdu.makeTimestampCurrent();
	fpdu.setRange(3000.0f);
	behaviorStreamBuffer.sendPdu(fpdu, ipAddress, portNumber);
	espdu.makeTimestampCurrent();
	behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber);
	try
	{
		Thread.sleep(50);
	}
	catch (InterruptedException ie)
	{
		System.out.println ("sleep InterruptedException" + ie);
	}
} // end sendFirePdu()

public void hover()
{			// stabilize the helo at a hover at the current location
	userChangeHeard = true;
	espdu.setEntityOrientationPhi(0.0f);
	espdu.setEntityLinearVelocityX(0.0f);
	espdu.setEntityLinearVelocityY(0.0f);
	espdu.setEntityLinearVelocityZ(0.0f);
	espdu.setEntityAngularVelocityX(0.0f);
	espdu.setEntityAngularVelocityY(0.0f);
	espdu.setEntityAngularVelocityZ(0.0f);
	speed = 0;
	torque = 0;
	setAttitude(0);
}

public void leftPedalTurn()
{
	userChangeHeard = true;

	espdu.setEntityAngularVelocityZ(-.1058f * 1.5f);

} // end leftpedalTurn()

public void rightPedalTurn()
{
	userChangeHeard = true;
	espdu.setEntityAngularVelocityZ(.1058f * 1.5f);

} // end rightPedalTurn()

//=============================================================================================
// Kill Determination Methods
//=============================================================================================

public void collectFirePdus()
{
	Vector pdus = behaviorStreamBuffer.receivedPdus();
	for (Enumeration enum = pdus.elements(); enum.hasMoreElements();)
	{
		ProtocolDataUnit tempPdu = (ProtocolDataUnit)enum.nextElement();
		UnsignedByte pduType = tempPdu.getPduType();
		if (pduType.shortValue() == PduTypeField.FIRE)
		{
			System.out.println("Received a fire Pdu");
			EntityID shooter = ((FirePdu)tempPdu).getFiringEntityID();
//			System.out.println("shooterID = (" + shooter.getSiteID() + "/" +
//				shooter.getApplicationID() + "/" + shooter.getEntityID() + ")" +
//				",   MyID = (" + espdu.getEntityID().getSiteID() + "/" +
//				espdu.getEntityID().getApplicationID() + "/" +
//				espdu.getEntityID().getEntityID() + ")");
			if (!shooter.equals(espdu.getEntityID()))
			{
				checkFirePdu((FirePdu)tempPdu);
				LinearVelocity missileVel = ((FirePdu)tempPdu).getVelocity();
//				System.out.println("missile velocity = " + missileVel.getX() + " / " +
//						missileVel.getY() + " / " + missileVel.getZ());
//				System.out.println("timestamp = " + ((FirePdu)tempPdu).getTimestamp());
//				System.out.println("VRML timeStamp = " + ((FirePdu)tempPdu).getVRMLTimestamp());
			}
		}
		if (pduType.shortValue() == PduTypeField.ENTITYSTATE)
		{
			checkCollision((EntityStatePdu)tempPdu);
		}
	} // end for
} // end collect FirePdus

/*
public void checkFirePdu(FirePdu shot){

	double proximityOfKillDistance = 25.0;//meters from target that still kills
	WorldCoordinate launchLocation = shot.getLocationInWorldCoordinate();
	LinearVelocity projectileVelocity = shot.getVelocity();

	double xNought = espdu.getEntityLocationX();		// This is the entities
	double yNought = espdu.getEntityLocationY();		// current location
	double zNought = espdu.getEntityLocationZ();
	double xLaunch = launchLocation.getX();		// The position the projectile
	double yLaunch = launchLocation.getY();		// was launched from
	double zLaunch = launchLocation.getZ();

	double vLaunchX = projectileVelocity.getX();		// The projectile's velocity
	double vLaunchY = projectileVelocity.getY();		// at launch
	double vLaunchZ = projectileVelocity.getZ();

	double vLaunchMagnitude = Math.sqrt(vLaunchX*vLaunchX + vLaunchY*vLaunchY);
	double vLaunchTheta = Math.atan2 (vLaunchY, vLaunchX);
	double targetTheta  = Math.atan2 ((yNought-yLaunch), (xNought-xLaunch));
	
	// ensure vLaunchTheta, targetTheta positive (0..2pi) since range atan2 is (-pi .. pi)
	if (vLaunchTheta < 0.0) vLaunchTheta += 2.0 * Math.PI;
	if (targetTheta < 0.0)  targetTheta  += 2.0 * Math.PI;

	double horizontalLaunchRange = Math.sqrt((xLaunch-xNought)*(xLaunch-xNought) + (yLaunch-yNought)*(yLaunch-yNought));
	

	System.out.println("vLaunchTheta=" + vLaunchTheta * 180.0/Math.PI +
		", targetTheta=" + targetTheta * 180.0/Math.PI);
	System.out.println("horizontalLaunchRange=" + horizontalLaunchRange +
	", fire PDU shot.getRange()=" + shot.getRange());

	if (horizontalLaunchRange > shot.getRange())
	{
		System.out.println("no hit, out of range");
		return;
	}
//*****new firing code here*****8
	double bulletFlightTime = horizontalLaunchRange/vLaunchMagnitude;
	double bulletAtTargetRangeX = xLaunch + (vLaunchX * bulletFlightTime);
	double bulletAtTargetRangeY = yLaunch + (vLaunchY * bulletFlightTime);
 System.out.println("bulletFlightTime=" + bulletFlightTime);
System.out.println("bullet at target X range=" + bulletAtTargetRangeX);
System.out.println("bullet at target Y range=" + bulletAtTargetRangeY);
System.out.println("differenceY=" +(  (bulletAtTargetRangeX- xNought) * (bulletAtTargetRangeX- xNought)) );
System.out.println("differenceX=" + (  (bulletAtTargetRangeX- xNought) * (bulletAtTargetRangeX- xNought)));
System.out.println("proxysquared=" + proximityOfKillDistance*proximityOfKillDistance);
System.out.println("actualsquared=" + (  (bulletAtTargetRangeX- xNought) * (bulletAtTargetRangeX- xNought)
	+ (bulletAtTargetRangeY- yNought) * (bulletAtTargetRangeY- yNought)));

	if ((proximityOfKillDistance*proximityOfKillDistance) <
	(  (bulletAtTargetRangeX- xNought) * (bulletAtTargetRangeX- xNought)
	+ (bulletAtTargetRangeY- yNought) * (bulletAtTargetRangeY- yNought)))
	{
	return; // no hit
	}
	copdu.setCollisionType(new UnsignedByte(CollisionTypeField.INELASTIC));
	copdu.setLinearVelocity(0.0f, 0.0f, 0.0f);
	copdu.makeTimestampCurrent();
	behaviorStreamBuffer.sendPdu(copdu, ipAddress, portNumber);
	System.out.println("Sent CollisionPdu.");
	dpdu.setLocationInWorldCoordinate(espdu.getEntityLocationX(),espdu.getEntityLocationY(),espdu.getEntityLocationZ());
	dpdu.setTargetEntityID(espdu.getEntityID()  );
	dpdu.setVelocity(0.0f, 0.0f, 0.0f);
	dpdu.makeTimestampCurrent();
	behaviorStreamBuffer.sendPdu(dpdu, ipAddress, portNumber);
	System.out.println("Sent DetonationPdu.");
	isCollided = true;
	System.out.println("isCollided = " + isCollided);
	while(espdu.getEntityLocationZ() <= -1.4f){

	espdu.setEntityLinearVelocityX(.5f * espdu.getEntityLinearVelocityX() );
	espdu.setEntityLinearVelocityY(.5f * espdu.getEntityLinearVelocityY());
	espdu.setEntityLinearVelocityZ(4.9f);

	// simple kinematics to update X, Y, Z location in world coordinates
	espdu.setEntityLocationX(espdu.getEntityLocationX() +
		espdu.getEntityLinearVelocityX() * pduSendInterval);
	espdu.setEntityLocationY(espdu.getEntityLocationY() +
		espdu.getEntityLinearVelocityY() * pduSendInterval);
		espdu.setEntityLocationZ(espdu.getEntityLocationZ() +
		espdu.getEntityLinearVelocityZ() * pduSendInterval);

	// update linear velocities
	espdu.setEntityLinearVelocityX((float)(speed *
		Math.cos(espdu.getEntityOrientationPsi())));
	espdu.setEntityLinearVelocityY((float)(speed *
		Math.sin(espdu.getEntityOrientationPsi())));
	// update orientations
	espdu.setEntityOrientationPhi(espdu.getEntityOrientationPhi() +
		espdu.getEntityAngularVelocityX() * pduSendInterval);
	espdu.setEntityOrientationPsi(espdu.getEntityOrientationPsi() +
		espdu.getEntityAngularVelocityZ() * pduSendInterval);
	espdu.setEntityOrientationTheta(espdu.getEntityOrientationTheta() +
		espdu.getEntityAngularVelocityY() * pduSendInterval);
	
	behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber);
	System.out.println("My current Altitude is ... " + espdu.getEntityLocationZ());
	try{
		Thread.sleep(450);
	} // end try
	catch(InterruptedException ie){ie.printStackTrace();}
	} // end while
	System.out.println("Calling the reset functions");
	//reset();
	hcp.reset();
} // end checkFirePdus()
*/
   /**
    *  Function Works as Follows :
    *	1.   Get X, Y, Z components of launch velocity.
    *	2.   Calculate launch velocity vector.
    *	3.   Get X, Y, Z location of the launch.
    *	4.   Get X, Y, Z velocity componets of this helo at launch.
    *	5.   Get X, Y, Z location of this helo at launch.
    *	6.   Calculate the distance between this helo and the launch point.
    *	7.   Calculate the relative velocity of the fire with respect to this helo.
    *	8.   Find the time to fly for this distance.
    *	9.   Find the actual X, Y, Z location for the fire after "time to fly".
    *	10.  Calculate the distance between this helo and the new location of the fire after "time
    *		to fly.
    *	11.  If the distance is equal to or less than HIT_RADIUS a hit has occured.
    *
    *  @param	shot Fire PDU that is being checked.
    **/
public void checkFirePdu(FirePdu shot)
{
	double HIT_RADIUS = 25.0; //??????????
	
	double launchVelocityX = shot.getVelocity().getX();
	double launchVelocityY = shot.getVelocity().getY();
	double launchVelocityZ = shot.getVelocity().getZ();
	
/*
Add this code after the wind is implemented	
	launchVelocityX += windVelocity.x;
	launchVelocityY += windVelocity.x;
	launchVelocityZ += windVelocity.x;
*/
	
	double launchVelocity = Math.sqrt((launchVelocityX * launchVelocityX) +
					(launchVelocityY * launchVelocityY) +
					(launchVelocityZ * launchVelocityZ) );
	
	double launchLocationX = shot.getLocationInWorldCoordinate().getX();
	double launchLocationY = shot.getLocationInWorldCoordinate().getY();
	double launchLocationZ = shot.getLocationInWorldCoordinate().getZ();
	
	double myVelocityAtLaunchX = espdu.getEntityLinearVelocityX();
	double myVelocityAtLaunchY = espdu.getEntityLinearVelocityY();
	double myVelocityAtLaunchZ = espdu.getEntityLinearVelocityZ();
/*	
	double myVelAtLaunch = Math.sqrt((myVelAtLaunchX * myVelAtLaunchX) + (myVelAtLaunchY * myVelAtLaunchY) +
										(myVelAtLaunchZ * myVelAtLaunchZ) );
*/	
	
	double myLocationAtLaunchX = espdu.getEntityLocationX();
	double myLocationAtLaunchY = espdu.getEntityLocationY();
	double myLocationAtLaunchZ = espdu.getEntityLocationZ();
	
	double distanceFromTargetAtLaunch = Math.sqrt(
					(myLocationAtLaunchX - launchLocationX) *
					(myLocationAtLaunchX - launchLocationX) +
					(myLocationAtLaunchY - launchLocationY) *
					(myLocationAtLaunchY - launchLocationY) +
					(myLocationAtLaunchZ - launchLocationZ) *
					(myLocationAtLaunchZ - launchLocationZ));
					
	double relativeLaunchVelocityX = launchVelocityX - myVelocityAtLaunchX;
	double relativeLaunchVelocityY = launchVelocityY - myVelocityAtLaunchY;
	double relativeLaunchVelocityZ = launchVelocityZ - myVelocityAtLaunchZ;

	double relativeLaunchVelocity = Math.sqrt(
					(relativeLaunchVelocityX * relativeLaunchVelocityX) +
					(relativeLaunchVelocityY * relativeLaunchVelocityY) +
					(relativeLaunchVelocityZ * relativeLaunchVelocityZ));
											
	double timeToTravel = distanceFromTargetAtLaunch / relativeLaunchVelocity;
	double launchLocationAfterFlyX = launchLocationX + relativeLaunchVelocityX * timeToTravel;
	double launchLocationAfterFlyY = launchLocationY + relativeLaunchVelocityY * timeToTravel;
	double launchLocationAfterFlyZ = launchLocationZ + relativeLaunchVelocityZ * timeToTravel;

	//Closest point of approach
	double CPA = Math.sqrt((launchLocationAfterFlyX - myLocationAtLaunchX) *
								(launchLocationAfterFlyX - myLocationAtLaunchX) +
							(launchLocationAfterFlyY - myLocationAtLaunchY) *
							(launchLocationAfterFlyY - myLocationAtLaunchY) +
							(launchLocationAfterFlyZ - myLocationAtLaunchZ) *
							(launchLocationAfterFlyZ - myLocationAtLaunchZ) );

	
	if (CPA <= HIT_RADIUS) {

		System.out.println("Help Me, I'm dying, MEDIC!!! MEDIC!!!*****************************************************");
	//	copdu.setLocation(espdu.getEntityLocation());
			copdu.setCollisionType(new UnsignedByte(CollisionTypeField.INELASTIC));
			copdu.setLinearVelocity(0.0f, 0.0f, 0.0f);
			copdu.makeTimestampCurrent();
			behaviorStreamBuffer.sendPdu(copdu, ipAddress, portNumber);
			System.out.println("Sent CollisionPdu.");
	dpdu.setLocationInWorldCoordinate(espdu.getEntityLocationX(),espdu.getEntityLocationY(),espdu.getEntityLocationZ());
	dpdu.setTargetEntityID(espdu.getEntityID()  );
	dpdu.setVelocity(0.0f, 0.0f, 0.0f);
	dpdu.makeTimestampCurrent();
	behaviorStreamBuffer.sendPdu(dpdu, ipAddress, portNumber);
	System.out.println("Sent DetonationPdu.");
			isCollided = true;
			System.out.println("isCollided = " + isCollided);
			while(espdu.getEntityLocationZ() <= -1.4f){
				espdu.setEntityLinearVelocityX(.5f * espdu.getEntityLinearVelocityX() );
			espdu.setEntityLinearVelocityY(.5f * espdu.getEntityLinearVelocityY());
			espdu.setEntityLinearVelocityZ(4.9f);

			// simple kinematics to update X, Y, Z location in world coordinates
			espdu.setEntityLocationX(espdu.getEntityLocationX() +
						espdu.getEntityLinearVelocityX() * pduSendInterval);
			espdu.setEntityLocationY(espdu.getEntityLocationY() +
						espdu.getEntityLinearVelocityY() * pduSendInterval);
			espdu.setEntityLocationZ(espdu.getEntityLocationZ() +
										espdu.getEntityLinearVelocityZ() * pduSendInterval);

			// update linear velocities
			espdu.setEntityLinearVelocityX((float)(speed * Math.cos(espdu.getEntityOrientationPsi())));
			espdu.setEntityLinearVelocityY((float)(speed * Math.sin(espdu.getEntityOrientationPsi())));
			// update orientations
			espdu.setEntityOrientationPhi(espdu.getEntityOrientationPhi() +
											espdu.getEntityAngularVelocityX() * pduSendInterval);
			espdu.setEntityOrientationPsi(espdu.getEntityOrientationPsi() +
										espdu.getEntityAngularVelocityZ() * pduSendInterval);
			espdu.setEntityOrientationTheta(espdu.getEntityOrientationTheta() +
					espdu.getEntityAngularVelocityY() * pduSendInterval);
	
			behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber);
			System.out.println("My current Altitude is ... " + espdu.getEntityLocationZ());
			try{
					Thread.sleep(450);
			} // end try
			catch(InterruptedException ie){ie.printStackTrace();}
		} // end while
		System.out.println("Calling the reset function");
		//reset();
		hcp.reset();
	}//end if
}// end checkFirePdus()

public void checkCollision(EntityStatePdu entityPdu )
{

	if (	entityPdu.getEntityID().equals( espdu.getEntityID())   ||
  		entityPdu.getEntityID().getEntityID().intValue() == 99 ||
  		entityPdu.getEntityID().getEntityID().intValue() == 98)
	{
		return;
	}
	// check for obstacle collision
	double[] entityMinusMe = new double[3];
	entityMinusMe[0] = entityPdu.getEntityLocationX() - espdu.getEntityLocationX();
	entityMinusMe[1] = entityPdu.getEntityLocationY() - espdu.getEntityLocationY();
	entityMinusMe[2] = entityPdu.getEntityLocationZ() - espdu.getEntityLocationZ();
	double distBtwn = entityMinusMe[0]*entityMinusMe[0] + entityMinusMe[1]*entityMinusMe[1] +
		entityMinusMe[2]*entityMinusMe[2];

	// test if within 10 meters of each other
	if (distBtwn <= 400.0) {
		System.out.println("Helo impacts other vehicle.  Helo Dies.");	
		selfDestruct();
		//hcp.reset();
	} //endif
} //end checkCollision

} // end class HeloActionInterpreter