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 AgentHeloActionInterpreter 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 AgentHeloControlPanel 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;
  
   //Agent variables below:
   private Hashtable sensedPdus;
   private Integer enemyFlag;
   private EntityStatePdu homeBaseEspdu;
   private boolean offensive = false;
   private boolean defensive = false;
   private boolean flagCaptured = false;
   private double newAboveGround = 0.0d;
   public static final int OFFENSIVE = 0;
   public static final int DEFENSIVE = 1;

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

public AgentHeloActionInterpreter(String ipAddr, int portNum, int timeToLive,
                                     short siteNum, short appNum, short id,
                                     String marking, boolean rtpHeaderEnabled,
                                     AgentHeloControlPanel parent)
{
   setFlying(true);
   speed = torque = 0.0f;    // initially helo on ground with no fwd airspeed
   pduSendInterval = 0.5f;		// approx 500 ms 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

/////////////////////////////////////////////////////////////////////////////////////////////////////
//agent thing
//this identifies this entity as an agent to the referee
   espdu.setForceID(1);


   // 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

   //******************** added agent stuff *********************

   homeBaseEspdu = new EntityStatePdu();//(EntityStatePdu)espdu.clone();

   //determine which team I'm on and then set the enemy flag as the
   //puppy I'm after
   int myIDNumber = espdu.getEntityID().getEntityID().intValue();
   if (myIDNumber < 30)//I'm a red team helicopter
   {
      enemyFlag = new Integer(99);
      homeBaseEspdu.setEntityLocationX(-5000); // set starting location into espdu, all other
	    homeBaseEspdu.setEntityLocationY(2000); // initial quantities are set to zero when the
	    homeBaseEspdu.setEntityLocationZ(39.8); // espdu is constructed in the dis-java-vrml code
   }
   else //I'm a blue team helicopter
   {
      enemyFlag = new Integer(98);
      homeBaseEspdu.setEntityLocationX(0); // set starting location into espdu, all other
      homeBaseEspdu.setEntityLocationY(2000); // initial quantities are set to zero when the
	    homeBaseEspdu.setEntityLocationZ(35.8); // espdu is constructed in the dis-java-vrml code
   }

   offensive = true;
   defensive = false;
   flagCaptured = false;
   sensedPdus = new Hashtable();

} // end constructor


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

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

public void setIsCollided(boolean setValue) {isCollided = setValue;}


public boolean getIsCollided() {return isCollided;}


public void setFlying(boolean setValue) {flying = setValue;}


public boolean getFlying() {return flying;}


// 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)
{
//   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())));
   hcp.updateNoseAttitude((int) newSpeed, (int) getAttitude());
//   System.out.println("Setting speed to ... " + speed + " m/s");
} // end setSpeed()


public void raiseNose()
{
   setAttitude(getAttitude() + 1);
}// end raiseNose()


public void lowerNose()
{
   setAttitude(getAttitude() - 1);
}// end lowerNose()


public void setAttitude(float angle)
{
//   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.0f;
   }
   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
}// end getAttitude()


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


// 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)
{
   torque = tor;
   // 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)));
   }// end if
   else if (tor == 64)
   {
      espdu.setEntityLinearVelocityZ(0.0f);
   }// end else if
   else
   {
      espdu.setEntityLinearVelocityZ((float)(.24 * (64 - tor)));
   }
} // 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)
{
   // conversion to radians is embedded here, values received from
   // TestFrame.java are in degrees
   espdu.setEntityOrientationPhi((float)(bankAngle * 3.1415 / 180.0));
	 espdu.setEntityAngularVelocityZ((float)(.1058 * espdu.getEntityOrientationPhi()));

   // because the bank angle and angular velocity have changed the heading (Psi)
   // and X & Y linear velocities must must be updated as well
   espdu.setEntityOrientationPsi(espdu.getEntityOrientationPsi() +
	        									     espdu.getEntityAngularVelocityZ() *
                                 pduSendInterval);
   espdu.setEntityLinearVelocityX((float)(speed *
                                    Math.cos(espdu.getEntityOrientationPsi())));
   espdu.setEntityLinearVelocityY((float)(speed *
	  											 Math.sin(espdu.getEntityOrientationPsi())));
} // end setBankAngle()


