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.*;

/**
 *<P>
 * Class TeamActionInterpreter
 * @author - Thomas E. Miller
 * @version 1.0
 * Last Modification 20 September 2000
 * The TeamActionInterpreter.java file contains the logic that supports
 * and interprets user input from the TeamPanel.java.  This file also 
 * supports the aggregation and disaggregation of an independent human entity
 * to a group, and allows the user the ability to order coordinated, tactical 
 * actions among human and non-human entities.
 *<dt><B>Entity coordinate systems (right-hand rule applies):</B>
 *<PRE>
 *
 *          DIS                 VRML
 *
 *      -Z entity up         Y entity up
 *           ^                   ^
 *           |                   |
 *           |                   |
 *           |                   |
 *           +------> X          +-------> X    nose of entity body ( North )
 *          /                   /
 *         /                   /
 *        /                   /
 *       Y                   Z
 *         right-hand side
 *         of entity body ( East )
 *
 *  Rotation angle axes (right-hand rule applies):
 *
 *           DIS        VRML      Angle of rotation
 *
 *  Roll      X          X         phi
 *  Pitch     Y          Z        theta
 *  Yaw       Z         -Y         psi
 *</PRE>
 */


public class TeamActionInterpreter extends Thread {
  
  /**
   *  pduSendInterval time delay between PDUs in seconds
   */
  
   private float pduSendInterval;      
  
  /**
   *  speed represents the magnitude of linear velocity in meters per second
   */
  
   private float speed;
   
  /**
   * df - provides output format for floating point numbers 
   */               
  
   private DecimalFormat df;
   
  /**
   * teamEspdu - data structure to hold team state to the VRML scene
   */	       
   
   private EntityStatePdu teamEspdu;
   
  /**
   * espdu - Pdu to send entity state to VRML scene
   */   
   
   private EntityStatePdu espdu;
   
  /**
   * heloPDU - data structure to hold the aggregated helicopter entity's state
   */       
   
   EntityStatePdu heloPDU;
   
  /**
   * fpdu - Pdu to send fire info to VRML scene 
   */
   
   private FirePdu fpdu;
   
  /**
   * copdu - PDU to send collision info to the VRML scene 
   */
                  
   private CollisionPdu copdu;
   
  /**
   * behaviorStreamBuffer - tie in to the network 
   */
    
   private BehaviorStreamBufferUDP behaviorStreamBuffer; 
   
  /**
   * dpdu - PDU to send detonation info to the VRML scene
   */ 
     
   private DetonationPdu dpdu;
   
  /**
   * cepdu - PDU to send simulation information, create entity 
   */
   
   private CreateEntityPdu cepdu;
   
  /**
   * cpdu - PDU to send information about the entity 
   */
   
   private CommentPdu cpdu;
   
  /**
   * repdu - PDU to send simulation information, remove entity 
   */
   
   private RemoveEntityPdu repdu;
   
  /**
   * datum 
   */
   
   private DatumSpecification datum;
   
  /**
   * variableDatum 
   */
   
   private VariableDatum variableDatum;
   
  /**
   * geometryURL - URL where geometry is located for the entity
   */
   
   private String geometryURL;
   
  /**
   * geometryURLbyteArray - URL where geometry is stored in a byte array
   * for the entity 
   */
   
   private byte geometryURLbyteArray[];
   
  /**
   * geometryURLlongArray - - URL where geometry is stored in a long array
   * for the entity 
   */
   
   private long geometryURLlongArray[];
   
  /**
   * ipAddress - the IP address the simulation 
   */
   
   private String ipAddress;
   
  /**
   * portNumber - port number for IP multicast 
   */
   
   private int portNumber;
   
  /**
   * id - id marking of the entity
   */
   
   private int id;
   
  /**
   * isCollided - collision state of the entity
   */
   
   private boolean isCollided;
   
  /**
   * myPanel - the teamPanel which feeds user input to this class
   */
   
   private TeamPanel myPanel;
   
  /**
   * oldAltitude - most recent previous altitude
   */
   
   private double oldAltitude;
   
  /**
   * newAltitude -  most current altitude
   */
   
   private double newAltitude;
   
    
  /**
   * visibilityCode - Enumeration of visible state, which is sent to the VRML
   * scene via articulated parameters
   */
      
   private double visibilityCode;
   
  /**
   * ApVisibility - articulated parameter holding the aggregation entity's
   * visibility state
   */                      
   		      
   private ArticulationParameter ApVisibility;
   
  /**
   * ApLeader - articulated parameter holding Leader's mounted state
   */ 
   
   private ArticulationParameter ApLeader;
   
  /**
   * ApRifle1 - articulated parameter holding Rifle1's mounted state
   */
   
   private ArticulationParameter ApRifle1;
   
  /**
   * ApRifle2 - articulated parameter holding Rifle2's mounted state
   */
   
