/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedEchoSounder.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 "SimulatedEchoSounder.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 "EchoSounderDriver.h"

#define MAXRANGE 99.99
#define MAXSIMRANGE 160.

SimulatedEchoSounder::SimulatedEchoSounder()
   : EchoSounderIF_SK(),  _attributes("echoSounderServer")
{
  //
  // Read in parameters from the .cfg file:
  //
  char configFileName[256];

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

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


  strcpy(configFileName, System::configurationFile("echoSounder.cfg"));

  _attributes.add( new IntegerAttribute("range",
                   "Maximum Range, meters", &_maxRange, 200 ) );
  _attributes.add( new FloatAttribute("noReturnRange",
                   "No Return Range, meters", &_noReturnRange, 60. ) );
  _attributes.add( new IntegerAttribute("startGain",
                   "Start Gain, dB", &_startGain, 10 ) );
  _attributes.add( new FloatAttribute("absorption",
                   "Two-Way Absorption, dB/m", &_absorpEng, .1 ) );
//                 "Two-Way Absorption, dB/m*100", &_absorption, 10 ) );
  _attributes.add( new AngleAttribute("trainAngle", "pointing angle",
				      &_trainRef, 0));
  _attributes.add( new AngleAttribute("sector", "scan sector size",
				      &_sectorEng, 0));
  _attributes.add( new AngleAttribute("stepSize", "step size",
				      &_stepSizeEng, .3));
  _attributes.add( new IntegerAttribute("pulseLen",
                   "Pulse Length, usec", &_pulseLenEng, 60 ) );
//                 "Pulse Length, usec/10", &_pulseLen, 10 ) );
  _attributes.add( new IntegerAttribute("minRange",
                   "Minimum range, m", &_minRangeEng, 0 ) );
//                 "Minimum range, m*10", &_minRange, 20 ) );
  _attributes.add( new IntegerAttribute("triggerCtrl",
                   "Enable, Rising/Falling", &_triggerCtrl, 0 ) );
  _attributes.add( new BooleanAttribute("profile",
                   "Turn off Echo Data", &_profile, 1 ) );
  _attributes.add( new IntegerAttribute("delay",
                   "Return Delay, ms", &_delay, 0 ) );
  _attributes.add( new IntegerAttribute("acousticFreq",
                   "acoustic frequency kHz", &_acousticFreqEng, 330 ) );
  _attributes.add( new FloatAttribute("pingPeriod",
                   "ping period in Hz", &_pingPeriod, 2 ) );
  _attributes.add( new AngleAttribute("xzAngle", "x-z plane mounting angle",
				      &_xzAngle, 90));
  _attributes.add( new AngleAttribute("minGrazingSim", "simulated min grazing angle",
				      &_minGrazing, 30));

  _maxRange = MAXSIMRANGE;

  AttributeParser::parse(configFileName, &_attributes);
  System::copyToLogDir(configFileName);


  //
  // Make the vehicle parameters available in the constructor. This is a
  // local variable.
  //
  VehicleConfigurationIF vehicleConfig("vehicleConfig");
  //
  // Extract some into class variables.
  //
  // _xzAngle is between body-fixed z axis and sonar los
  //
  // Compute the unit vector:
  _los_B[0] = sin( _xzAngle );
  _los_B[1] = 0.;
  _los_B[2] = cos( _xzAngle );

  _train = (short) (_trainRef*180/PI/trainDegPerStep + trainShift);

   _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 EchoSounderLog( this, DataLog::BinaryFormat, _profile );
  }
  catch ( ... ) 
  {
     throw Exception("SimulatedEchoSounder - EchoSounderLog constructor failed.\n");
  }

  int i;
  for( i=0; i<EchoSounderIF::EchoArraySize; i++ ) _echoData[i] = (char)(i/4);

  for( i=0; i<EchoSounderIF::ScanArraySize; i++ ) 
  {
     //_ranges[i] = -2;        
     // Avoid annoying "uninitialized data" message.
     _ranges[i] = 0;           
     //Time::gettime( &(_times[i]) );
     _times[i] = _askTime;
  }

  _sector = (unsigned char) ((_sectorEng)*180/PI/3);


  Syslog::write("SimulatedEchoSounder - sector = %d.", _sector);


  if( (_sector < 0) ) 
  {
     //
     // Must be [0, 120].  3 degrees/step.
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for sector. Setting it to zero.", _sector);
     _sector = 0;
  }

  if( (_sector > 120) )
  {
     //
     // Must be [0, 120].  3 degrees/step.
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for sector. Setting it to 120.", _sector);
     _sector = 120;
  }

  //
  // Units:  tindx is the integer number of 2.4 degree steps.

  _train = (short) (_trainRef*180/PI/trainDegPerStep + trainShift);
  _tindx  = _train / 8;
  _toffset = _train % 8;

  _scanDirection = Ascending;


  if( _sector )
  {
     //
     // If scanning.
     _tindxMax = _tindx + (short)(_sector*3./(_stepSizeEng*180./PI)/2.);
     _tindxMin = _tindx - (short)(_sector*3./(_stepSizeEng*180./PI)/2.);
     if( _tindxMax >= EchoSounderIF::ScanArraySize ) 
	_tindxMax = EchoSounderIF::ScanArraySize - 1;
     if( _tindxMin <= 0 ) _tindxMin = 0;
  }
  else
  {
     //
     // If not scanning:
     _tindxMax = _tindx;
     _tindxMin = _tindx;
  }