public EntityStatePdu getEspdu() {return espdu;}


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

// this method loops indefinitely sending espdus every 300 ms or so. This loop
// runs on its own thread
public void startFlying()
{
   while(flying) // loops until mother thread exits thus killing this thread
   {
//      System.out.println("starting while flying");

      // simple kinematics to update X, Y, Z location in world coordinates
      float newXLocation =(float) (espdu.getEntityLocationX() +
      espdu.getEntityLinearVelocityX() * pduSendInterval);
      espdu.setEntityLocationX(newXLocation);
      float newYLocation= (float)(espdu.getEntityLocationY() +
      espdu.getEntityLinearVelocityY() * pduSendInterval);
      espdu.setEntityLocationY(newYLocation);

	    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);

      // 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();
      newAboveGround = ( normalHeight[3] - (espdu.getEntityLocationZ()) );
      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() );

         // this is where the altitude above ground is calculated in the control
         // panel it simply takes the entity "z" and subtracts the height of the
         // terrain [3] I'll do this here and use the variable for the agent
         // manipulations
         hcp.updateAltitude( espdu.getEntityLocationZ(), normalHeight[3] );
      }// end if

      if (first)
      {
         hcp.updateHeading( espdu.getEntityOrientationPsi() );
         hcp.updateAltitude( espdu.getEntityLocationZ(), normalHeight[3] );
         first = false;
      }// end if

      checkForGroundCollision();

      checkForReload();

	    // send espdu to the specified multicast address
      espdu.makeTimestampCurrent();
 	    behaviorStreamBuffer.sendPdu(espdu, "224.2.181.145", 62040);

      collectSensorPdus();

      runAgent();

      try
      {			      // put thread to sleep for 250 ms, thus when
	       Thread.sleep(250);	      // time to complete the loop (hopefully about
	    } // end try		      // 50ms) is added the pdu send interval will
	    catch(InterruptedException ite)
      {  // be approximately 300ms
	       ite.printStackTrace();
	    }// end catch
   }// end while
}// end startFlying()

public void setOffensive()
{
   offensive = true;
   defensive = false;
   String tempTitle = new String("Agent Helo Control Panel      " + getFrameHeaderInfo());
   hcp.setTitle(tempTitle);
   espdu.setForceID(1);

}

public void setDefensive()
{
   offensive = false;
   defensive = true;
   String tempTitle = new String("Agent Helo Control Panel      " + getFrameHeaderInfo());
   hcp.setTitle(tempTitle);
   espdu.setForceID(2);

}

public boolean getOffensive( ) { return offensive; }

public boolean getDefensive( ) { return defensive; }