   private ArticulationParameter ApRifle2;
   
  /**
   * ApGrenadier - articulated parameter holding Grenadier's mounted state
   */
   
   private ArticulationParameter ApGrenadier;
   
   
  /**
   * ApAutogun - articulated parameter holding Autogun's mounted state
   */
   
   private ArticulationParameter ApAutogun;
   
   
  /**
   * ApHuey - articulated parameter holding Huey's mounted state
   */
   
   private ArticulationParameter ApHuey;
   
  /**
   * red - boolean holding the entitiy's team designation
   */
   
   private boolean red;
   
  /**
   * alive - boolean showing whether or not the entity is active
   */
   
   private boolean alive;
   
  /**
   * terrain - an instance of the CollisionDetectionTerrainReader class.
   * Used to determine an entity's height and orientation in the VRML scene.
   */
   
   private CollisionDetectionTerrainReader terrain;
   
   
  /**
   * oldHeading - previous heading of the entity
   */
   
   private double oldHeading;
   
  /**
   * newHeading - the most current heading of the entity
   */
   
   private double newHeading;
   
  /**
   * first - first time thru the run cycle for panel updates 
   */
   
   private boolean first;  // first time thru the run cycle for panel updates 
    
  /**
   * sleepInterval - the amount of time an entity sleeps between Pdu sends in
   * milliseconds per sleep
   */
   
   private int sleepInterval = 100; 
   
  /**
   * sleepCounter - the number of times that the whileLiving method while loop
   * executes between action times
   */ 
    
   private int sleepCounter  =  0;
   
  /**
   * userChangeHeard - a boolean flag that when true forces the action 
   * interpreter to send a PDU
   */
   
   private boolean userChangeHeard = false;
       
  /**
   * Meters above local terrain for entity's geometric center.
   */
   
   public double verticalOffset = 0.0;
   
  /**
   * Visibility code. NOTHING.
   */
    
   public final double NOTHING = -1.0;
   
  /**
   * Visibility code.  VISIBLE.
   */
   
   public final double VISIBLE   = 0.0; 
   
   private boolean bound; 
   
   private boolean firstBound;
   
   private boolean firstNewBound; 
   
   private double startX;
   
   private double startY;
   
   private float heightAboveGround;
   
   private boolean heloLoaded = false;
   
   private boolean load = false;
   
   double groundHeight = 0;
   
   double [] normalHeight = new double [4];
   
   private boolean launchLoadHelo = true;
   
   private boolean unload = false;
   
   private boolean unloadLeft = true;
   
   private boolean firstUnload = true;
   
  
//=============================================================================
// Constructors & Start
//=============================================================================
  /**
   * Constructor
   * Constructor
   * @param String ipAddr - a string defing the IP address
   * @param int portNum - the port number receiving the network packets
   * @param int timeToLive - Number of routing hops allowed
   * @param short siteNum - site Number of simualtion
   * @param short appNum - application number of the simulation
   * @param short idNum - entiy's id number
   * @param String marking - entity's text name
   * @param boolean rtpHeaderEnabled - wheteher or not the real-time header
   *                                   is enabled or not
   * @param HumanPanel panel - the human control panel
   * @param boolean independent - the entity's mounted state
   * @param BehaviorStreamBufferUDP bsb
   * @param CollisionDetectionTerrainReader ter - instance of CollisionDetection
   *                           TerrainReader for the Fort Irwin terrain database
   *
   */
   
