#ifndef _NAVIGATION_H
#define _NAVIGATION_H

#define CoreSensor True
#define AuxillarySensor False

#include "NavigationOutput.h"
#include "NavigationInput.h"
#include "NavSensor.h"
#include "Ahrs.h"
#include "EchoSounder.h"
#include "Multibeam.h"
#include "RangeFinder.h"
#include "DepthSensor.h"
#include "TailCone.h"
#include "Gps.h"
#include "Lbl.h"
#include "Dvl.h"
#include "Usbl.h"
#include "Ins.h"
#include "Velocimeter.h"
#include "NavigationLog.h"
#include "AcousticModemIF.h"

#include <math.h>

#define INS_AHRS_TIMEOUT 15

// This is structure used to contain information on the heading-dependent
// compass bias.
// Example use:
// compBias = new compassErrorT;
// correctedHeading = currHeading - compBias.evalCompassBias(currHeading);
struct compassErrorT{
  double* cosineCoeff;
  double* sineCoeff;
  double constCoeff;
  int seriesOrder;
  compassErrorT()
  {
    seriesOrder = 3;
    cosineCoeff = new double[seriesOrder];
    sineCoeff = new double[seriesOrder];
    constCoeff = -0.002800887506;
    cosineCoeff[0] = 0.0192245946634;
    sineCoeff[0] = 0.3049004455222;
    cosineCoeff[1] = 0.033519791431345;
    sineCoeff[1] = -0.0022754105288;
    cosineCoeff[2] = -0.0035822997664;
    sineCoeff[2] = -0.0054575460693;
  }

  ~compassErrorT()
  {
    delete [] cosineCoeff;
    delete [] sineCoeff;
  }
  
  double evalCompassBias(double psi)
  {
    double psi_bias = constCoeff;
    int i;
    
    for(i = 0; i < seriesOrder; i++) 
      psi_bias += cosineCoeff[i]*cos((i+1)*psi) + sineCoeff[i]*sin((i+1)*psi);
    
    return psi_bias;
  };
};


/*
CLASS
NewNavigation

DESCRIPTION
Collects and processes navigation data from various sensors

AUTHOR
Tom O'Reilly
*/
class Navigation {

  friend class NavigationLog;

public:

  Navigation();

  ~Navigation();

  char *name() { return "Navigation"; }
  ///////////////////////////////////////////////////////////////////
  // Used to be the Periodic callback function
  int process();

protected:

  ///////////////////////////////////////////////////////////////////
  // Initialize NavigationIF::State structure
  void initializeState();

  ///////////////////////////////////////////////////////////////////
  // Create TaskInterface objects for all NavSensors and connect to
  // their servers
  void connectSensors();

  ///////////////////////////////////////////////////////////////////
  // Check that the correct combination of the Dvl, Echosounder, or
  // both are present as required by the _altimeterInstrument switch.
  void verifyOASensors();

  ///////////////////////////////////////////////////////////////////
  // Read data from all NavSensors. Determine whether server is
  // still running and data is valid.
  void readSensors();

  ///////////////////////////////////////////////////////////////////
  // Process and log sensor data, write to navigation output
  virtual void processSensorData();

  ///////////////////////////////////////////////////////////////////
  // Compute position based on LBL net
  void lblPosition(int reset);

  ///////////////////////////////////////////////////////////////////
  // Compute position based on dead-reckoning (adjust with GPS if
  // available)
  void deadReckon();

  ///////////////////////////////////////////////////////////////////
  // Initiate mission abort
  void initiateAbort();

  // Core sensors
  Ahrs *_ahrs;
  DepthSensor *_depthSensor;
  TailCone *_tailCone;

  // Auxillary sensors
  EchoSounder *_echoSounder;
  EchoSounder::RangeToObstacle _rangeToObstacle;
  Multibeam *_multibeam;
  Gps *_gps;
  Lbl *_lbl;
  Velocimeter *_velocimeter;
  Dvl *_dvl;
  Ins *_ins;
  Usbl *_usbl;
  RangeFinder *_rangeFinder;
  AcousticModemIF *_acommsIF;

  // Lists containing NavSensors
  NavSensors _coreSensors;
  NavSensors _auxSensors;

  NavigationOutput *_output;
  NavigationOutput::State _state;
  NavigationLog *_log;
  NavigationInput *_input;

  compassErrorT _compassError;

  double _magneticVar;
  double _maxMount;
  double _z0;            //Initial depth

  double _waterSpeed;
  //
  // It is imperative that the velocity maintain it's value between calls.
  //
  double vel_B_N_LV[3];
  
  Boolean _firstCall;
  double _currentTime;
  double _lastTime;
  double _navStartTime;
  double _navLoopInterval;
  long   _lastUsblTime;

  Boolean _gpsValid;
  double _latitude;
  double _longitude;
  double _northing;
  double _easting;
  long   _utmZone;

  //indicates whether LBL server is running onboard
  Boolean Lbl_up;
  //
  // Declare the 7 lbl kalman filter states, and the fix values:
  //
  double _nfix, _efix;
  double _filter_north, _filter_east, _filter_depth, _north_current,
         _east_current, _speed_bias, _heading_bias;

  //
  // For the DVL
  //
  double m_lastUpdateTime, m_deltaT, dvl_lastUpdateTime, dvl_deltaT;
  enum AltimeterInstrument {DVL, ECHOSOUNDER, DVL_FIXED_ECHO_PERP,
			    DVL_FIXED_ECHO_MIN,DVL_SCANNING_ECHO, 
			    MULTIBEAM, NOT_SPECIFIED};
  AltimeterInstrument _altimeterInstrument;

  short _logOnly;

  //
  // Obstacle Avoidance/Altitude following:
  // Distance between the Dvl and the Psa, meters.
  double m_distDvlPsa;
  double m_DtanThM;
  double m_DoverCosThM;
  double m_thetaM;
  double m_sinThetaM;
  double m_cosThetaM;
  double m_delta;
  double m_sinThetaMpDel;
  double m_cosThetaMpDel;
  double m_a, m_b;
  double m_gamma;
  double m_cosDelta;
  double m_dvlAltitude;
  double m_echoAltitude;
  double m_echoHorizRange;
  double _multibeamAltitude;
  double _lastOA, _lastOR;
  int    _nadirCnts, _obstacleCnts, _nGood, _medianBeamNo;
  int    _obstacleSlopeCnts;
  double _avoidRange;
  double _minBeamRange;
  int    _minBeamCnts;
  double _thetaB, _lsAlt;

  
  Boolean _useLbl;
  Boolean _useIns;
  Boolean m_dvlValid, m_dvlNewData;
  Boolean _dvlFailed, _echoSounderFailed, _multibeamFailed;
	
};

//added by eickstedt to incorporate new LBL navigation algorithm
extern "C" void jerome_position(int,lbl_array*,double,double,double,
				double,double,double,double*,double*,
                                double good_tof[],double*,double*,
				double*,double*,double*,double*,double*);

extern "C" void bam_dy_position(int,lbl_array*,double,double,double,
				double,double,double,double*,double*,
				double*,double*);

extern "C" void init_matrices();

#endif