// **********************************************************************
//  EXECUTIVE SUMMARY
// Module Name: Ownship.java
// Description: Definition of the Ownship class
// Author: Kent A. Watsen, http://www.mbay.net/~watsen
// **********************************************************************

package mil.navy.nps.eaiDemoBoids;


// **********************************************************************
//   IMPORTS
// **********************************************************************


// Standard classes
import java.*;
import java.awt.*;
import java.util.*;

// EAI classes
import vrml.*;
import vrml.external.Node;
import vrml.external.Browser;
import vrml.external.field.*;
import vrml.node.*;
import vrml.external.exception.*;

import netscape.javascript.JSObject;

// NPS library stuff
import mil.navy.nps.dis.*;
import mil.navy.nps.math.*;


// **********************************************************************
//   CLASS
// **********************************************************************


public class Ownship extends Object
  {
  static final boolean DEBUG        = true;

  static final String vrmlString    = "Shape { appearance Appearance { material Material { diffuseColor 0 1 1 } } geometry IndexedFaceSet { coord Coordinate { point [0 0 -3, -2 0 3, 2 0 3, 0 2 3] } coordIndex [0 2 1 -1, 0 1 3 -1, 0 3 2 -1, 1 2 3 -1] ccw FALSE} }";
  static final float  initPos[]     = {0f, 5f, 0f}; // x, y, z
  static final float  initPosDt[]   = {0f, 0f, 0f}; // aka: velocity
  static final float  initAng[]     = {0f, 0f, 0f}; // heading, pitch, roll
  static final float  initAngDt[]   = {0f, 0f, 0f}; // aka: angular rates
  public static final int    UP            = 0;
  public static final int    DOWN          = 1;
  public static final int    LEFT          = 2;
  public static final int    RIGHT         = 3;
  public static final int    FASTER        = 4;
  public static final int    SLOWER        = 5;
  static final float  aDeg          = ((float)Math.PI)/180.0f;
  NIU                 niu           = null; // for sending pdus
  Node                myNode[]      = null;
  Node                myTransform[] = null;
  EventInSFVec3f      myPos         = null;
  EventInSFRotation   myRot         = null;
  Quaternion          tmpQuat       = null;
  int                 currFrame     = 0;
  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
  EntityStatePdu      espdu         = null;
  float               pduTimer      = 0.0f;

  public Ownship (Browser browser, NIU _niu, String tmpParam)
    {
    short entityID [] = {-1, -1, -1};
    
    System.out.println("Ownship:  in constructor");

    // use passed browser now
    myNode = browser.createVrmlFromString(vrmlString);

    myTransform = browser.createVrmlFromString("Transform {}" );

    System.out.println("Ownship myNode and myTransform set");

    // save reference to NIU for later use
    niu = _niu;

    if (tmpParam != null && tmpParam.length() != 0)
      {
      StringTokenizer st = null;
      st = new StringTokenizer(tmpParam, " ,");  // skip 'space' & ',' 
      try
        {
        entityID[0] = (short)(Integer.valueOf(st.nextToken())).intValue();
        entityID[1] = (short)(Integer.valueOf(st.nextToken())).intValue();
        entityID[2] = (short)(Integer.valueOf(st.nextToken())).intValue();
        }
      catch (NoSuchElementException e)
        {
        System.out.println("Ownship bad ownship entityID passed");
        }
      }
    if(DEBUG)
      System.out.println("Ownship entity ID is set to " + entityID[0] + ", " + entityID[1] + ", " + entityID[2]);

    if(DEBUG)
      System.out.println("Ownship instantiating espdu");

    espdu = new EntityStatePdu();
    espdu.setEntityID(entityID[0], entityID[1], entityID[2]);

    if(DEBUG)
      System.out.println("Ownship instantiate espdu complete");

    EventInMFNode tmpAddChildren = (EventInMFNode) myTransform[0].getEventIn("addChildren");
    tmpAddChildren.setValue(myNode);

    myPos = (EventInSFVec3f) myTransform[0].getEventIn("set_translation");
    myRot = (EventInSFRotation) myTransform[0].getEventIn("set_rotation");

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

    // these are used for hpr->axis/angle conversions
    tmpQuat = new Quaternion();
    rot     = new float[4]; // axis/angle

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

    // now convert hpr (ang) to axis/angle (rot)
    tmpQuat.setEulers(ang);
    tmpQuat.getAxisAngle(rot);

    // finally, push values to vrml
    myPos.setValue(pos);
    myRot.setValue(rot);

    if(DEBUG)
      System.out.println("Ownship exiting constructor, about to send PDU...");

    // let the world know about it
    sendPDU();

    if(DEBUG)
      System.out.println("Ownship just sent PDU");

    if(DEBUG)
      System.out.println("Ownship:  completed constructor");

    }


  public void cycle()
    {
    float   dt                = 0.1f;  // assume 10Hz
    boolean thresholdExceeded = false;
    Vec3f   forward           = new Vec3f();

    // update hi-fi model

    // set velocity vector = speed*direction
    forward.set(0.0f, 0.0f, -speed);
    tmpQuat.setEulers(ang);
    tmpQuat.xform(forward);
    posDt[0] = forward.get(0);
    posDt[1] = forward.get(1);
    posDt[2] = forward.get(2);

    // now update first order equations
    pos[0] = pos[0] + dt*posDt[0]; // x
    pos[1] = pos[1] + dt*posDt[1]; // y
    pos[2] = pos[2] + dt*posDt[2]; // z
    ang[0] = ang[0] + dt*angDt[0]; // h
    ang[1] = ang[1] + dt*angDt[1]; // p
    ang[2] = ang[2] + dt*angDt[2]; // r

    // enforced caps on euler angles
    if (ang[0] < -180.0f) ang[0] = 360.0f - ang[0];
    if (ang[0] >  180.0f) ang[0] = ang[0] - 360.0f;
    if (ang[1] < -89.0f) { ang[1] = -89.0f; angDt[1] = 0.0f; }
    if (ang[1] >  89.0f) { ang[1] =  89.0f; angDt[1] = 0.0f; }
    if (ang[2] < -180.0f) ang[2] = 360.0f - ang[2];
    if (ang[2] >  180.0f) ang[2] = ang[2] - 360.0f;

    // now convert hpr (ang) to axis/angle (rot)
    tmpQuat.setEulers(ang);
    tmpQuat.getAxisAngle(rot);

    // finally, push values to vrml
    myPos.setValue(pos);
    myRot.setValue(rot);

    // deadreckon lo-res model
    drPos[0] = drPos[0] + dt*drPosDt[0]; // x
    drPos[1] = drPos[1] + dt*drPosDt[1]; // y
    drPos[2] = drPos[2] + dt*drPosDt[2]; // z
    drAng[0] = drAng[0] + dt*drAngDt[0]; // h
    drAng[1] = drAng[1] + dt*drAngDt[1]; // p
    drAng[2] = drAng[2] + dt*drAngDt[2]; // r

    // send pdu if threshold exceeded
    pduTimer = pduTimer + dt;
    if (pduTimer > 5.0f) // 5 sec update
      {
      pduTimer = 0.0f;
      sendPDU();
      return;
      }
    if (Math.abs(pos[0]-drPos[0]) > 2.0f) // 2 meters
      {
      sendPDU();
      return;
      }
    if (Math.abs(pos[1]-drPos[1]) > 2.0f) // 2 meters
      {
      sendPDU();
      return;
      }
    if (Math.abs(pos[2]-drPos[2]) > 2.0f) // 2 meters
      {
      sendPDU();
      return;
      }
    if (Math.abs(ang[0]-drAng[0]) > aDeg) // 1 degree
      {
      sendPDU();
      return;
      }
    if (Math.abs(ang[1]-drAng[1]) > aDeg) // 1 degree
      {
      sendPDU();
      return;
      }
    if (Math.abs(ang[2]-drAng[2]) > aDeg) // 1 degree
      {
      sendPDU();
      return;
      }
    }

  public void sendPDU()
    {
    // first set dr* vars from hi-fi vars
    System.arraycopy(pos, 0, drPos, 0, 3);
    System.arraycopy(posDt, 0, drPosDt, 0, 3);
    System.arraycopy(ang, 0, drAng, 0, 3);
    System.arraycopy(angDt, 0, drAngDt, 0, 3);

    // now fill in espdu
    espdu.setEntityLocationX(pos[0] - 20.0f);
    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

    // send it now
    niu.sendPDU(espdu);
    }

  public void update(int affect)
    {
    switch (affect)
      {
      case Ownship.UP:
        {
        angDt[1] = angDt[1] + 0.1f*aDeg;
        break;
        }
      case Ownship.DOWN:
        {
        angDt[1] = angDt[1] - 0.1f*aDeg;
        break;
        }
      case Ownship.LEFT:
        {
        angDt[0] = angDt[0] + 0.1f*aDeg;
        break;
        }
      case Ownship.RIGHT:
        {
        angDt[0] = angDt[0] - 0.1f*aDeg;
        break;
        }
      case Ownship.FASTER: // increase 0.1 m/s
        {
        speed = speed + 0.1f;
        break;
        }
      case Ownship.SLOWER: // decrease 0.1 m/s
        {
        speed = speed - 0.1f;
        break;
        }
      }
    }


  public void dispose()
    {
    // nothing to do just yet
    }


  public Node[] getNode()
    {
    return myTransform;
    }

  }