   public TeamActionInterpreter ( String ipAddr, int portNum, int timeToLive, 
                                   short siteNum, short appNum, short idNum,
                                   String marking, boolean rtpHeaderEnabled, 
                                   TeamPanel panel, BehaviorStreamBufferUDP bsb, 
                                   CollisionDetectionTerrainReader ter )  {
                                   
      behaviorStreamBuffer = bsb;
      terrain = ter;
      
      speed  = 0.0f;                  // initially human no fwd speed
      pduSendInterval = 0.5f;	      // seconds between pdu sends
      df = new DecimalFormat( "###.00" );
      
      // instantiate an cepdu for announcing the creation of an entity
      cepdu = new CreateEntityPdu ();	
      cepdu.setRtpHeaderEnabled ( rtpHeaderEnabled );
      cepdu.setRequestID (( long ) id );
      
      // instantiate an cpdu, used to store the URL address
      cpdu = new CommentPdu ();		
      cpdu.setRtpHeaderEnabled ( rtpHeaderEnabled );
      
      // instantiate a repdu, used to remove an entity
      repdu = new RemoveEntityPdu ();	   
      repdu.setRtpHeaderEnabled ( rtpHeaderEnabled );
      repdu.setRequestID (( long ) id );

      datum = new DatumSpecification ();
      variableDatum = new VariableDatum ();
      geometryURLbyteArray = new byte [ 120 ];
      geometryURLlongArray = new long [ 120 ];
      // note generic geometry convention not yet established, 
      // this has EspduTransform embedded...
      // place URL address here
      geometryURL = new String ("http://www.web3D.org/WorkingGroups/vrtp/" +
                                "dis-java-vrml/demo/helicopter/NancySquad.wrl");	

      teamEspdu = new EntityStatePdu ();
      teamEspdu.setRtpHeaderEnabled ( rtpHeaderEnabled );
      
      // instantiate an espdu, used as a data structure
      espdu = new EntityStatePdu();  
      espdu.setRtpHeaderEnabled(rtpHeaderEnabled);
      // instantiate a fire Pdu, used as data structure
      fpdu = new FirePdu();
      fpdu.setRtpHeaderEnabled(rtpHeaderEnabled);
      copdu = new CollisionPdu();
      copdu.setRtpHeaderEnabled(rtpHeaderEnabled);
      dpdu  = new DetonationPdu();
      dpdu.setRtpHeaderEnabled(rtpHeaderEnabled);
      heloPDU = new EntityStatePdu ();;
      short [] heloID = new short [ 3 ];
      heloID [ 0 ] = siteNum;
      heloID [ 1 ] = appNum;
      heloID [ 2 ] = ( short ) 406;
      // set helo id triplet 
      heloPDU.setEntityID ( heloID [ 0 ], heloID [ 1 ], heloID [ 2 ]);
      
      System.out.println ("marking=" + marking + ", timeToLive=" + timeToLive);
      espdu.setMarking   (marking);
      
      alive = true;
      id = idNum;                 
      if (id < 30){
         red = false;}
      else { red = true;}
    
      if (red == true)  {
         // create entity id triplet
         short [] RedentityID = new short [ 3 ];       
	      RedentityID [ 0 ] = 0;
	      RedentityID [ 1 ] = 1;
	      RedentityID [ 2 ] = 98;
	      // set entity id triplet
	      teamEspdu.setEntityID ( RedentityID [ 0 ], RedentityID [ 1 ], 
	                              RedentityID [ 2 ]);   
         
         espdu.setEntityLocationX ( -10f );	               	     //  meters
         espdu.setEntityLocationY ( 1990f + id % 35 * 50 );          //  meters   	
         espdu.setEntityOrientationPsi ( -6.7f + ( float ) Math.PI );//  radians
         terrain.getNormalHeight ( espdu.getEntityLocationX (), espdu.getEntityLocationY (), normalHeight );  
         espdu.setEntityLocationZ ( normalHeight [ 3 ] - verticalOffset );        //  meters
        
      }  //  end if
      
      else {
          // create entity id triplet
         short [] BlueentityID = new short [ 3 ];      
   	 BlueentityID [ 0 ] = 0;
   	 BlueentityID [ 1 ] = 1;
   	 BlueentityID [ 2 ] = 99;
   	 // set entity id triplet 
   	 teamEspdu.setEntityID ( BlueentityID [ 0 ], BlueentityID [ 1 ],
   	                         BlueentityID [ 2 ]);  
         
         espdu.setEntityLocationX ( -5010f );                     //  meters
         espdu.setEntityLocationY ( 1990f + id % 27 * 50 );       //  meters
         espdu.setEntityOrientationPsi ( -6.7f );                 //  radians  
         espdu.setEntityLocationZ ( 40.5f - verticalOffset );     //  meters		
      
      }  //  end else
      
      
      short [] entityID = new short [ 3 ];
      entityID [ 0 ] = siteNum;
      entityID [ 1 ] = appNum;
      entityID [ 2 ] = ( short ) id;
      // set entity id triplet into the espdu and fire pdu
      espdu.setEntityID ( entityID [ 0 ], entityID [ 1 ], entityID [ 2 ]);  
      fpdu.setFiringEntityID ( espdu.getEntityID ());               
      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 ];
      
      }  //  end for
      variableDatum.setVariableDatumValue ( geometryURLlongArray );
      // add URL address as variable datum value
      datum.addVariableDatum(variableDatum);		

      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

      EntityID temp = espdu.getEntityID();
      oldHeading = newHeading = espdu.getEntityOrientationPsi();
      oldAltitude = newAltitude = groundHeight = espdu.getEntityLocationZ ();
      first = true;

      ApVisibility = new ArticulationParameter ();
      ApVisibility.setParameterValue (( double ) 0 );   // 
      espdu.addArticulationParameter ( ApVisibility );

      ApLeader = new ArticulationParameter();
      ApLeader.setParameterValue( (double) 0 );   // 
      espdu.addArticulationParameter(ApLeader);
      
      ApRifle1 = new ArticulationParameter();
      ApRifle1.setParameterValue( (double) 0 );   // 
      espdu.addArticulationParameter(ApRifle1);
      
      ApRifle2 = new ArticulationParameter();
      ApRifle2.setParameterValue( (double) 0 );   // 
      espdu.addArticulationParameter(ApRifle2);
      
      ApGrenadier = new ArticulationParameter();
      ApGrenadier.setParameterValue( (double) 0 );   // 
      espdu.addArticulationParameter(ApGrenadier);
      
      ApAutogun = new ArticulationParameter();
      ApAutogun.setParameterValue( (double) 0 );   // 
      espdu.addArticulationParameter(ApAutogun);
      
      ApHuey = new ArticulationParameter();
      ApHuey.setParameterValue( (double) 0 );   // 
      espdu.addArticulationParameter(ApHuey);
      
      myPanel = panel;
      isCollided = false;
      ipAddress = ipAddr;
      portNumber = portNum;

      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
      bound = false;
      firstBound = true;
      startX = 0;
      startY = 0;
      firstNewBound = true;
      heightAboveGround = 0f;
       
   } //  end constructor
   

   public void run () {
   
   	startLiving ();
   	
   }	 // start sending espdus, called from TestFrame.java
         // calls live which is an infinite espdu sending loop
  
