/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedTerrainAidServer.cc                                  */
/* Author   : Henthorn                                                      */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 05/22/2015                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

#include "Syslog.h"
#include "SimulatedTerrainAid.h"
#include "TerrainAidDriver.h"

#define CONTROL_LOOP_PERIOD  200   // Milliseconds

SimulatedTerrainAid::SimulatedTerrainAid(const char *driverName, Boolean test)
  : TerrainAidServer(driverName, test, True),
  _ta(NULL), _taCount(0), _loopsPerTRN(1)
{
   _ta = new TerrainAid(test, True);
   _ta->initialize();
   _loopsPerTRN = _ta->samplePeriod() / CONTROL_LOOP_PERIOD;
}

SimulatedTerrainAid::~SimulatedTerrainAid()
{
  if (_ta) delete _ta;
}

void SimulatedTerrainAid::get( TerrainAidIF::Data *data )
{
  // TerrainAid stuff is run in real-time since the TRN calculation
  // is not faked and can take some time (~1 second)
  //
  // Let TerrainAid provide input to and receive estimates from TRN
  // every, say, 15 loops (3 seconds). Determined by the sample period
  // specified in the config file for TerrainAid (terrainAid.cfg).
  //
  if ( _ta && (_taCount++ % _loopsPerTRN) == 0 )
  {
    _ta->execute();
  }

  // Do the usual TerrainAidServer get stuff
  //
  TerrainAidServer::get(data);
}