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

#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, "V11_2");

#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.;

  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);
double 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];

   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]) + (((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.;
      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 _status = UsblIF::AllGood;
      _tat = 1000;//Fix this later - see above.
      sampleTime->seconds = time(0);
      sampleTime->nanoSeconds = 0.;
#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;
}