#ifndef _NAVIGATION_H
#define _NAVIGATION_H

#define CoreSensor True
#define AuxillarySensor False

#include "PeriodicTask.h"
#include "NavigationOutput.h"
#include "NavSensor.h"
#include "Ahrs.h"
#include "Altimeter.h"
#include "DepthSensor.h"
#include "TailCone.h"
#include "Gps.h"
#include "Lbl.h"
#include "Dvl.h"
#include "Ins.h"
#include "Velocimeter.h"
#include "NavigationLog.h"

/*
CLASS
Navigation

DESCRIPTION
Collects and processes navigation data from various sensors

AUTHOR
Tom O'Reilly
*/
class Navigation : public PeriodicTask {

  friend class NavigationLog;

public:

  Navigation(int millisec);

  ~Navigation();


protected:

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

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

  ///////////////////////////////////////////////////////////////////
  // Periodic callback
  void callback();

  ///////////////////////////////////////////////////////////////////
  // 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;
  Altimeter *_altimeter;
  DepthSensor *_depthSensor;
  TailCone *_tailCone;

  // Auxillary sensors
  Gps *_gps;
  Lbl *_lbl;
  Velocimeter *_velocimeter;
  Dvl *_dvl;
  Ins *_ins;

  // Lists containing NavSensors
  NavSensors _coreSensors;
  NavSensors _auxSensors;

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

  double _magneticVar;

  double _waterSpeed;

  Boolean _firstCallback;
  double _currentTime;
  double _lastTime;
  double _navStartTime;
  double _navLoopInterval;

  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_lastPingTime;
  enum AltimeterInstrument {DVL, PSA, NOT_SPECIFIED};
  AltimeterInstrument _altimeterInstrument;
  
  Boolean _useLbl;
  Boolean _useIns;
};

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