public void runAgent()
{
/*
//little tester to package up a comment pdu and send out some data, for now this will be received and displayed by the referee
//I'm using the same commentPdu as defined in the constructor
   //I'll send 6 ints 0-5 as a test
   DataPdu dataPdu = new DataPdu();

   DatumSpecification datum2 = new DatumSpecification();

   //load up the datumSpecification with variabledatum objects
   for (int ix = 0; ix < 6; ix++)
   {
      FixedDatum fixedDatum2 = new FixedDatum();
	   fixedDatum2.setFixedDatumID(2);
	   fixedDatum2.setValue(ix);
      //this puts these variableDatums into a vector
	   datum2.addFixedDatum(fixedDatum2);
   }

   //let's look at what we are sending
   for (int ix = 0; ix < datum2.getFixedDatumCount(); ix++)
   {
      FixedDatum tempData = datum2.fixedDatumAt(ix);
      System.out.println("Sending the value " + tempData.getValue());
   }

   //use the dataPDU, load data
   dataPdu.setDatumInformation(datum2);
	dataPdu.setOriginatingEntityID(espdu.getEntityID());
	dataPdu.setReceivingEntityID(teamEspdu.getEntityID());//not sure for now if I need to mess with this
//System.out.println("Packaged the data pdu up" );


//tester to see if I'm packaging this up right

   DatumSpecification datum3 = dataPdu.getDatumInformation();
   for (int kx = 0; kx < datum3.getFixedDatumCount(); kx++)
   {
      FixedDatum fixedDatum3 = datum3.fixedDatumAt(kx);
      System.out.println("Checking the value " + fixedDatum3.getValue());
   }
System.out.println("number of items I'm sending 4 =" + datum2.getFixedDatumCount() );

   //send this puppy out
//   System.out.println("sending Agent Data PDU with data...");
   behaviorStreamBuffer.sendPdu(dataPdu, ipAddress, portNumber);	// send cpdu   //cpdu.getExemplar()

*/

   if( offensive )
   {
      EntityStatePdu enemyFlagEspdu;

      // Determine turn angle to go to flag or return home after capturing it
      Enumeration enum = sensedPdus.keys();
      while(enum.hasMoreElements())
      {
         Integer tempKey = (Integer)enum.nextElement();
         if (tempKey.equals(enemyFlag))
         {
            enemyFlagEspdu = (EntityStatePdu)sensedPdus.get(enemyFlag);
            getFlag(enemyFlagEspdu);
         }// end if
      }// end while
   }// end if


   // FUTURE WORK: IMPLEMENT METHOD TO DEFEND HOME FLAG ************************
   if( defensive )
   {
     //defendHome(); to be implemented later
   }// end if

   // FUTURE WORK: IMPELEMENT engageEnemy() METHOD TO SHOT HELO GUNS************

}// end runAgent()


public void getFlag(EntityStatePdu enemyFlagEspdu)
{
   int angle;
   double range;

   range =  determineRange( espdu, enemyFlagEspdu );
//   System.out.println("Range = " + range);

   if (flagCaptured)
   {
      if (  range > 100 )
      {
         flagCaptured = false;
      }
   }

   // Check to see if flag is captured
   if( !flagCaptured )
   {
      if( range <= 100 )
      {
         flagCaptured = true;
 //        System.out.println("flagCaptured = true");
      }
   }

   if( !flagCaptured )  // go get it!
   {
      flyToHere( enemyFlagEspdu );
   }
   else
   {
      System.out.println("driving home");
      flyToHere( homeBaseEspdu );   // and win!
   }
}// end getFlag()


public void flyToHere( EntityStatePdu here )
{
   //this takes care of speed
   setSpeed(60.0f);

   //this takes care of the heading
   int angle = interceptAngle( espdu, here );
//   System.out.println("interceptAngle = " + angle);

   float bankAngle = -((float)angle * 0.35f);
   setBankAngle(bankAngle);
   hcp.bankTextField.setText((new Integer((int)bankAngle)).toString());
   hcp.bankScroll.setValue((int)bankAngle);

   double rangeToGoal = determineRange(espdu, here);
//   System.out.println("Range to Goal = " + rangeToGoal );

   if (rangeToGoal > 1000)
   {
      //this takes care of speed
      setSpeed(60.0f);
      double upTorqueSetting = 64.0d - ( ( newAboveGround - 300 ) * .5d );

      if( upTorqueSetting < 0)
      {
         upTorqueSetting = 0;
      }
      if( upTorqueSetting > 100)
      {
         upTorqueSetting = 100;
      }

      setTorque((float)upTorqueSetting);
      hcp.torqueTextField.setText((new Integer((int)upTorqueSetting)).toString());
      hcp.torqueScroll.setValue((int)upTorqueSetting);

   }
   else //make the approach to the goal
   {
      double targetZ = here.getEntityLocationZ();
      if(targetZ < 0.0d)
      {
         targetZ = 0.0d;
      }
      double myZ = -(espdu.getEntityLocationZ());
      double upTorqueSetting = 64.0d - ( ( myZ - targetZ ) * .5d );

      if( upTorqueSetting < 0)
      {
         upTorqueSetting = 0;
      }
      if( upTorqueSetting > 100)
      {
         upTorqueSetting = 100;
      }

      setTorque((float)upTorqueSetting);
      hcp.torqueTextField.setText((new Integer((int)upTorqueSetting)).toString());
      hcp.torqueScroll.setValue((int)upTorqueSetting);

      double approachSpeeed = (determineRange(espdu, here) * 0.1d);
      //this takes care of speed
      setSpeed((float)approachSpeeed);
   }
}// end flyToHere()