//=============================================================================================
// Properties
//=============================================================================================
   public void setIsCollided (boolean setValue) {isCollided = setValue;}
   public boolean getIsCollided() {return isCollided;}
   public void setLiving(boolean setValue) {alive = setValue;}
   public boolean getLiving() {return alive;}

  /**
   * setSpeed
   * @param float spd - the entity's speed in knots
   * @return void
   */
   
   public void setSpeed ( float spd ) {
   
      userChangeHeard = true;
      speed = .5144f * spd;    // convert from knots to meters/second
      espdu.setEntityLinearVelocityX((float)(speed * Math.cos(espdu.getEntityOrientationPsi())));
      espdu.setEntityLinearVelocityY((float)(speed * Math.sin(espdu.getEntityOrientationPsi())));
  
      
   } // end setSpeed()

  /**
   * getSpeed
   * @param void
   * @return double - the entity's speed in knots
   */
   
   public double getSpeed() {return speed;}
   
   public void setClimbRate ( float climbRate )  {
      
      espdu.setEntityLinearVelocityZ (( float ) ( 0.24 * climbRate * -1.0 ));
      
   }  //  end setClimbRate
   
   
  /**
   * setVisibilty
   * @param void
   * @return void
   */ 
   
   public void setVisibility ()  {
   
      userChangeHeard = true;
      if ( visibilityCode  == NOTHING )  {
      
         visibilityCode = VISIBLE;
         
      }  //  end if
      else  {
      
         visibilityCode = NOTHING;
         
      }  //  end else 
      
      ApVisibility.setParameterValue ( visibilityCode );
      
   }  // end setVisibility
  


   public void setTurnAngle(float turnAngle){
      userChangeHeard = true;
      espdu.setEntityAngularVelocityZ((float)(.1058 * (turnAngle * 3.1415 / 180.0)));
//********************Necessary???????     
//      espdu.setEntityOrientationPsi(espdu.getEntityOrientationPsi() +
//            espdu.getEntityAngularVelocityZ() * sleepInterval/100.0);
      espdu.setEntityLinearVelocityX((float)(speed *
             Math.cos(espdu.getEntityOrientationPsi())));
      espdu.setEntityLinearVelocityY((float)(speed *
	     Math.sin(espdu.getEntityOrientationPsi())));
   } // end setturnAngle()
  
    
  /**
   * getEspdu returns the vehicles current entity state protocol data unit
   * @param void
   * @return EntityStatePDU
   */ 
  
   public EntityStatePdu getEspdu () { return espdu; }
   
   
    
  /**
   * setIndependence
   * @param double indy - represents the value of the sending entity's 
   *   independence articulated parameter
   * @param short id - represents the the sending entities ID number
   * @return EntityStatePDU
   */ 
   
   public void setIndependence ( double indy, short id )  {
   
      userChangeHeard = true;  
      if ( id == 401 )  {
         ApLeader.setParameterValue ( indy );
     
      }  //  end if
      else if ( id == 402 )  {
      
         ApRifle1.setParameterValue ( indy ); 

      }  //  end else if
      else if ( id == 403 )  {
      
         ApRifle2.setParameterValue ( indy );
 
      }  // end else if
      else if ( id == 404 )  {
      
         ApGrenadier.setParameterValue ( indy );
         
      }  //  end else if
      else if ( id == 405 )  {
      
         ApAutogun.setParameterValue( indy );
         
      }  //  end else if
      else if ( id == 406 )  {
      
         ApHuey.setParameterValue( indy );
         
      }  //  end else if
      
      else {}
      
   }  //  end setIndependence

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

  /**
   * startLiving loops indefinitely sending espdus as needed. This loop
   * runs on its own thread
   * @param void
   * @return void
   */ 
     
   public void startLiving ()  {
      
      float deltaTime;
      
      // loops until mother thread exits thus killing this thread
      while(alive){	  	
         
         if ( bound )  {
         
            doBound ();
            	
         }  //  end if
         if ( load )  {
         
            doLoadHelo ();
            
         }  //  end if
         if ( unload )  {
         
            doUnloadHelo ();
            
         }  //  end if
         
         // send PDU prerequisites test
         // compare seconds, not msec; keepalive heartbeat.  
         // otherwise, send if userChangeHeard
	 if ((sleepCounter * sleepInterval >= pduSendInterval * 1000.0) ||  
		((userChangeHeard == true) && (sleepCounter * sleepInterval > 50)))  
	 {
	    System.out.println ("TeamActionInterpreter sending espdu");
	    espdu.makeTimestampCurrent();
	    // send espdu to the specified multicast address
	    behaviorStreamBuffer.sendPdu(espdu, "224.2.181.145", 62040);

	    sleepCounter=0;
	    userChangeHeard = false;
	    
         }  //  end if 
         
         deltaTime = (float) sleepInterval / 1000.0f;
         // simple kinematics to update X, Y, Z location in world coordinates
         double rotatedPitchX;
         double rotatedPitchY;
         double rotatedPitchZ;

         double rotatedRollX;
         double rotatedRollY;
         double rotatedRollZ;

         double turnAngle = espdu.getEntityOrientationPsi() +
          	                espdu.getEntityAngularVelocityZ() * deltaTime;

         rotatedRollY = rotatedPitchX = Math.cos(turnAngle);
         rotatedRollX = rotatedPitchY = Math.sin(turnAngle);
         rotatedRollX *= -1;

         float newXLocation =(float) (espdu.getEntityLocationX() +
   	       espdu.getEntityLinearVelocityX() * deltaTime);
         espdu.setEntityLocationX(newXLocation);
         float newYLocation= (float)(espdu.getEntityLocationY() +
	         espdu.getEntityLinearVelocityY() * deltaTime);
	      espdu.setEntityLocationY(newYLocation);
         
		
         // get normal and height at current terrain position
         
         terrain.getNormalHeight(( double ) newXLocation, ( double ) newYLocation, normalHeight );
 
         // use eqn of plane to get roll and pitch angles
         rotatedPitchZ = ( 0.0 - normalHeight[0] * rotatedPitchX -                
                          normalHeight[1] * rotatedPitchY ) / normalHeight[2];     
         rotatedRollZ = ( 0.0 - normalHeight[0] * rotatedRollX -                  
                         normalHeight[1] * rotatedRollY ) / normalHeight[2];      

         float thetaAngle = ( float ) Math.atan ( -1 * rotatedPitchZ );      
         float phiAngle   = ( float ) Math.atan ( rotatedRollZ );          

         System.out.println ( "** here is the (x,y) ** = " +
                             newXLocation + " " + newYLocation);

         groundHeight = (normalHeight[3] - verticalOffset);  // negative number offset = m above local terrain
         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.setEntityOrientationTheta ( thetaAngle );
         espdu.setEntityOrientationPhi ( phiAngle );
         espdu.setEntityOrientationPsi ( espdu.getEntityOrientationPsi () +
       	   espdu.getEntityAngularVelocityZ () * deltaTime );

         // for display in the Team control panel
         newHeading = espdu.getEntityOrientationPsi ();
         newAltitude = espdu.getEntityLocationZ ();
         
         if (( Math.abs (( oldHeading - newHeading ) * 180 / 3.1416 ) >= 1.0 ) ||
            ( Math.abs(oldAltitude - newAltitude) >= 1.0 )) {
            
            myPanel.updateHeading( newHeading );
            myPanel.updateAltitude( espdu.getEntityLocationZ (),
	                            groundHeight);
            oldHeading = newHeading;
            oldAltitude = newAltitude;
            
         }  //  end if
         
        if ( first )  {
        
            myPanel.updateHeading( newHeading );
            myPanel.updateAltitude ( espdu.getEntityLocationZ (), 
	                            groundHeight);
            first = false;
            
        }  //  end if
         
         // send espdu to the specified multicast address
         espdu.makeTimestampCurrent ();
         behaviorStreamBuffer.sendPdu ( espdu, ipAddress, portNumber );
         collectFirePdus ();
      
        sleepCounter++;
        
	try  {
	 
	   Thread.sleep ( sleepInterval );
	   
	}  //  end try
	catch ( InterruptedException ite )  {
	
	   System.out.println ( "sleep InterruptedException " + ite );
	   ite.printStackTrace ();
	   
	}  //  end catch
	
      } // end while
      
   } // end startLiving
   
   
  /**
   * doBound
   * @param void
   * @return void
   */
   
   public void doBound ()  {
      
      if ( firstBound == true )  {
         setSpeed ( 0f );
         setTurnAngle ( 0f );
         myPanel.setSpeedTextField ( " 0" );
         myPanel.setTurnRateTextField ( " 0" );
         myPanel.setSpeedSlider ( 0 );
         myPanel.setDirectionSlider ( 0 );
         startX = espdu.getEntityLocationX ();
         startY = espdu.getEntityLocationY ();
         firstBound = false;
         myPanel.boundPositions ();
             	
      }  // end if
      else  {
      
         double distance = Math.sqrt (( Math.pow ( espdu.getEntityLocationX () - startX, 2 )) +
                                   ( Math.pow ( espdu.getEntityLocationY () - startY, 2 )));

         myPanel.setAllInLine ();
         
         if ( distance < 20 )  {
         
            setSpeed ( 2f );
            myPanel.setSpeedTextField ( " 2" );
            myPanel.setSpeedSlider ( 2 );
            
         }  //  end if
         
         if (( distance >= 20.0 )  && ( firstNewBound == true ))  {

            myPanel.setNewBound ();
            setSpeed ( 0f );
            myPanel.setSpeedTextField ( " 0" );
            myPanel.setSpeedSlider ( 0 );
            firstNewBound = false;
                  
         }  //  end if
         
         if  (( distance >= 20.0 ) && ( myPanel.getAllInLine () == true ))  {
         
            firstBound = true;
            firstNewBound = true;
            
         }  // end if
         
      }  //  end else
      	
   }  //  end doBound
   

  /**
   * doLoadHelo
   * @param void
   * @return void
   */
   
   public void doLoadHelo ()  {
   
     myPanel.setAllInLine ();
   
     if ( ! heloLoaded )  {
     
        calculatePosition ();
     
     }  //  end if
     if (( heloLoaded ) && ( myPanel.getAllInLine () == true ) && ( launchLoadHelo ))  {
     
        myPanel.launchLoadHelo ();
        launchLoadHelo = false;
        
     }  //  end if
   
   }  //  end doLoadhelo
   
   
  /**
   * doUnloadHelo
   * @param void
   * @return void
   */
   
   public void doUnloadHelo ()  {
   
      if ( firstUnload )  { 
      
         if ( unloadLeft )  {
         
            espdu.setEntityOrientationPsi (( float ) 
               (espdu.getEntityOrientationPsi() - ( Math.PI / 2 )));
               
         }  //  end if
         else  {
         
            espdu.setEntityOrientationPsi (( float ) 
               ( espdu.getEntityOrientationPsi() + ( Math.PI / 2 )));
               
         }  // end else
         setSpeed ( 0f );
         myPanel.setSpeedTextField ( " 0" );
         myPanel.setSpeedSlider ( 0 );
         startX = espdu.getEntityLocationX ();
         startY = espdu.getEntityLocationY ();
         firstUnload = false;
               
      }  //  end if
      else  {
      
         myPanel.setAllExited ();
         if ( myPanel.getAllExited ())  {
      
            unload = false;
            firstUnload = true;
         
         }  //  end if
         
      }  //  end else
   
   }  //  end doLoadhelo
   
   
  /**
   * stopRun 
   * @param void
   * @return void
   */ 
   
   public void stopRun ()  {
   
     behaviorStreamBuffer.shutdown ();
     
   }  //  end stopRun
 
  
  /**
   * selfDestruct
   * @param void
   * @return void
   */
   
   public void selfDestruct ()  {

      copdu.setCollisionType(new UnsignedByte(CollisionTypeField.INELASTIC));
      copdu.setLinearVelocity(0.0f, 0.0f, 0.0f);
      copdu.makeTimestampCurrent();
      behaviorStreamBuffer.sendPdu(copdu, ipAddress, portNumber);
      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);

      // send espdu to the specified multicast address
      espdu.makeTimestampCurrent();
      espdu.setEntityLinearVelocityX (0.0f);
      espdu.setEntityLinearVelocityY (0.0f);
      espdu.setEntityLinearVelocityZ (0.0f);
      setTurnAngle (20.0f);  // spin!  then stop after DR times out
      ApVisibility.setParameterValue( NOTHING );
      behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber);

      isCollided = true;
      
      try{
        Thread.sleep(4500); 
      } // end try	  
      catch(InterruptedException ite){
        ite.printStackTrace();
      } // end catch
