#include <math.h>
#include "Navigation.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "VehicleConfigurationIF.h"
#include "TimeP.h"
#include "MathP.h"
#include "NavUtils.h"
#include "matrixMath.h"

#ifndef PI
#define PI     3.14159265358979323846
#endif
#define R2D(r) (r*180.0/PI)

#define TaskIFtimeout 10

//
// This is how to bring in simulated data for debugging:
//
// #include "SimulatorIF.h"         
// SimulatorIF sim("simulator");
// double simPos[3], simVel[3], simEuler[3], simOmega[3];
// -Then, inside a method- sim.state( simPos, simVel, simEuler, simOmega );
//

Navigation::Navigation(int millisec)
  : PeriodicTask("navigation")
{
  Boolean debug = False;

  dprintf("New Navigation component constructor");

  initializeState();

  _log = new NavigationLog(this);

  VehicleConfigurationIF vehicleConfig("vehicleConfig");
  // Create core sensors
  _ahrs = new Ahrs("Ahrs", &_coreSensors, 15);
  _altimeter = new Altimeter("Altimeter", &_auxSensors);
  //
  // The integer argument after &_coreSensors is the number of 
  // consecutive bad readings that will be rejected.  The following argument
  // is the maximum allowable depth change, in one sample interval, that
  // can occur without being declared bad.
  //
  _depthSensor = new DepthSensor("DepthSensor", &_coreSensors, 20, 10.0);
  _tailCone = new TailCone("TailCone", &_coreSensors, 100);

  // Create auxillary sensors
  _gps = new Gps("Gps", &_auxSensors,
                 (const NavigationIF::Position *)&_state.position,
                 MaxGpsFixDepth);

  _lbl = new Lbl("Lbl", &_auxSensors);
  _dvl = new Dvl("Dvl", &_auxSensors);
  _useIns = False;
  if ( vehicleConfig.useIns() == 1. ) _useIns = True;
  if (_useIns) {
    _ins = new Ins("KearfottServer", &_auxSensors);
  }

  //Assume LBL is not running
  Lbl_up = False;

  _velocimeter = new Velocimeter("Velocimeter", &_auxSensors);

  // Create TaskInterfaces, and check to see that all core sensor
  // servers are running.
  connectSensors();

  addPeriodicCallback(millisec, (CallbackMethod )Navigation::callback);

  _firstCallback = True;
  //
  // Get stuff from worksite.cfg.
  //
  WorkSiteIF workSite("workSite");
  // Get magnetic variation
  _magneticVar =  workSite.magneticVariation();
  _utmZone = workSite.utmZone();
  _latitude  = workSite.latitude();
  _longitude = workSite.longitude();
  //
  // Initialize the position state.  Below, position.x is the Northing,
  // and position.y is the Easting, and they're in UTM coordinates.
  //
  NavUtils::geoToUtm(_latitude, _longitude, _utmZone,
                     &_state.position.x, &_state.position.y);

  dprintf(" Navigation: The utm zone is %d\n", workSite.utmZone());
  dprintf(" Navigation: The lat/long in workSite.cfg,  %f, %f\n",
            R2D(workSite.latitude()), R2D(workSite.longitude()));

  dprintf(" Navigation: The lat/long in workSite.cfg, "
          "cvt'ed to UTM is %f, %f\n",
            _state.position.x, _state.position.y);

  m_lastPingTime = 0.;
  //
  // Specify whether to use the PSA916 or the DVL as the altimeter, if both
  // are present.  
  //
  // This implementation below where a number is read in from vehicle.cfg is
  // crude.  Probably a better way is to use the StringAttribute family to 
  // read in a string from vehicle.cfg, but it is unfortunately rather 
  // complicated. Do it later...
  //
  _altimeterInstrument = NOT_SPECIFIED;


  if( vehicleConfig.altimeterInstrument() == 0. )
  {
    _altimeterInstrument = DVL;
  } 
  else if( vehicleConfig.altimeterInstrument() == 1. )
  {
    _altimeterInstrument = PSA;
  }
  //
  // Use the Lbl for nav, or just log it?
  //
  _useLbl = False;
  if( vehicleConfig.useLbl() == 1. ) _useLbl = True;



}


Navigation::~Navigation()
{
  // Delete NavSensor objects
  NavSensor *sensor;
  int i;
  for (i = 0; i < _coreSensors.size(); i++) {
    _coreSensors.get(i, &sensor);
    delete sensor;
  }

  for (i = 0; i < _auxSensors.size(); i++) {
    _auxSensors.get(i, &sensor);
    delete sensor;
  }

  delete _log;
}