//
//  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 )SimulatedEchoSounder::compute);

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

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


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

void SimulatedEchoSounder::name(DeviceIF::Name name)
{
  strcpy(name, "Simulated EchoSounder");
}


void SimulatedEchoSounder::serialNumber(DeviceIF::Name number)
{
  strcpy(number, "XXX");
}


DeviceIF::Status SimulatedEchoSounder::initialize()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedEchoSounder::powerOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedEchoSounder::powerOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedEchoSounder::dataLoggingOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedEchoSounder::dataLoggingOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedEchoSounder::status()
{
  return DeviceIF::Ok;
}



DeviceIF::Status SimulatedEchoSounder::headAngleCmd( double trainCmd)
{
#if 0
   double trainCmdArg;
   trainCmdArg = trainCmd;
   //
   // Declare an enum and initialize it.
   EchoSounderCommand::EchoSounderCommands cmdEnum;
   cmdEnum = EchoSounderCommand::MoveHead;
   //
   // Construct an object and initialize it with the enum and 
   // corresponding argument.
   EchoSounderCommand::Command cmdObj(cmdEnum, trainCmdArg);

   _command->write(&cmdObj);
#endif
   return DeviceIF::Ok;
}

double SimulatedEchoSounder::getHeadAngle()
{
  // Dummy implementation for now
   return 0.0;
}

double SimulatedEchoSounder::getHeadAngle(short tindx)
{
   short train;

   if( tindx < 0 || tindx > EchoSounderIF::ScanArraySize )
   {
      Syslog::write("SimulatedEchoSounder::getHeadAngle() Error.  "
      "Index tindx = %d out of range.", train);
      return -1;
   }
   train = tindx*8 + _toffset;
   return ((double)(train-trainShift))*trainDegPerStep*PI/180.;
}

DeviceIF::Status SimulatedEchoSounder::scanSectorCmd( double sector)
{
#if 0
   double sectorArg;
   sectorArg = sector;
   //
   // Declare an enum and initialize it.
   EchoSounderCommand::EchoSounderCommands cmdEnum;
   cmdEnum = EchoSounderCommand::ScanSector;
   // Construct an object and initialize it with the enum and 
   // corresponding argument.
   EchoSounderCommand::Command cmdObj(cmdEnum, sectorArg);

   _command->write(&cmdObj);
#endif
   return DeviceIF::Ok;
}


void SimulatedEchoSounder::xzAngle(double *radians)
{
  *radians = _xzAngle;
}


