/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedUsbl.cc                                       */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
//////////////////////////////////////////////////////////////////////
//
// PURPOSE:  Simulate the Sonardyne Inverted Usbl
// AUTHOR:   McEwen
// DATE:     2005/11/28
//
//////////////////////////////////////////////////////////////////////
//
#include <time.h>
#include "SimulatedUsbl.h"
#include "MathP.h"
#include "TimeP.h"
#include "matrixMath.h"
#include "Syslog.h"
#include "System.h"
#include "VehicleConfigurationIF.h"
#include "NavUtils.h"
#define RECORD_TERMINATOR "\xa"

//
// Beacon array:
struct beacons
{
      int number;               //Beacon number
      char address[32];      //Interrogation address
      double depth;
      double lat, lon;          //Location, decimal degrees.
      double northing, easting; //Location, UTM
};

struct beacons beacontab[] = {
   27, "V11_2", 93,  36.81249,   -121.973463, 0.,0., //Upper Soquel,Rock,June 09
   50, "V4_5",  298, 36.8161143, -121.9835191,0.,0., //Mid-Soquel, Rock, July 09
   28, "V12_2", 30,  36.81249,   -121.973463, 0.,0., //Sim, same as 27 but shallower
   29, "V13_2", 30,  36.81349,   -121.977463, 0.,0.,  //Simulated 100N, 300W of 28
   00, "V7_7",  12,  36.606819,  -121.874569, 0.,0.  //Aug 2016 Resurrecting docking simulations.
};
// 00, "V7_7",  30,  36.606685,  -121.874230, 0.,0.  //Aug 2016 Resurrecting docking simulations.

#define NBEACONS  ( (sizeof beacontab / sizeof beacontab[0]) )


SimulatedUsbl::SimulatedUsbl()
   : UsblIF_SK()
{
  //
  // It would be good to read in parameters from sonardyne.cfg here. But,
  // this takes more coding than I have time for now.  This file would have
  // to call methods in SonardyneDriver.cc, such as createCfgAttributes() and
  // loadConfigFile(configFile).  So, for now, just start with the first
  // beacon in the table.
  //
  //
  // Set the first beacon address here:
  strcpy(_beaconAddress, "V7_7");

#if 0
  char configFileName[256];
  strcpy(configFileName, System::configurationFile("sonardyne.cfg"));

//  _attributes(configFileName);
  _attributes.add( new StringAttribute  ("Beacon",
                   "Name of beacon to interrogate", &_beaconAddress ) );
  AttributeParser::parse(configFileName, &_attributes);
#endif
  Syslog::write("SimulatedUsbl:: Initial beacon interrogation address is %s", 
		_beaconAddress);
  //
  // 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");

  try 
  {
     _log = new SonardyneLog( this, DataLog::BinaryFormat );
  }
  catch ( ... ) 
  {
     throw Exception("SimulatedUsbl - SonardyneLog contructor failed\n");
  }

  Syslog::write("SimulatedUsbl:: There are %d beacons in the table.\n", NBEACONS);
  //
  // Compute the UTM location of each beacon in the array. Hardcode the zone
  // for Monterey bay:
  long utmZone = 10;
  for( int i=0; i<NBEACONS; i++ )
  {
     NavUtils::geoToUtm(beacontab[i].lat*PI/180., beacontab[i].lon*PI/180., 
			utmZone, &(beacontab[i].northing), 
			&(beacontab[i].easting) );

     Syslog::write("SimulatedUsbl:: beacon %d %s lat=%.5f lon=%.5f\n"
		   "  SimulatedUsbl:: northing = %.1f easting=%.1f\n",
		   beacontab[i].number, 
		   beacontab[i].address, 
		   beacontab[i].lat, 
		   beacontab[i].lon, 
		   beacontab[i].northing, 
		   beacontab[i].easting); 
  }
  //
  // Initialize the simulator's beacon location to the first in the table.
  // Reset during the run through the homing behavior.
  set_beacon( _beaconAddress );

  _flipflop = True;

  _running = False;

  _x = _y = _z = _r = 0.;

  _idum = -100;
  _pIdum = &_idum;

  _missDist = 10000.;
  
  //
  // Select probability density function:
  _density = Gaussian;
  char *DensityNames[] = {"None","Gaussian","Uniform"};

  Syslog::write(" Simulating Usbl with %s noise.", DensityNames[ _density ] );
  
  triggerEvent(DeviceIF::Ok);

}


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

void SimulatedUsbl::name(DeviceIF::Name name)
{
  strcpy(name, "Simulated Usbl");
}


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


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


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


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


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


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


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

DeviceIF::Status SimulatedUsbl::set_beacon( UsblIF::String32 newBeaconAddress )
{
   for( int i=0; i<NBEACONS; i++ )
   {
      if( !strcmp( newBeaconAddress, beacontab[i].address ) )
      {
	 _beaconUTM[0] = beacontab[i].northing;
	 _beaconUTM[1] = beacontab[i].easting;
	 _beaconUTM[2] = beacontab[i].depth;
	 break;
      }
   }
   if( i < NBEACONS )
   {
      Syslog::write("SimulatedUsbl:: Changing to beacon %d at (%.1f, %.1f)",
		    beacontab[i].number, beacontab[i].northing,
		    beacontab[i].easting);
      return DeviceIF::Ok;
   }
   else
   {
      Syslog::write("SimulatedUsbl:: Error: interrogation address "
		    "%s not found!", newBeaconAddress );
      return DeviceIF::Error;
   }
   return DeviceIF::Error;
}