void Navigation::initializeState()
{
  _state.attitude.roll = _state.attitude.pitch = _state.attitude.yaw = 0.;

  _state.attitude.omega_B_x = _state.attitude.omega_B_y =
    _state.attitude.omega_B_z = 0.;

  _state.position.x = _state.position.y = _state.position.z = 0.;
  _state.position.xRate = _state.position.yRate = _state.position.zRate = 0.;
  _state.position.altitude = 1000.;
  _state.position.depthRate = _state.position.altitudeRate = 0.;

  _northing = _easting = 0.;
}


void Navigation::connectSensors()
{
  int i;

  // Create TaskInterfaces core sensors, and check to see that all
  // servers are present
  Boolean coreMissing = False;
  for (i = 0; i < _coreSensors.size(); i++) {
    NavSensor *sensor;
    _coreSensors.get(i, &sensor);

    // Create the TaskInterface and connect to its server
    sensor->connect(TaskIFtimeout);

    // Verify that server is out there
    if (!sensor->taskIF()) {
      Syslog::write("Navigation: Server for core sensor \"%s\" not found",
                    sensor->name());
      coreMissing = True;
    }
  }

  // Create TaskInterfaces for auxillary sensors
  for (i = 0; i < _auxSensors.size(); i++) {
    NavSensor *sensor;
    _auxSensors.get(i, &sensor);

    // Create the TaskInterface and connect to its server
    sensor->connect(TaskIFtimeout);

    // Verify that server is out there
    if (!sensor->taskIF()) {
      Syslog::write("Navigation: Server for aux sensor \"%s\" not found",
                    sensor->name());
    }
  }
  //
  // Definitions:
  //
  // The following are true if:
  //
  //  _device->valid() - The device is connected, the server thread is present
  //                     and operating, _status == Ok, and the current 
  //                     measurement is valid.
  //
  //  _device->status()- Is an enum { Ok, Initializing, Offline, Error}.  If
  //                     the device isn't present, _status == Initializing.
  //
  //  _device->taskIF()- The task interface exists. (createTask() completed)
  //                     
  //  _device->taskIF()->connected() -  The server is present and connected?
  //
  // The "if" statement below checks ->taskIF(), and not ->valid, because we
  // just want to know if the device is present and healthy, not if the current
  // measurement is valid.  
  //
  if( !_altimeter->taskIF() && !_dvl->taskIF() )
  {
    // 
    // Both the altimeter (the PSA916) and the Dvl are declared above as 
    // auxiliary sensors, but really one of them is a core sensor.  The problem
    // is that as long as one is present, the other need not be.  So, here
    // we'll require that at least one of them is present.
    //
    Syslog::write(" Navigation: Neither an altimeter (PSA916) or a Dvl"
		  " was found. Aborting.\n");
    initiateAbort();
  }
  //
  // Override the altimeter selection in vehicle.cfg if only one instrument 
  // is present. 
  //
  if( !_altimeter->taskIF() )
  {
    //
    // PSA is not present, but the DVL is.  Use the DVL instead.
    //
    if( _altimeterInstrument != DVL )
    {
      _altimeterInstrument = DVL;
      Syslog::write(" Navigation:: The DVL will be used as the altimeter.\n"
		    " The instrument specified in vehicle.cfg is not"
		    " present.\n");
    }
  }
  else if( !_dvl->taskIF() )
  {
    //
    // DVL is not present, but the PSA is.  However, the PSA may not be 
    // pointing down.  Abort if the PSA is not explicity selected in
    // vehicle.cfg to be use as the altimeter.
    //
    if( _altimeterInstrument != PSA )
    {
      Syslog::write(" Navigation:: The PSA is present, but was not selected \n"
		    " to be used as the altimeter in vehicle.cfg.  Abort.\n");
      initiateAbort();
    }
  }
  else
  {
    // Both are present.  vehicle.cfg must specify which to use.
    if( _altimeterInstrument == NOT_SPECIFIED )
    {
      Syslog::write("Neither the PSA916 nor the Dvl was specified "
		    "as the altimeter in vehicle.cfg.  Aborting.\n");
      initiateAbort();
    }    
  }
  //
  // Set the flag Lbl_up if we can connect to the lbl server.  We'll only check 
  // this once, and assume that if the server is up, it will remain up for 
  // the duration of the mission.
  //
  if (_lbl->taskIF())
  {
    Lbl_up = True;
    //initialize matrices for Jerome's LBL algorithm
    //comment the following line out if using BAM-DY algorithm
    init_matrices();
  }

  if (coreMissing) {
    // Fatal error if core sensor(s) missing
    Syslog::write("Server for one or more core sensors not found");
    initiateAbort();
  }
}


