/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedDeltaT.cc                                       */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 09/21/2007                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
//////////////////////////////////////////////////////////////////////
//
// PURPOSE:  Simulate the Imagenex 881AL echo sounder.  
// AUTHOR:   McEwen
// DATE:     07/9/21
//
//////////////////////////////////////////////////////////////////////
//
#include <time.h>
#include "SimulatedDeltaT.h"
#include "MathP.h"
#include "Syslog.h"
#include "System.h"
#include "VehicleConfigurationIF.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "IntegerAttribute.h"
#include "BooleanAttribute.h"
#include "DeltaT.h"

#define MAXRANGE 99.99
#define MAXSIMRANGE 160.
#define MINRANGE 1.
#define BAD_DT_BEAM_RANGE 0

SimulatedDeltaT::SimulatedDeltaT()
   : DeltaTIF_SK()
{
   _minGrazing = 0.;

  _askTime = Time::milliseconds();
  Syslog::write("SimulatedDeltaT::Constructor starting at t = %d",
     _askTime);

  Syslog::write("SimulatedDeltaT::minGrazing = %.2f deg.",
     _minGrazing*180/PI);

  _maxRange = MAXSIMRANGE;
  _minRange = MINRANGE;

  //
  // Make the vehicle parameters available in the constructor. This is a
  // local variable.
  //
  VehicleConfigurationIF vehicleConfig("vehicleConfig");
  //
  // Extract some into class variables.
  //
  // Initialize the unit vector:
  for( int i=0; i<NBEAMS; i++ ) 
  {
     _los_B[i][0] =  sin( (i*NBEAMS/(NBEAMS-1)-60)*PI/180.);
     _los_B[i][1] = -cos( (i*NBEAMS/(NBEAMS-1)-60)*PI/180.);  //Looking out the left side.
     _los_B[i][2] = 0.;
     _intensities[i] = 0;
     _beam_ranges[i] = 0.;
  }

   _serialStatus = (char)65;

  //
  // Now read in constants from simulator.cfg.  The best way to do this
  // is to have Simulator's constructor read them in, and then pass them
  // through SimulatorIF. (Generated from the .idl).
  //
  _simulator = new SimulatorIF("simulator");

  //
  // Read zMax from simulation.cfg, someday.
  //
  zMax = _simulator->waterDepth(0.,0.);                         

  triggerEvent(DeviceIF::Ok);

  try 
  {
     _log = new DeltaTLog( this, DataLog::BinaryFormat );
  }
  catch ( ... ) 
  {
     throw Exception("SimulatedDeltaT - DeltaTLog constructor failed.\n");
  }

//
//  compute() is now called by get(), further below.  Get() is an IDL
//  interface function, called at the start of Navigation.cc, thus ensuring
//  that it is never called when the simulator is running, which avoids nasty
//  concurrency IPC problems.  The event call back below was incorrect,
//  because it caused this code to run concurrently with the simulator.
//
//  Beware that the simulator server isn't used, so the call to
//  _tailCone->command() at the bottom of DynamicControlServer.cc causes
//  DynamicControlServer to block and transfer the thread of execution across
//  the IDL interface to the simulator (not a server), so that the entire
//  simulation runs before resuming execution of DynamicControlServer (which
//  just subsequently exits).  Since DynamicControlServer is slaved to
//  navigation with an event call back, everything runs in the correct
//  sequence.  The sequence is Navigation, DynamicControl, Simulator.
//  Navigation makes calls to simulated instruments, which may make use of
//  the simulator state, and also simulate that particular instrument.  But,
//  the simulated instruments do not, and are not, intended to run
//  concurrently with the simulator.  They are not re-entrant.
//
//  Apparently LayeredControl is architected as a "service" and runs
//  asynchronously.  It would be better to synchronize it with the above
//  sequence so that it runs after navigation and before DynamicControl.
//  This would prevent LayeredControl from running just before Navigation,
//  and thus having stale (latent) navigation data, while the rest of the GNC
//  loop does not.


//  subscribe(_simulator, SimulatorIF::NewOutput,
//	    (EventCallback )SimulatedDeltaT::compute);

  Time::getEpoch( &_epoch );
  Syslog::write("SimulatedDeltaT:: epoch = (%d,%d)",
                 _epoch.tv_sec, _epoch.tv_nsec);

  Syslog::write("SimulatedDeltaT::Constructor finished at t = %d",
                 Time::milliseconds());


  _enabled    = True;
  _interval   = 200;
  _nbeams     = NBEAMS;
}


SimulatedDeltaT::~SimulatedDeltaT()
{
  delete _simulator;
  delete _log;
}