// Sim start point:  4066375.5, 599958.3
//double trans_N[3] = {4066375.5, 599658.3, 0.};
//double trans_N[3] = {4066475.5, 599658.3, 0.};   //For SimHomingLawnPhoto
//double trans_N[3] = {4066525.5, 599808.3, 0.};   //For SimStarPhoto
//Rock upper Soquel, 93m depth, #27, V11_2, 36.81249, -121.973463
  double trans_N[3] = {4074563.0, 591562.1, 0.};   //For Soquel Cyn Beacon 2009
//Rock lower Soquel, 298m depth, #50, V4_5,
//
//Mars, 900m depth, #73, V7_7.  Removed Oct 2009.
//double trans_N[3] = {4063162.6, 572627.7, 0.};   //Mars transponder July 2009


SimulatorIF::Vector usblOffset_B = {2.62, 0., 0.};
short transponder=1;
double minRange = 1.0;
//double minRange = 5.;
//double maxRange = 1000.;
double maxRange = 250.;
double usblFov  = PI/3.;
double tanFov   = tan(usblFov);
float noiseMag = 10.;
//double noiseMag = 0.;

DeviceIF::Status SimulatedUsbl::get_range_data(
   float *range, float *x, float *y,
   float *z, short *turnAroundTime,
   TimeIF::TimeSpec *sampleTime, 
   UsblIF::UsblDataStatus *status)
{
   Boolean debug = True;
   double usblRange[3];
   double lambda, phi;
   //long idum[1] = {-100};
   double slantLamError = .02;
   double slantPhiError = .02;
   double slantRangeError = .02;

   if( !_running ) return DeviceIF::Ok;

   //
   // For now, fix the sampling period at one second and assume instantaneous
   // transmission of sound.  To do next: check the simulation time and
   // compute the tat based on range, so that as the vehicle approaches the
   // transponder the sampling interval decreases.
   //
   if( (_simulator->nTs() % 5) == 0 ) 
   {
      //_simulator->usbl(usblOffset_B, trans_N, minRange, maxRange, usblRange);
      _simulator->usbl(usblOffset_B, _beaconUTM, minRange, maxRange, usblRange);

      _r = (float) Vnorm( usblRange );
      _x = (float) (usblRange[0]);
      _y = (float) (usblRange[1]);
      _z = (float) (usblRange[2]);

      if( ( _r > (float) maxRange || _r < (float) minRange ) ||
	  ( _x < 1.0 ) || ( sqrt( _y*_y + _z*_z )/_x > tanFov ) )
      {
	 _r = _x = _y = _z = 0.;
	 _status = UsblIF::Stale;
      }
      else
      {
	 if( _r < _missDist ) _missDist = _r;
	 //
	 // Add noise in spherical coordinates.
	 //
	 lambda = atan( _y/_x );
	 phi    = asin( _z/_r );

	 switch(_density)
	 {
	    case Gaussian:
	       lambda += slantLamError * Math::gasdev( _pIdum );
	       phi    += slantPhiError * Math::gasdev( _pIdum );
	       _r     *= 1. + slantRangeError * Math::gasdev( _pIdum );
	       break;

	    case Uniform:
	       //
	       // Uniform r.v.  Sqrt(12) scales it to have unit standard deviation:
	       lambda += slantLamError * sqrt(12.)*(Math::ran1( _pIdum ) - .5);
	       phi    += slantPhiError * sqrt(12.)*(Math::ran1( _pIdum ) - .5);
	       _r     *= 1. + slantRangeError*sqrt(12.)*(Math::ran1( _pIdum ) - .5);
	       break;

	    case None:
	    default:
	       break;
	 }

	 _x = (float) _r * cos(phi) * cos(lambda);
	 _y = (float) _r * cos(phi) * sin(lambda);
	 _z = (float) _r * sin(phi);

//	 _x = (float) (usblRange[0]) + (((float)rand())/RAND_MAX-.5)*noiseMag*_r/50.;
//	 _y = (float) (usblRange[1]) + (((float)rand())/RAND_MAX-.5)*noiseMag*_r/50.;
//	 _z = (float) (usblRange[2]) + (((float)rand())/RAND_MAX-.5)*noiseMag*_r/50.;

	 _status = UsblIF::AllGood;
      }
      _tat = 1000;//Fix this later - see above.

      //sampleTime->seconds = time(0);
      //sampleTime->nanoSeconds = 0.;

      TimeIF::TimeSpec ts;
      Time::gettime(&ts);
      sampleTime->seconds = ts.seconds;
      sampleTime->nanoSeconds = ts.nanoSeconds;

#if 0
      if( _flipflop || 1 )           //Set argument to true
      {
	 _status = UsblIF::AllGood;
	 _flipflop = False;
      }
      else
      {
	 _status = UsblIF::Stale;
	 _flipflop = True;
      }
#endif
      _log->write();
   }
   *status = _status;
   *range = _r;
   *x = _x;
   *y = _y;
   *z = _z;
   *turnAroundTime = _tat;
   return DeviceIF::Ok;
}

Boolean SimulatedUsbl::ready()
{
   if( _simulator->nTs() % 5 ) return False;
   return True;
}


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


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

DeviceIF::Status SimulatedUsbl::start() 
{
   _running = True;
   Syslog::write("SimulatedUsbl::Start\n");
   return DeviceIF::Ok;
}


DeviceIF::Status SimulatedUsbl::stop() 
{
   _running = False;
   Syslog::write("SimulatedUsbl::Stop\n");
   return DeviceIF::Ok;
}


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