public int interceptAngle( EntityStatePdu me, EntityStatePdu target )
{
   //calculation variables
   int myX = (int)me.getEntityLocationX();
   int myY = (int)me.getEntityLocationY();
   int targetX = (int)target.getEntityLocationX();
   int targetY = (int)target.getEntityLocationY();

   //this gives the angle (0-90) between the two points
   double a = Math.abs(myX - targetX);
   double b = Math.abs(myY - targetY);
   double c = Math.sqrt((a * a) + (b * b));
   double theta = Math.toDegrees(Math.acos(a / c));

   int heading;
   int interceptAngle;
   int targetBearing = 0;

   //used to get the heading of the "me" object
   double mypsi = me.getEntityOrientationPsi();

   // determine my heading
   if( mypsi < 0 )
   {
      heading = (int)( 360 + ((( mypsi * 180 ) / Math.PI ) % 360 ));
   }
   else
   {
      heading = (int)((( mypsi * 180 ) / Math.PI ) % 360 );
   }

   // determine target bearing with relation to quadrants
   // North West quadrant
   if( (( myX == targetX ) && ( myY >  targetY )) ||
       (( myX <  targetX ) && ( myY >  targetY )) ||
       (( myX <  targetX ) && ( myY == targetY )) )
   {
      theta = 360 - theta;
   }
   // North East quadrant
   else if(( myX < targetX ) && ( myY < targetY ))
   {
      theta += 0 ;
   }
   // South East quadrant
   else if( (( myX == targetX ) && ( myY <  targetY )) ||
            (( myX >  targetX ) && ( myY <  targetY )) ||
            (( myX >  targetX ) && ( myY == targetY )) )
   {
      theta = 180 - theta;
   }
   // South West quadrant
   else if(( myX > targetX ) && ( myY > targetY ))
   {
      theta = 180 + theta;
   }

   // determine intercept angle
   interceptAngle = heading - (int)theta;

   //check to see if there is a turn greater than 180 degrees
   if (interceptAngle > 180)
   {
      interceptAngle -= 360;
   }
   if (interceptAngle < -180)
   {
      interceptAngle += 360;
   }

   return interceptAngle;
}// end interceptAngle()


public double determineRange(EntityStatePdu firstEspu,
                             EntityStatePdu secondEspdu)
{
   double range;
   double firstX = firstEspu.getEntityLocationX();
   double firstY = firstEspu.getEntityLocationY();
   double firstZ = firstEspu.getEntityLocationZ();
   double secondX = secondEspdu.getEntityLocationX();
   double secondY = secondEspdu.getEntityLocationY();
   double secondZ = secondEspdu.getEntityLocationZ();

   range =  Math.sqrt( (firstX-secondX)*(firstX-secondX) +
                       (firstY-secondY)*(firstY-secondY) +
                       (firstZ-secondZ)*(firstZ-secondZ) );

   return range;
}// end determineRange()


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();// This is the entities
   double yNought = espdu.getEntityLocationY();
	 if(red == true)
   {
      xreload = 0;                   // mls
      yreload = 2000;
   }
   else
   {
      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()
{
	 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);
	 } // end try
	 catch(InterruptedException ite)
   {
	    ite.printStackTrace();
	 } // end catch
	 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()
{
	 behaviorStreamBuffer.shutdown ();
}// end stopRun()


//==============================================================================
// Button Methods - These methods are all called by TestFrame.java
//==============================================================================
public void reset()
{
   if(red == true)
   {
      espdu.setEntityLocationX(50f);					  // mls

      // state variables (other than location) zeroed
	    espdu.setEntityLocationY(2000f + idNum%30*50);
	    espdu.setEntityOrientationPsi((float)(Math.PI));      // mls

      // adjust shrike32 altitude to 37 below sea level
      if (idNum == 32)
      {
         espdu.setEntityLocationZ(37.0f);	      // mls
      }
      else
      {
         espdu.setEntityLocationZ(37.25f);	    // mls
      }// endif
   }// 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)// adjust hawk22 altitude to 41.15 below sea level
      {                        // mls
         espdu.setEntityLocationZ(41.15f);	// mls
      }                                        // mls
      else
      {                                        // mls
         espdu.setEntityLocationZ(41.5f);	     // mls
      }// end if else                          // mls
   }// end if else
   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);
   speed = 0;
   torque = 0;
   isCollided = false;