double rangeTol    = 0.2;            //Range accuracy to 20 cm.
//
// Note: Later, read this in from the psa.cfg. It will require adding "len"
// to the vehicle.cfg, and then computing the 2.62.  The number in psa.cfg
// should be the distance back from the nose to the sonar head.
//
double offset_B[3] = {2.62, 0., 0.};   //Distance to echoSounder in body coords.
//double offset_B[3] = {0., 0., 0.};   //Distance to echoSounder in body coords.

//void SimulatedDeltaT::compute(TaskInterface *taskInterface, EventCode code)


void SimulatedDeltaT::compute(void)
{
   Boolean debug = True;

   SimulatorIF::Vector position, euler;
   SimulatorIF::Vector vel_B_N_B, omega_B_N_B;
   double drange, dtrain;
   double altitude;
   int i;


   if( !_enabled ) return;

   _askTime = Time::milliseconds();
   _echoReceived = True;

   //
   // Extract the vehicle state from the simulation.
   //
   _simulator->state(position, vel_B_N_B, euler, omega_B_N_B);
   //
   // 
   for( i=0; i<_nbeams; i++ )
   {
      //
      // Compute the range, given vehicle orientation, depth, and bottom depth.

      _beam_ranges[i]= _simulator->beamRange( offset_B, _los_B[i], 0., _maxRange, 
					      rangeTol, &(_bisectCnt[i]), &(_grazing[i]) );
#if 0      
      if( _grazing[i] < _minGrazing ) _beam_ranges[i] = 0.;
      //
      // The imagenex 837 returns a 0 if it doesn't detect an echo, which could
      // mean that the bottom is either too far or too close.
      if( _beam_ranges[i] >= _maxRange || _beam_ranges[i] < _minRange) 
      {
	 _echoReceived = False;
	 _beam_ranges[i] = BAD_DT_BEAM_RANGE;
      }
#endif
   }
   //_times[_tindx] = Time::milliseconds();
   //Time::gettime( &(_times[_tindx]) );


   _k = _simulator->nTs();
   //
   // Implement these later.
   //
   //_sampleTime.seconds = time(0);
   //_sampleTime.nanoSeconds = 0.;
   Time::gettime( &_sampleTime );
   _time = Time::milliseconds();

   /*
   ** Average center beam ranges:
   */
   double range = 0.;
   int numValidBeams = 0;
   for( i=55; i<65; i++)
   {
      if( _beam_ranges[i] != (float) BAD_DT_BEAM_RANGE )
      {
	 numValidBeams++;
	 range += _beam_ranges[i];
      }
   }
   _range = range / ( (double) numValidBeams );
   //
   // Print out debug info
   //
   if( (_k % 5) == 0 && (_k != 0) )
      dprintf(" SimulatedDeltaT::range() \n"
	      "               time = %d \n"
	      "               waterDepth(0.,0.) = %.1f \n"
	      "               _beam_ranges[60] = %.1f, \n"
	      "               _range = %.1f, \n"
	      "               iterations[60] = %d \n",
              _time,
	      _simulator->waterDepth(0.,0.),
	      _beam_ranges[60], _range, _bisectCnt[60] );
   
   if(_log) _log->write();
}


double SimulatedDeltaT::range(void) 
{
   return _range;
}

unsigned short SimulatedDeltaT::nbeams(void)
{
   return _nbeams;
}
unsigned short SimulatedDeltaT::interval(void)
{
   return _interval;
}
void SimulatedDeltaT::update_time(TimeIF::TimeSpec *timestamp) 
{
   timestamp->seconds      = _sampleTime.seconds;
   timestamp->nanoSeconds  = _sampleTime.nanoSeconds;
}
void SimulatedDeltaT::start(void) 
{
   _enabled = True;
}
void SimulatedDeltaT::stop(void) 
{
   _enabled = False;
}

void SimulatedDeltaT::get(DeltaTIF::Data *data)
{
   Boolean echoReceived;
   double temprange;

   compute();

   data->update_time.seconds      = _sampleTime.seconds;
   data->update_time.nanoSeconds  = _sampleTime.nanoSeconds;
   data->enabled                  = _enabled;
   data->range                    = _range;
   data->interval                 = _interval;
   data->nbeams                   = _nbeams;
   for( int i=0; i<NBEAMS; i++ )
   {
      data->beam_ranges[i] = _beam_ranges[i];
      data->intensities[i] = _intensities[i];
   }
}


Boolean SimulatedDeltaT::enabled() 
{
   return _enabled;
}


int SimulatedDeltaT::spawnAuxTasks()
{
  return 0;
}