void Navigation::callback()
{
  struct timespec timeSpec;
  clock_gettime(CLOCK_REALTIME, &timeSpec);
  _currentTime = Time::seconds(&timeSpec);

  if( _firstCallback ) {
    _firstCallback = False;
    _navLoopInterval = 0.;
    _navStartTime = _currentTime;
  }
  else
    _navLoopInterval = _currentTime - _lastTime;

  _lastTime = _currentTime;

  // Read sensors
  readSensors();

  // Process data and write to output
  processSensorData();

}


void Navigation::readSensors()
{
  int i;
  Boolean abort;
  NavSensor *sensor;
  Boolean debug = False;

  // Read each core sensor
  for (i = 0; i < _coreSensors.size(); i++) {
    _coreSensors.get(i, &sensor);

    dprintf("Navigation::readSensors() - sensor %s", sensor->name());
    sensor->read();

    if (sensor->failed()) {

      // Too many bad hits for this core sensor; abort!
      Syslog::write("Navigation - %d consecutive bad readings from sensor %s",
                    sensor->nConsecutiveBad(), sensor->name());

      Syslog::write("Navigation - aborting!");

      initiateAbort();
    }
  }

  // Read each aux sensor
  for (i = 0; i < _auxSensors.size(); i++) {
    _auxSensors.get(i, &sensor);
    sensor->read();
  }
}