//      myPanel.reset();
      reset();     
   
   }  // end selfDestruct

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

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

  /**
   * reset
   * @param void
   * @return void
   */
     
   public void reset () {
   				
      if (red == true){
      	espdu.setEntityLocationX(-10f);               // mls		 
         espdu.setEntityLocationY(1990f + id%35*50);
         espdu.setEntityOrientationPsi(-6.7f+(float)Math.PI);          // mls
         espdu.setEntityLocationZ(36.3f - verticalOffset);		// mls
      }  //  end if 
      
      else {
      	espdu.setEntityLocationX(-5010f);             // mls
         espdu.setEntityLocationY(1990f + id%27*50);
         espdu.setEntityOrientationPsi(-6.7f);         // mls
         espdu.setEntityLocationZ(40.5f - verticalOffset);		// mls
      }  //  end else 
                                                    // 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);
      speed = 0;
      isCollided = false;
      
   } // end reset()

   

   public void brake(){                           //stop the team 
	   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;
   } // end public brake()
   
   
   public void setBound ( boolean newValue )  {
      
      bound = newValue;
      
      if ( bound == false )  {
      
         firstBound = true;
      
      }  //  end if
      
   }  //  end setBound
   
   
   public void setLoad ( boolean newValue )  {
       
      load = newValue;
      heloLoaded = false;
      launchLoadHelo = true;
   
   }  //  end setLoad
   
   
   public void setUnloadAndDirection  ( boolean newValue, boolean direction )  {
   
      unload = newValue;
      unloadLeft = direction;
   
   }  //  end setUnloadAndDirection

  


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

   public void collectFirePdus ()  {
   
      System.out.println ( "\nEntering collectFirePdus ()" );
      Vector firedPdus = new Vector ();
      Vector pdus = behaviorStreamBuffer.receivedPdus ();
      System.out.println ( "\n# of Pdus in the Vector = " + pdus.size ());
      
      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 );
          
         } // end if
      
      } // 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));
