/****************************************************************************/
/* Copyright (c) 2006 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : TerrainAid.h                                             */
/* Author   : Rob McEwen                                                    */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 12/01/2006                                                    */
/* Modified : 06/30/14  Henthorn: Added measurements for IDT data           */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#ifndef _TerrainAid_H
#define _TerrainAid_H
#include "PeriodicTask.h"
#include "TerrainAidIF.h"
#include "NavigationIF.h"
#include "DepthSensorIF.h"
#include "DvlIF.h"
#include "DeltaTIF.h"
#include "TerrainAidLog.h"
#include "TerrainAidOutput.h"
#include "Attributes.h"
//#include "TerrainNav.h"
class TerrainNav;
class poseT;
class measT;
class mbT;
class MultibeamerIF;


#define MAXSAMPLES 128

class TerrainAid {  //} : public PeriodicTask {

  friend class TerrainAidLog;

public:

  TerrainAid( Boolean test, Boolean sim, char *configFile = "terrainAid.cfg" );
  ~TerrainAid();

  int samplePeriod() { return _samplePeriod; }

  ////////////////////////////////////////////////////////////////////
  // Initialize device. Returned DeviceIF::Status
  // will be propagated to subscribers in other tasks.
  void initialize();

  ////////////////////////////////////////////////////////////////////
  // 04/24/2015
  //
  // In readData() decoupled code to read data from publishing of TRN estimate.
  // Created publishData(). Mainly, the comes from the desire to precisely
  // control when the new estimate is available (Rock).
  //
  void execute();                // Calls readData() and publishData()

  ////////////////////////////////////////////////////////////////////
  // Read a fixed-length record - use the default:
  void readData();              // Just gathers input for and calls TRN

  ////////////////////////////////////////////////////////////////////
  void publishData();           // Copies the TRN estimate to output

protected:

  NavigationIF  *_nav;
  DepthSensorIF *_parosci;
  DvlIF         *_dvl;

  DeltaTIF      *_idt;
  
  MultibeamerIF *_mb;

  TerrainAidLog *_log;
  TerrainAidOutput *_output;

  Attributes _attributes;
  const long NotSpecified;
      
  void createCfgAttributes();
  void loadConfigFile(const char* file);
  void printCfgAttributes();

private:

  NavigationIF::Position _position;
  NavigationIF::Attitude _attitude;

  DvlIF::Data    _dvlIFData;

  DeltaTIF::Data _idtIFData;

  Boolean _dvlValid;
  Boolean _first;
  Boolean _useIns;

  TerrainNav  *_tercom;
  poseT       *_navData, *_mleEst, *_mmseEst;
  measT       *_dvlData;
  measT       *_idtData;
  measT       *_measData;
  mbT         *_mbData;

  char *_config;
  char  _mapFile[128];
  char *_mapFileName;
  char  _vehicleCfg[128];
  char *_vehicleCfgName;
  char  _dvlCfg[128];
  char *_dvlCfgName;
  char  _resonCfg[128];
  char *_resonCfgName;
  char *_terrainNavServer;
  long  _terrainNavPort;
  long  _map_type;

  long _samplePeriod;
  Boolean _forceLowGradeFilter;
  Boolean _allowFilterReinits;
  Boolean _useModifiedWeighting;
  Boolean _notUsed;
  double _depth;
  double _navTime0, _dvlTime0;
  int _numReinits;
  int _filterState;

  double _maxNorthingCov;
  double _maxEastingCov;
  double _maxNorthingError;
  double _maxEastingError;
  double _phiBias;
  Boolean _useIDTData;

  /** do test if TRUE */
  Boolean _test;
  /** Simulate if TRUE */
  Boolean _sim;

  long _cntr;
};


//////////////////////////////////////////////////////////////////////////////
// Decouple the periodic task stuff from the code that performs the work of
// Terrain Aid. Allows use of a TerrainAid object within other tasks.
//
class TerrainAidTask : public PeriodicTask {

public:

  TerrainAidTask( Boolean test, Boolean sim, char *configFile = "terrainAid.cfg" );
  ~TerrainAidTask();

  ////////////////////////////////////////////////////////////////////
  // Initialize() and execute() simply call the same functions in
  // the TerrainAid object.
  //
  void initialize();
  void execute();

protected:

  TerrainAid* _ta_object;


};

#endif