void Navigation::processSensorData()
{
  Boolean reset;

  reset = 0;

  if (_ahrs->valid()) {
    _ahrs->attitude(&_state.attitude);

    if (_ahrs->magneticCompass()) {
      // Correct heading for magnetic variation
      _state.attitude.yaw =
        PI + Math::modPi( _state.attitude.yaw  + _magneticVar - PI);
    }
  }
  else {
    // Estimate from previous?
  }

  if (_depthSensor->valid()) {
    _state.position.z = _depthSensor->depth();
  }
  else {
    // Estimate from previous?
  }
  //
  // Now, we'll use either the Dvl or the altimeter/rangefinder to measure 
  // altitude.
  //
  switch( _altimeterInstrument )
  {

  case DVL:
    //
    // Only update if the dvl sees the bottom, and the data is new.
    //
    if ( _dvl->valid() && (_dvl->data.bottomStatus == 0) && _dvl->newData() ) 
    {
      _state.position.altitude = _dvl->altitude(&_state.attitude);
    }
    break;

  case PSA:
    if( _altimeter->valid() )
    {
      _state.position.altitude =
	_altimeter->altitude(&_state.attitude);
    }
    break;

  default:
    Syslog::write(" Navigation: Error - altimeterInstrument == %d"
		  " doesn't exist.  Abort.\n", _altimeterInstrument );
    initiateAbort();
    break;
  }
  //
  // Measure the forward speed through the water, commonly known as "u".
  //
  if( _dvl->valid() && (_dvl->data.waterStatus == 0) )
  {
    if( _dvl->newData() )
    {
      _waterSpeed = _dvl->data.waterMassVelocity[0];
    }
  }
  else if( _tailCone->valid() ) 
  {
    _waterSpeed = _tailCone->waterSpeed();
  }
  else 
  {
    // if (_velocimeter->valid()) {...}
    // Estimate from previous?
  }
  //
  // Begin NAVIGATION algorithms.  Determine x/y position.
  //
  _gpsValid = _gps->valid();
  if ( _gpsValid )
  {
    Boolean debug = False;
    //
    // Update position from GPS fix
    //
    dprintf("Navigation:: - using GPS reading...");

    _latitude  = _gps->fix.latitude;
    if( _gps->fix.nsHemisphere == GpsIF::Southern ) _latitude  *= -1.;

    _longitude = _gps->fix.longitude;
    if( _gps->fix.ewHemisphere == GpsIF::Western  ) _longitude *= -1.;

    NavUtils::geoToUtm(_latitude, _longitude, _utmZone,
                       &_state.position.x, &_state.position.y);

    _northing = _state.position.x;
    _easting = _state.position.y;

    dprintf("GPS northing = %f, GPS easting = %f",_northing,_easting);

    //reset LBL algorithm to use this GPS fix as new initial fix
    if (Lbl_up){
    _lbl->lbl_params.east_init = _state.position.y;
    _lbl->lbl_params.north_init = _state.position.x;
     reset = 1;
    }
  }
  //
  // Call the Lbl/Jerome algorithm and log the solution.
  //
  if( Lbl_up ) lblPosition(reset);
  //
  // Now, if _useLbl == True, use the Lbl/Jerome solution for control.  
  // Otherwise, use the DVL.  If the Dvl is invalid, dead-reckon.
  //
  if( Lbl_up && _useLbl )
  {
    _state.position.x = _filter_north;
    _state.position.y = _filter_east;
  }

  else if( _dvl->valid() )        //The Dvl has bottom or water lock.
  {
    if( _dvl->newData() )    //The Dvl updates more slowly than this Nav loop.
    {
      double deltaT, cpsi, spsi, ctheta, stheta, cphi, sphi, T_LV_B[3][3];
      double vel_B_N_LV[3], roll;

      Boolean debug = False;
      //
      // BEWARE:  The AHRS puts out projected, or space-fixed, roll and pitch 
      // angles.  THESE ARE NOT EULER ANGLES.  The following relation converts
      // the space-fixed roll angle into a 2-1 Euler.  See Wertz, p.226.
      //
      // At 20 Deg pitch, the Euler roll and the space-fixed roll differ 
      // by about 20 milli-Radians.
      //
      roll = atan( cos(_state.attitude.pitch)*tan(_state.attitude.roll) );

      cpsi   = cos(_state.attitude.yaw);
      spsi   = sin(_state.attitude.yaw);
      ctheta = cos(_state.attitude.pitch);
      stheta = sin(_state.attitude.pitch);
      cphi   = cos(roll);
      sphi   = sin(roll);
      //
      // Form the direction cosine matrix that recoordinatizes a vector from the 
      // body frame (B) into the local-level-local-vertical (LV) navigation 
      // frame, which is (North, East, Down).  See Fossen, p.10, Eqn 2.11.
      //
      // N is the inertially fixed, or Newtonian frame.
      //
      // The transformation T_B_LV, is 3-2-1 Euler.  The matrix below, T_LV_B, is
      // the transpose.
      //
      T_LV_B[0][0] =  cpsi*ctheta; 
      T_LV_B[1][0] =  spsi*ctheta; 
      T_LV_B[2][0] = -stheta; 

      T_LV_B[0][1] = -spsi*cphi + cpsi*stheta*sphi; 
      T_LV_B[1][1] =  cpsi*cphi + sphi*stheta*spsi; 
      T_LV_B[2][1] =  ctheta*sphi; 

      T_LV_B[0][2] =  spsi*sphi + cpsi*cphi*stheta; 
      T_LV_B[1][2] = -cpsi*sphi + stheta*spsi*cphi;
      T_LV_B[2][2] =  ctheta*cphi; 
      //
      // Transform the bottom-referenced velocity, as measured by the dvl, from 
      // the body frame into the navigation frame.  If only water-referenced 
      // velocity is available, we'll substitute it in place of the other, 
      // keeping in mind that in this case the notation vel_B_N_LV isn't quite
      // correct, as it specifies an intertial (bottom) reference.
      //
      if( _dvl->data.bottomStatus == 0 )
	TVMult( vel_B_N_LV, T_LV_B, _dvl->data.bottomTrackVelocity );
      else if( _dvl->data.waterStatus == 0 )
	TVMult( vel_B_N_LV, T_LV_B, _dvl->data.waterMassVelocity );
      else
	Syslog::write(" Navigation:: Error - _dvl->valid is true, but neither\n"
		      " water mass nor bottom referenced velocities"
		      " are valid.\n");

      // deltaT is the time duration since the last position update (NOT 
      // necessarily the last Dvl measurement).
      //
      deltaT = _dvl->data.pingTime - m_lastPingTime;
      m_lastPingTime = _dvl->data.pingTime;
      //
      // Check to see if the DVL has just started, or has been invalid 
      // for awhile. If so, skip this reading.
      //
      if( (deltaT > 0.) && (deltaT < 2.0) )
      {
	//
	// Integrate to get the position change, and add to the state:
	//
	_state.position.x += deltaT * vel_B_N_LV[0];
	_state.position.y += deltaT * vel_B_N_LV[1];
	dprintf(" Navigation - Deadreckoning with DVL.\n");
	dprintf(" Navigation - Dvl North increment = %f\n",
		deltaT * vel_B_N_LV[0]);
	dprintf(" Navigation - East increment = %f\n",deltaT * vel_B_N_LV[1]);
	dprintf(" Navigation - deltaT = %f\n",deltaT);
	dprintf(" Navigation - deltaNorm = %f\n",
		Vnorm(vel_B_N_LV) - Vnorm(_dvl->data.bottomTrackVelocity) );
      }
      else  
      {
	//
	// Need to account for the vehicle motion during this interval...
	//
	dprintf(" Navigation:: deltaT is impermissible.\n");
      } // if( deltaT ... )
    }   // if( _dvl->newData() )
    //
    // Here we do NOT dead reckon.  The Dvl is valid, but just doesn't have
    // new data yet.  We have to wait for it.
    //
  } 
  else  
  {
    //
    // The Dvl is not valid.  Here, deadReckon() adds the position change
    // since **the last (5Hz) sampling period** to the state.  
    //
    Boolean debug = False;
    deadReckon(); 
    dprintf(" Navigation:: Deadreckon because Dvl is invalid.\n");
  } //  if( _dvl->valid() )

  //if we are using the kearfott, just overwrite the relevant fields
  //with its state vector
  if (_useIns) {
    if ( _ins->valid() )
      {
	Boolean debug = False;
	//
	// Update position from INS fix
	//
	dprintf("Navigation:: - using INS reading...");

	_latitude  = _ins->inertialState.lat;
	_longitude = _ins->inertialState.lon;
	_gpsValid = True;

	NavUtils::geoToUtm(_latitude, _longitude, _utmZone,
			   &_state.position.x, &_state.position.y);

	_northing = _state.position.x;
	_easting = _state.position.y;

	dprintf("ins northing = %f, GPS easting = %f",_northing,_easting);
	dprintf("ins latitude = %f, GPS longitude = %f",
		Math::radToDeg(_latitude),
		Math::radToDeg(_longitude));

	//update lbl position estimate
	if (Lbl_up){
	  _lbl->lbl_params.east_init = _state.position.y;
	  _lbl->lbl_params.north_init = _state.position.x;
	  reset = 1;
	}

	//update AHRS data

	_state.attitude.roll = _ins->inertialState.roll;
	_state.attitude.pitch = _ins->inertialState.pitch;
	_state.attitude.yaw = _ins->inertialState.yaw;
	_state.attitude.omega_B_x = _ins->inertialState.rollRate;
	_state.attitude.omega_B_y = _ins->inertialState.pitchRate;
	_state.attitude.omega_B_z = _ins->inertialState.yawRate;

      }

  }

  // Make data available to server and trigger NavigationIF::NewOutput event
  _output.write(&_state);


  triggerEvent(NavigationIF::NewOutput);

  // Write data to log
  _log->write();
}