//
//      if (horizontalLaunchRange > shot.getRange())
//      {
//      	System.out.println("no hit, out of range");
//      	return;
//      }
//
//      double bulletFlightTime = horizontalLaunchRange/vLaunchMagnitude;
//      double bulletAtTargetRangeX = xLaunch + (vLaunchX * bulletFlightTime);
//      double bulletAtTargetRangeY = yLaunch + (vLaunchY * bulletFlightTime);
//      if (proximityOfKillDistance*proximityOfKillDistance < 
//	(  (bulletAtTargetRangeX- xNought) * (bulletAtTargetRangeX- xNought) 
//	 + (bulletAtTargetRangeY- yNought) * (bulletAtTargetRangeY- yNought))){
//        
//	 return; // no hit
//      }
//      System.out.println("Ack, You got me partner!!!*******");
//      copdu.setCollisionType(new UnsignedByte(CollisionTypeField.INELASTIC));
//      copdu.setLinearVelocity(0.0f, 0.0f, 0.0f);
//      copdu.makeTimestampCurrent();
//      behaviorStreamBuffer.sendPdu(copdu, ipAddress, portNumber);
//      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;
//      try{
//        Thread.sleep(4500); 
//      } // end try	  
//      catch(InterruptedException ite){
//        ite.printStackTrace();
//      } // end catch
//      myPanel.reset();
//      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 tank at launch.
    *	5.   Get X, Y, Z location of this tank at launch.
    *	6.   Calculate the distance between this tank and the launch point.
    *	7.   Calculate the relative velocity of the fire with respect to this tank.
    *	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 tank 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) );