void SimulatedEchoSounder::maxRange(double *meters)
{
  *meters = _maxRange;
}




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 SimulatedEchoSounder::compute(TaskInterface *taskInterface, EventCode code)
void SimulatedEchoSounder::compute(void)
{
   Boolean debug = False;
   Boolean surfaceDetect;

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

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

   //
   // Extract the vehicle state from the simulation.
   //
   _simulator->state(position, vel_B_N_B, euler, omega_B_N_B);
   //
   // 

   dtrain = ((double)(_train-trainShift))*trainDegPerStep*PI/180.;

   _los_B[0] = sin( _xzAngle + dtrain );
   _los_B[1] = 0.;
   _los_B[2] = cos( _xzAngle + dtrain );

   //
   // Compute the range, given vehicle orientation, depth, and bottom depth.

   drange = _simulator->beamRange( offset_B, _los_B, 0., _maxRange, 
                                   rangeTol, bisectCnt, &_grazing, 
                                   &surfaceDetect);

   _serialStatus = (char)65;


   if( _grazing < _minGrazing ) drange = _maxRange;


   if( drange < 0. )
   {
      //
      // Model this as an instrument error
      _echoReceived = False;
      _serialStatus = (char)0;
      drange = 0.;
   }

   _range = (short)(drange*100.);

   _ranges[_tindx] = _range;

   //
   // The imagenex 881A returns a 0 if it doesn't detect an echo, which could
   // mean that the bottom is either too far or too close.
   if( drange > _noReturnRange || drange < _minRangeEng) 
   {
      _echoReceived = False;
      _ranges[_tindx] = 0;
   }
   _times[_tindx] = Time::milliseconds();
   //Syslog::write("SimulatedEchoSounder - \t\t\t\tmilliseconds = %d", _times[_tindx]);
   if (_times[_tindx] < 0) exit(0);
   
   //Time::gettime( &(_times[_tindx]) );


   _k = _simulator->nTs();
   //
   // Implement these later.
   //
   //_sampleTime.seconds = time(0);
   //_sampleTime.nanoSeconds = 0.;
   Time::gettime( &_sampleTime );
   //
   // Print out debug info
   //
   if( (_k % 5) == 0 && (_k != 0) )
      dprintf(" SimulatedEchoSounder::range() \n"
	      "               time = %d \n"
//	      "               time = (%d,%d) \n"
	      "               waterDepth(0.,0.) = %.1f \n"
	      "               range = %.1f, \n"
	      "               train = %d\n"
	      "               iterations = %d \n",
//	      _times[_tindx].seconds,
//	      _times[_tindx].nanoSeconds,
              _times[_tindx],
	      _simulator->waterDepth(0.,0.),
	      _range/100., _train,  *bisectCnt );

   _log->write();


   if( _sector )
   {
      //
      // Scanning.  This simulation model scans at 5Hz, so it increments once
      // upon each call.
      if( _scanDirection == Ascending ) 
      {
	 _tindx++;
	 if( _tindx >= _tindxMax ) 
	 {
	    _scanDirection = Descending;
	    _tindx = _tindxMax;
	 }
      } 
      else if( _scanDirection == Descending ) 
      {
	 _tindx--;
	 if( _tindx <= _tindxMin ) 
	 {
	    _scanDirection = Ascending;
	    _tindx = _tindxMin;
	 }
      } 
      _train = _tindx*8 + _toffset;
   }

}


DeviceIF::Status SimulatedEchoSounder::range(Boolean *echoReceived, 
					     double *value, 
					     TimeIF::TimeSpec *sampleTime)
{

   *echoReceived = _echoReceived;
   *value = _range/100.;

   sampleTime->seconds = _sampleTime.seconds;
   sampleTime->nanoSeconds = _sampleTime.nanoSeconds;

// if (_echoReceived) return DeviceIF::Ok;
// else               return DeviceIF::Error;

   return DeviceIF::Ok;

}

DeviceIF::Status SimulatedEchoSounder::get(EchoSounderIF::Data *data)
{
   DeviceIF::Status status;
   Boolean echoReceived;
   double temprange;

   compute();

   status = range( &echoReceived, &temprange, &(data->updateTime) );


   data->xzAngle         = _xzAngle;
   data->serialStatus    = _serialStatus;
   data->train           = _train;
   data->sector          = _sector;
   data->maxRange        = (unsigned char) _maxRange; //meters
   data->maxRangeCfrm    = (unsigned char) _maxRange; //meters
   data->error           = False;
   data->updateTime.seconds      = _sampleTime.seconds;
   data->updateTime.nanoSeconds  = _sampleTime.nanoSeconds;
   data->epoch.seconds      = _epoch.tv_sec;
   data->epoch.nanoSeconds  = _epoch.tv_nsec;
   data->range           = temprange;
   if(!_profile) for( int i=0; i<EchoSounderIF::EchoArraySize; i++ ) 
      data->echoData[i]  = _echoData[i];
   //nBytes;
   //nBytesRead;
   //freq;
   //crashCtr;
   data->echoReceived = _echoReceived;
   for( int i=_tindxMin; i<=_tindxMax; i++ )
   {
      data->ranges[i] = _ranges[i];
      //data->times[i].seconds      = _times[i].seconds;
      //data->times[i].nanoSeconds  = _times[i].nanoSeconds;
      data->times[i] = _times[i];
   }
   data->toffset  = _toffset;
   data->tindx    = _tindx;
   data->tindxMin = _tindxMin;
   data->tindxMax = _tindxMax;
   
   return status;
}

Boolean SimulatedEchoSounder::isScanning(void)
{
   return True;
}


DeviceIF::Status SimulatedEchoSounder::enable() 
{
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedEchoSounder::disable() 
{
  return DeviceIF::Ok;
}

DeviceIF::Status SimulatedEchoSounder::moveHead( double trainCmd) 
{
  return DeviceIF::Ok;
}


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