void Navigation::deadReckon()
{
  Boolean debug = False;
  //
  // Dead Reckoning Computation:
  //
  double ds, dn, de;

  dprintf("Navigation::deadReckon() - waterSpeed=%.3f, dt = %f", 
	  _waterSpeed,_navLoopInterval);

  ds = _navLoopInterval * _waterSpeed * cos( _state.attitude.pitch );
  dn = ds * cos( _state.attitude.yaw );
  de = ds * sin( _state.attitude.yaw );

  _state.position.x += dn;
  _state.position.y += de;

  dprintf("Navigation::deadreckon() - x = %f, y = %f, dn = %f, de = %f", 
	  _state.position.x, _state.position.y,dn,de);
}

void Navigation::lblPosition(int reset)
{
  double good_tof[MAX_BEACON_NUMBER];
  Boolean debug = False;
  static int callctr = 0;

  _nfix = 0.0;
  _efix = 0.0;

  for (int i = 1; i <= _lbl->lbl_params.number_of_beacon; i++)
    good_tof[i] = 0.0;

  callctr++;
  if( _lbl->lbl_params.ping_time != 0.0 ) callctr = 0;
  dprintf(" Navigation::lblPosition - %d calls to j. p. since last ping.\n",
	  callctr);

  jerome_position(reset,&_lbl->lbl_params,_currentTime,_navLoopInterval,
  		  _state.attitude.pitch,_state.attitude.yaw, 
  		  _state.position.z,_waterSpeed,&_nfix,&_efix,
  		  good_tof,&_filter_north,&_filter_east,&_filter_depth,
  		  &_north_current,&_east_current,&_speed_bias,&_heading_bias);

  if( (_nfix != 0.0) || (_efix != 0.0) )
  {
    dprintf("lblPosition() - x = %.2f, y = %.2f \n"
	    "north fix = %.2f, east fix = %.2f  \n"
	    "north current = %.2f east current = %.2f\n"
	    "Time = %.2f\n",
	    _filter_north, _filter_east,_nfix,_efix, _north_current, 
	    _east_current, _currentTime);
  }
}

void Navigation::initiateAbort()
{
  // Just exit. Should really request abort from Supervisor/LayeredControl?
  exit(1);
}