//End terrain hit control   	

	if(CPA <= HIT_RADIUS) {
//Terrain hit control 
/*	boolean isFirePDUCollidedWithTerrain = false;
	double collisionCheckInterval = 0.1;
	double currentFirePduLocationX = launchLocationX;
	double currentFirePduLocationY = launchLocationY;
	double currentFirePduLocationZ = launchLocationZ;
	CollisionDetectionTerrainReader terrainAtCurrentFirePduLocation = new CollisionDetectionTerrainReader();

   double [] normalHeight = new double [4];	
	
	for(float ix = 0; ix <= timeToTravel; ix += collisionCheckInterval) {
		currentFirePduLocationX += launchVelocityX * collisionCheckInterval;
		currentFirePduLocationY += launchVelocityY * collisionCheckInterval;
		currentFirePduLocationZ += launchVelocityZ * collisionCheckInterval;
	   terrainAtCurrentFirePduLocation.getNormalHeight(
	   					currentFirePduLocationX, currentFirePduLocationY, normalHeight);
	   if(currentFirePduLocationZ >= normalHeight[3]) {//bullet hits terrain
	   	isFirePDUCollidedWithTerrain = true;
   		System.out.println("\nFire hits the terrain\n");
	   	break;
	   }//end if
	}//end for
            if (!isFirePDUCollidedWithTerrain){
*/	       // 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);
      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;
		try{
        	   Thread.sleep(4500);            // put thread to sleep for 250 ms, thus when
		   //Thread.sleep(200);  // time to complete the loop (hopefully about
		} // end try	  		// 50ms) is added the pdu send interval will
		catch(InterruptedException ite){
			ite.printStackTrace();
		} // end catch
//         	myPanel.reset();
         	reset();
          }//end if