//   System.out.println("isCollided = " + isCollided);
// 	 System.out.println("sending RemoveEntity PDU...");

   // send notification to remove entity
	 behaviorStreamBuffer.sendPdu(repdu.getExemplar(), ipAddress, portNumber);
} // end reset()


public void sendFirePdu()
{
   // 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);
   } // end try
   catch(InterruptedException ie){}
}// end sendFirePdu()


public void hover()// stabilize the helo at a hover at the current location
{
   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);
}// end hover()


public void leftPedalTurn()
{
   espdu.setEntityAngularVelocityZ(-.1058f * 1.5f);
} // end leftpedalTurn()


public void rightPedalTurn()
{
   espdu.setEntityAngularVelocityZ(.1058f * 1.5f);
} // end rightPedalTurn()


//==============================================================================
// Kill Determination Methods
//==============================================================================
public void collectSensorPdus()
{
   // Vector firedPdus = new Vector();
   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();
         if(!shooter.equals(espdu.getEntityID()))
         {
            checkFirePdu((FirePdu)tempPdu);
            LinearVelocity missileVel = ((FirePdu)tempPdu).getVelocity();
         } // end if
      } // end if
      if (pduType.shortValue() == PduTypeField.ENTITYSTATE)
      {
         checkCollision((EntityStatePdu)tempPdu);
         EntityStatePdu tempPDU = (EntityStatePdu)tempPdu;
         Integer espduID =
                    new Integer(tempPDU.getEntityID().getEntityID().intValue());
         //this adds the received PDUs into the HashMap using the espduID as
         //the Key value.  Therefore only the most receint PDU is left in the
         //HashMap for the agent to look at in the sensed environment
         sensedPdus.put(espduID, tempPdu);
      } // end if

/*  TO BE IMPLEMENTED IN FUTURE WORK WHEN HELICOPTER CAN SHIFT TO DEFENSE ******

      if (pduType.shortValue() == PduTypeField.DATA)
      {
         Vector receivedData = new Vector();
         DataPdu dataPdu = (DataPdu)tempPdu;
         EntityID sender = dataPdu.getOriginatingEntityID();
         Integer senderID = new Integer(sender.getEntityID().intValue());
         if(senderID.intValue() == espdu.getEntityID().getEntityID().intValue())
         {
            // decode dataPdu and implement action
            DatumSpecification datum1 = dataPdu.getDatumInformation();
            for( int kx = 0; kx < datum1.getFixedDatumCount(); kx++ )
            {
               FixedDatum fixedDatum3 = datum1.fixedDatumAt(kx);
               Integer datumInteger = new Integer( fixedDatum3.getValue() );
               receivedData.addElement(datumInteger);
            }// end for
            interpretProtocol( receivedData );
         }// end if
*/
   } // end for
} // end collect FirePdus

public void interpretProtocol ( Vector receivedData )
{
   // Determine protocol and set booleans to take the appropriate action
   int protocol = ((Integer)receivedData.firstElement()).intValue();

   switch( protocol )
   {
      case 0:  int tacticalAction =
                      ((Integer)receivedData.elementAt(1)).intValue();
                      
               if( tacticalAction == OFFENSIVE ) setOffensive();
               if( tacticalAction == DEFENSIVE ) setDefensive(); 
               break;

      // ADD ADDITIONAL PROTOCOL DEFINITIONS HERE

      default: // do nothing

   }// end switch

}// end processDataPdu()

/**
 *  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 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.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);
      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");
      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)
      {
         // do nothing
      }
   else
   {
      // 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];

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


} // end class MotionInterpreter