/****************************************************************************/
/* 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 837 Multibeam. Based on the 881AL simulator
//          from 2007.
// AUTHOR:  McEwen
// DATE:    2013/1/29
//
//////////////////////////////////////////////////////////////////////
//
#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 MAXSIMRANGE 100.
#define MINRANGE .5
#define BAD_DT_BEAM_RANGE 0

SimulatedDeltaT::SimulatedDeltaT()
   : DeltaTIF_SK(), _runEveryOtherTime(True)
{
   _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");
  //
  // 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");


  _simulator->getSimDeltaTData( &_verticalMount, &_fwdEnabled );

  Syslog::write("SimulatedDeltaT:: _verticalMount = %d, "
		"_fwdEnabled = %d", _verticalMount, _fwdEnabled);

  //
  // Sonar CL at 45 degrees. Beam zero is 105 degrees down from vehicle x
  // (ahead).
  //_xzMountAngleCnts = 45;  
  //
  //Now we have two sonars.  Multibeam.cc will use the forward, so
  //specifiy the first 120 deg further back; 45-60-60.
  //_xzMountAngleCnts = -75;  
  //
  // If the forward is mounted straight down: 
  //_xzMountAngleCnts = -120; 
  //
  // If the Aft is mounted straight down: 
  _xzMountAngleCnts = 0; 
  //
  // 11 Nov 2014.  For the 13 Nov dive, the desire is to reduce the
  // down-looking sector and increase the forward-looking sector.  The
  // requirement is that both sectors add to 180 degrees, and that they
  // don't overlap.  
  //
  // This can be accounted for here by adding an offset to
  // _xzMountAngleCnts, _sectorOffset.
  //
  // Sector sizes:
  //
  // aft = 120,  fwd = 60  =>  _sectorOffset = 0
  // aft = 90,   fwd = 90  =>  _sectorOffset = 15
  // aft = 60,   fwd = 120 =>  _sectorOffset = 30
  //
  _sectorOffset = 0;
  //
  // _nbeams is the number of beams in the first, or Aft sonar.  _tbeams is
  // the sum of both.
  //
  // _beamOverlap is the number of overlapping beams due to orthogonal
  // mounting. If both FOVs are 120 then the overlap must be 30.
  //
  // NOTE: When time permits, compute this by reading in delta_tServer.cfg
  //

  if( _fwdEnabled )
  {
     _nbeams = NBEAMS;
     //
     // Assume the forward sonar is mounted along the vehicle
     // centerline.  Then:
     //
     // sectorSize = 120 => beamOverlap = 30.
     // sectorSize =  60 => beamOverlap =  0.
     _beamOverlap = BEAM_OVERLAP;
     _tbeams = 2*NBEAMS - _beamOverlap;
     //_fwdBeams = _tbeams - _nbeams + _beamOverlap;
     _fwdBeams = NBEAMS;
  }
  else 
  {
     _tbeams = NBEAMS;
     _nbeams = NBEAMS;
  }

  Syslog::write("SimulatedDeltaT:: _tbeams = %d, "
		"_nbeams = %d", _tbeams, _nbeams);

  //
  // Extract some into class variables.
  //
  // Initialize the unit vector:
  for( int i=0; i<_tbeams; i++ ) 
  {
     if( _verticalMount)
     {
	//Nose-mounted, with the sonar CL pointing ahead, and the sonar scan
	//plane coincident with the vehicle x-z plane.  
	_los_B[i][0] =  cos( ( i+_xzMountAngleCnts-150-_sectorOffset)*PI/180.);
	_los_B[i][1] = 0.;
	_los_B[i][2] = -sin( ( i+_xzMountAngleCnts-150-_sectorOffset)*PI/180.);  
     }
     else
     {
	//
	// Side-mounted, with the sonar CL looking out the vehicle -y (port),
	// with the scan plane coincdent with the vehicle x-y plane.
	_los_B[i][0] =  sin( (i-60)*PI/180.);
	_los_B[i][1] = -cos( (i-60)*PI/180.);  //Looking out the left side.
	_los_B[i][2] = 0.;
     }
     _intensities[i] = 0;
     _beam_ranges[i] = 0.;
  }

   _serialStatus = (char)65;

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

  triggerEvent(DeviceIF::Ok);

  try 
  {
     _log[0] = new DeltaTLog( this, DataLog::BinaryFormat, "idt");
     if( _fwdEnabled ) _log[1] = new DeltaTLog( this, DataLog::BinaryFormat, "idt_fwd" );
  }
  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;
  _sideReady     = True;
  _fwdReady      = True;

  _firstSurfaceDetect = True;

  //
  // Simulate sonar drop outs; either the sonar operates but gets no returns,
  // or the sonar stops operating.
//_noReturnTime =  5220E3;  //Start of acute approach; second lap 18v2Ed3.
//_noReturnTime =  5260E3;  //End of acute appr; veh lined up;scnd lap 18v2Ed3.
//_noReturnTime =  4100E3;
  _noReturnTime = 41000E3;
//_inoperativeTime =  4200E3;
  _inoperativeTime = 42000E3;
  _noReturnDuration    = 60E3;
  _inoperativeDuration = 60E3;

}


SimulatedDeltaT::~SimulatedDeltaT()
{
  delete _simulator;
  delete _log[0];
  delete _log[1];
}


double rangeTol    = 0.01;            //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 = False;

   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;

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

   //
   // Extract the vehicle state from the simulation.
   //
   _simulator->state(position, vel_B_N_B, euler, omega_B_N_B);
   //
   // 
   Boolean surfaceDetect, surfaceFound = False;


   for( i=0; i<_tbeams; 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]), &surfaceDetect );
      if(surfaceDetect) surfaceFound = True;
#if 0      
      if( _grazing[i] < _minGrazing ) _beam_ranges[i] = 0.;
#endif
      //
      // 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;
      }
   }

   if( surfaceFound ) 
   {
      if( _firstSurfaceDetect )
      {
	 Syslog::write("SimulatedDeltaT: Surface detected at "
		       "simtime = %.2f.", ((float)_time)/1000.);
	 _firstSurfaceDetect = False;
      }
   }
   else if( _firstSurfaceDetect == False )
   {
      Syslog::write("SimulatedDeltaT: Surface lost at "
		    "simtime = %.2f.", ((float)_time)/1000.);
      _firstSurfaceDetect = True;
   }

   //_times[_tindx] = Time::milliseconds();
   //Time::gettime( &(_times[_tindx]) );

   _k = _simulator->nTs();

   //
   // Fake up some bad hits to check the INVALID_xx handling:
   if( _time > _noReturnTime && _time < _noReturnDuration + _noReturnTime )
   {
      for( i=0; i<_tbeams; i++)
      {
	 _beam_ranges[i] = BAD_DT_BEAM_RANGE;
      }
   }

   /*
   ** 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] );
   
}


double SimulatedDeltaT::range(DeltaTIF::Sonar sonar) 
{
   return _range;
}

unsigned short SimulatedDeltaT::nbeams(DeltaTIF::Sonar sonar)
{
   if( sonar == DeltaTIF::Side )
      return _nbeams;      
   else if( sonar == DeltaTIF::Fwd )
      return (_tbeams - _nbeams + _beamOverlap);      
   else
      //
      // Should  never reach here.
      return _nbeams;      
}
unsigned short SimulatedDeltaT::interval(DeltaTIF::Sonar sonar)
{
   return _interval;
}
void SimulatedDeltaT::update_time(DeltaTIF::Sonar sonar, TimeIF::TimeSpec *timestamp) 
{
   timestamp->seconds      = _sampleTime.seconds;
   timestamp->nanoSeconds  = _sampleTime.nanoSeconds;
}
void SimulatedDeltaT::start(DeltaTIF::Sonar sonar) 
{
   _enabled = True;
}
void SimulatedDeltaT::stop(DeltaTIF::Sonar sonar) 
{
   _enabled = False;
}

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

   if( _runEveryOtherTime )
   {
      //
      // Run it every other sample period.
      //
      if( sonar == DeltaTIF::Side )
      {
	 if( _time > _inoperativeTime && _time < _inoperativeTime + _inoperativeDuration )
	 {
	    //Stop computing
	    data->update_time.seconds      = _lastSideSampleTime.seconds;
	    data->update_time.nanoSeconds  = _lastSideSampleTime.nanoSeconds;
	    Syslog::write("SimulatedDeltaT: Side off-line at "
		    "simtime = %.2f.", ((float)_time)/1000.);
	    return;
	 }
	 else
	 {
	    if( !_sideReady ) 
	    {
	       _sideReady = True;
	    }
	    else
	    {
	       //
	       // Set !_sideReady for the next time through.
	       _sideReady = False;
	       _lastSideSampleTime.seconds      = _sampleTime.seconds;
	       _lastSideSampleTime.nanoSeconds  = _sampleTime.nanoSeconds;
	       compute();
	       _nlog=0;
	       if(_log[_nlog]) _log[_nlog]->write();
	    }
	 }
      }
      else if( sonar == DeltaTIF::Fwd && _fwdEnabled )
      {
	 if( !_fwdReady ) 
	 {
	    _fwdReady = True;
	 }
	 else
	 {
	    _fwdReady = False;
	    compute();
	    _nlog=1;
	    if( _log[_nlog] ) _log[_nlog]->write();
	 }
      }
   }
   else   
   {
      //
      // Run it every sample period.
      compute();
      if( sonar == DeltaTIF::Side ) _nlog = 0;
      else if( sonar == DeltaTIF::Fwd && _fwdEnabled ) _nlog = 1;
      if(_log[_nlog]) _log[_nlog]->write();
   }

   data->update_time.seconds      = _sampleTime.seconds;
   data->update_time.nanoSeconds  = _sampleTime.nanoSeconds;
   data->enabled                  = _enabled;
   data->range                    = _range;
   data->interval                 = _interval;
   if( sonar == DeltaTIF::Side )
   {
      for( int i=0; i<_nbeams; i++ )
      {
	 data->beam_ranges[i] = _beam_ranges[i];
	 data->intensities[i] = _intensities[i];
      }
      data->nbeams  = _nbeams;
   }
   else if( sonar == DeltaTIF::Fwd )
   {
      if( _verticalMount)
      {
      	 for( int i=0; i<_tbeams-_nbeams; i++ )
      	 {
      	    data->beam_ranges[i] = _beam_ranges[i+_nbeams-_beamOverlap];
      	    data->intensities[i] = _intensities[i+_nbeams-_beamOverlap];
      	 }
      	 data->nbeams  = _tbeams - _nbeams + _beamOverlap;
      }
      else
      {
      	 for( int i=0; i<_fwdBeams; i++ )
      	 {
      	    data->beam_ranges[i] = _beam_ranges[i+_nbeams-_beamOverlap];
      	    data->intensities[i] = _intensities[i+_nbeams-_beamOverlap];
      	 }
      }
      data->nbeams  = _fwdBeams;
   }
}

void SimulatedDeltaT::get_mounting_angles(DeltaTIF::Sonar sonar, double *phi, double *theta, double *psi) 
{
   *phi   = _phi[sonar];
   *theta = _theta[sonar];
   *psi   = _psi[sonar]; 
}

Boolean SimulatedDeltaT::enabled(DeltaTIF::Sonar sonar) 
{
   Boolean enabled = False;
   if( sonar == DeltaTIF::Side )
   {
      enabled = _enabled;
   }
   else if( sonar == DeltaTIF::Fwd )
   {
      enabled = _fwdEnabled;
   }
   return enabled;
}

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