//	}//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 ||
           entityPdu.getEntityID().getEntityID().intValue() > 400 )  {}
      
      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 ];

        
        if (distBtwn <= 0) {
        
                  
        } //endif
        
      } // end else
      
   } // end checkCollision

  
   
   public void calculatePosition ()  {     
      
      double interceptAngle;
      double interceptAngleDeg;
      double angle;
      float turnAngle;
      
      heloPDU = myPanel.getHeloEspdu ();

      double placement = heloPDU.getEntityOrientationPsi(); 

      double theXComp = ( heloPDU.getEntityLocationX ());
      double theYComp = ( heloPDU.getEntityLocationY ());
      double distance = ( Math.sqrt((Math.pow(theXComp, 2 ))
                       + ( Math.pow(theYComp, 2 ))));
      
                       
      if (( theXComp > 0 ) && (( theYComp >= 0 ) || (theYComp < 0 )))  {
      
         angle = Math.atan ( theYComp / theXComp );
      
      }  // end if
      
      else if (( theXComp == 0 ) && (theYComp < 0 ))  {
      
         angle = -Math.PI/2;
              
      }  // end if else
      
      else if (( theXComp == 0 ) && (theYComp > 0 ))  {
      
         angle = Math.PI/2;
              
      }  // end if else
      
      else if (( theXComp < 0 ) && (theYComp == 0 ))  {
      
         angle = -Math.PI;
              
      }  // end if else
      
      else if (( theXComp < 0 ) && ( theYComp < 0 ))  {
      
         angle = Math.atan ( theYComp / theXComp ) - Math.PI;
              
      }  // end if else
      
      else  {
      
         angle = Math.atan ( theYComp / theXComp ) + Math.PI;
         
      }  // end else
      
      if ( distance <= 1.0 )  {
      
         espdu.setEntityLinearVelocityX ( 0f );
         espdu.setEntityLinearVelocityY ( 0f );
         interceptAngle = espdu.getEntityOrientationPsi() - heloPDU.getEntityOrientationPsi ();
         //check to see if there is a turn greater than 180 degrees
         
         if ( interceptAngle > Math.PI )  {
         
            interceptAngle -= ( Math.PI * 2 );
            
         }  //  end if
         
         if ( interceptAngle < -Math.PI )  {
         
            interceptAngle += ( Math.PI * 2 );
            
         }  //  end if
         
         interceptAngleDeg = interceptAngle * ( 180.0 / Math.PI );
         
         if ( Math.abs ( interceptAngleDeg ) <= 3 )   {
               
            heloLoaded = true;
            
         }  //  end if
         
         turnAngle = - (( float ) interceptAngleDeg * 0.8f );
         setTurnAngle ( turnAngle );
         myPanel.setTurnRateTextField ( "" + ( int ) turnAngle );
         myPanel.setDirectionSlider (( int ) turnAngle );
            
      
      }  //  end if
      
      else  {
      
         interceptAngle = espdu.getEntityOrientationPsi () - 
                             heloPDU.getEntityOrientationPsi ();
         //check to see if there is a turn greater than 180 degrees
         if ( interceptAngle > Math.PI )  {
         
            interceptAngle -= ( Math.PI * 2 );
            
         }  //  end if
         
         if ( interceptAngle < -Math.PI )  {
         
            interceptAngle += ( Math.PI * 2 );
            
         }  //  end if
         
         interceptAngleDeg = interceptAngle * ( 180.0 / Math.PI ); 
         turnAngle = - (( float ) interceptAngleDeg * 0.8f );
         setTurnAngle ( turnAngle );
         myPanel.setTurnRateTextField ( "" + ( int ) turnAngle );
         myPanel.setDirectionSlider (( int ) turnAngle );
         double possibleSpeed;
         
         if ( distance <= 10.0 )  {
         
            possibleSpeed = ( distance * 0.5144 * pduSendInterval );
            
         }  //  end if
         
         else  {
         
            possibleSpeed = 3;
            
         }  //  end else
          
         espdu.setEntityLinearVelocityX (( float ) ( possibleSpeed * 0.5144f * 
                                       Math.cos ( espdu.getEntityOrientationPsi ()))); 
         espdu.setEntityLinearVelocityY (( float ) ( possibleSpeed * 0.5144f * 
                                       Math.sin ( espdu.getEntityOrientationPsi ())));
                                       
         myPanel.setSpeedTextField ( "" + (int) possibleSpeed );  
         myPanel.setSpeedSlider (( int ) possibleSpeed );
          
      }  //  end else
      
   }  //  end calculatePosition

} // end class TeamActionInterpreter