/*-----------------------------------------------------------------------

  Class:        FishPduGenerator
  Author:       Mike McCarthy
  Last Update:  6 Jan. 1998

  Program Invocation: command line "java mil.navy.nps.testing.FishPduGenerator"

  This class generates a simulated "Fish" entity state PDU in the DIS protocol.  
  It creates an instance of the MulticastPduSender class and uses the instance 
  method sendMPDU to output the simulated DIS entity on the multicast group 
  address specified in the MulticastPduSender Class code

--------------------------------------------------------------------------*/

package mil.navy.nps.testing;

import mil.navy.nps.dis.*;

import java.io.*;
import java.lang.*;
import java.net.*;
import java.util.*;

public class FishPduGenerator
{
  MulticastPduSender	mcast;
  EntityStatePdu  	espdu;
  String                fishname;
  String		check;

 
  static final float  initPos[]     = {4000f, -5f, -4000f}; // x, y, z
  static final float  initPosDt[]   = {0f, 5f, 0f}; // aka: velocity
  static final float  initAng[]     = {0f, 0f, 0f}; // heading, pitch, roll
  static final float  initAngDt[]   = {0f, 0f, 0f}; // aka: angular rate
  float               speed         = 0;
  float               rot[]         = null; // axis, angle (radians)
  float               pos[]         = null; // x, y, z
  float               posDt[]       = null; // aka: velocity
  float               ang[]         = null; // heading, pitch, roll
  float               angDt[]       = null; // aka: angular rates
  float               drPos[]       = null; // x, y, z
  float               drPosDt[]     = null; // aka: velocity
  float               drAng[]       = null; // heading, pitch, roll
  float               drAngDt[]     = null; // aka: angular rates
  float               pduTimer      = 0.0f;  

//************************************************
//          main call
//************************************************

   public static void main(String argv[])	 
    { 
    FishPduGenerator myfish = new FishPduGenerator(); 
    myfish.cycle();  	//call for fish movement
    }//endmain

//***********************************************
//          constructor
//***********************************************

    public FishPduGenerator()
    {	

    
    espdu = new EntityStatePdu();
    mcast = new MulticastPduSender();
    fishname = new String("FishPdu");

    espdu.setMarking(fishname);
    
    System.out.println(check = espdu.getMarking());

    short[] entityID = new short[3];
    entityID[0] = 0;
    entityID[1] = 1;
    entityID[2] = 0;

    System.out.println("entity ID is set to [" + entityID[0] + ", " + entityID[1] + ", " + entityID[2] + "]");
    espdu.setEntityID(entityID[0], entityID[1], entityID[2]);

     // hi-fi vars
    pos     = new float[3]; // x, y, z
    posDt   = new float[3]; // aka: velocity
    ang     = new float[3]; // heading, pitch, roll
    angDt   = new float[3]; // aka: angular rates


    // lo-fi vars
    drPos   = new float[3]; // x, y, z

    drPosDt = new float[3]; // aka: velocity
    drAng   = new float[3]; // heading, pitch, roll
    drAngDt = new float[3]; // aka: angular rates


    // now set init vars

    System.arraycopy(initPos, 0, pos, 0, 3);
    System.arraycopy(initPosDt, 0, posDt, 0, 3);

    System.arraycopy(initAng, 0, ang, 0, 3);
    System.arraycopy(initAngDt, 0, angDt, 0, 3);

    {
    sendmcast();   //spit out new fish on mbone
    }
   

}//end MyFish construct

//*****************************************************
//		fish movement dynamics
//*****************************************************		
 

public void cycle()
 {    float dt = 	10.0f; 
   
   while(true){  
   
     for(int i = 1; i <= 128; i++){
            
       if(i <= 8){
              posDt[0] = 0f;   	//set X positional speed
	      posDt[1] = 5f;	//set Y positional speed
              ang[0] = 4.712f;  //set psi (heading)
              
	      pos[1] = pos[1] + dt*posDt[1]; //dr position Y
           
          if((i%2) ==  0){
             ang[0] = ang[0] - 0.05f;}
	 
                else {ang[0] = ang[0] + 0.10f;}//fi heading ocillation
           System.out.println("rotation=" + ang[0]);
           
       }//end if        
      
	 else if(i <= 64){
              posDt[0] = 5f;
	      posDt[1] = 0f;
              ang[0] =  0f;
              
	      pos[0] = pos[0] + dt*posDt[0];

             if((i%2) ==  0){
             ang[0] = ang[0] - 0.05f;}
	      
                else {ang[0] = ang[0] + 0.10f;}//fi heading ocillation
		  System.out.println("rotation=" + ang[0]);
           }//end elseif        
       
	 else if(i <= 96){
              posDt[0] = 5f;
	      posDt[1] = 0f;
              ang[0] = 1.571f;


              if((i%2) ==  0){
                ang[0] = ang[0] - 0.05f;}
	         
                else {ang[0] = ang[0] + 0.10f;}//fi heading ocillation
              System.out.println("rotation=" + ang[0]);
	      pos[1] = pos[1] - dt*posDt[1];
           }//end elseif        
      
	 else if(i <= 128f){
              posDt[0] = 0f;
	      posDt[1] = 5f;
              ang[0] = 3.142f;

              if((i%2) ==  0){
                 ang[0] = ang[0] - 0.05f;}
	         
                 else {ang[0] = ang[0] + 0.10f;}//fi heading oscillation
              System.out.println("rotation=" + ang[0]);
	      pos[0] = pos[0] - dt*posDt[0];
           }//end elseif        
      
   
     //sleep for 1 second, gives an approx. 1 Hz output 
    try
	 {
         Thread.sleep(10000);          // go to sleep
         }
         catch(InterruptedException interruptedException)
         {
         throw new
         RuntimeException("FishPduGenerator: Exceptional sleep ");
         }
         
     sendmcast();

System.out.println(check = espdu.getMarking() + " 224.2.181.145/62040");  // hard coded :(

	}//endfor

  }//endwhile        

}//end cycle() 

//*********************************************************
//              send multicast call method   
//*********************************************************
    
   public void sendmcast()
    { 	
    espdu.setEntityLocationX(pos[0]);
    espdu.setEntityLocationY(pos[1]);
    espdu.setEntityLocationZ(pos[2]);
    espdu.setEntityLinearVelocityX(posDt[0]);
    espdu.setEntityLinearVelocityY(posDt[1]);
    espdu.setEntityLinearVelocityZ(posDt[2]);
    espdu.setEntityOrientationPsi(ang[0]);   // h
    espdu.setEntityOrientationTheta(ang[1]); // p
    espdu.setEntityOrientationPhi(ang[2]);   // r
    espdu.setEntityAngularVelocityY(angDt[0]); // h
    espdu.setEntityAngularVelocityX(angDt[1]); // p
    espdu.setEntityAngularVelocityZ(angDt[2]); // r
    espdu.setMarking(fishname);

   

   

    mcast.SendMPDU(espdu); 
    }//end sendmcast() 
   
      
}// end FishPduGenerator.class