/****************************************************************************/
/* Copyright (c) 2005 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SonardyneDriver.h                                             */
/* Author   : henthorn                                                      */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 11/15/2005                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <math.h>
#include "SonardyneDriver.h"
#include "SonardyneLog.h"
#include "SonardyneOutput.h"
#include "Syslog.h"
#include "TimeP.h"
#include <FloatAttribute.h>
#include <StringAttribute.h>
#include <IntegerAttribute.h>
#include <AttributeParser.h>

#define RECORD_TERMINATOR "\xa"

Boolean debug_flag = False;

SonardyneDriver::SonardyneDriver(SerialDevice *device, char* config)
   : Task("sonardyneDriver"), _config(config), _attributes(config),
     _pingDelay(0), _device(device), _timeout(0), _diagCount(0)
{

  _log = new SonardyneLog(this, DataLog::BinaryFormat);
  _output = new SonardyneOutput();
  _command = new SonardyneCommand(MessageQueue::ReadWrite);
  _running = False;

  //
  // Initialize:
  //
  init();
}


SonardyneDriver::~SonardyneDriver()
{
   delete _log;
   delete _output;
}

void SonardyneDriver::init()
{
   Syslog::write("SonardyneDriver.cc was last compiled on %s, %s\n",__DATE__, __TIME__);

  Boolean debug = debug_flag;
  char configFile[100];
  int nbytes = 0;
  strcpy(configFile, System::configurationFile(_config));

  createCfgAttributes();
  loadConfigFile(configFile);
  System::copyToLogDir(configFile);
  reportCfgAttributes();

  
  // Set the interrogation string to use for continuous operation
  //
  strcpy(_interrogation, "POS ");
  strcat(_interrogation, _beacon);
  strcat(_interrogation, RECORD_TERMINATOR);
  _timeout = _lw*1000 + 5000;
  //_timeout = _lw*1000 + 2000;

  dprintf("SonardyneDriver - interrogation string is %s, timeout is %d ms",
          _interrogation, _timeout);
  char cmd[40];
  try {

    // Setup the transceiver according to the configuration settings
    //
    _device->flushReceiveBuf();
    _device->clearPort();
    sprintf(cmd, "PG%d\xd\xa", _pg);  // Preset Gain setting
    _device->write(cmd, strlen(cmd));
    _response[0] = '\0';
    nbytes = _device->readUntil(_response,sizeof(_response),RECORD_TERMINATOR, _timeout);
    dprintf("SonardyneDriver - PG response: \n%s", _response);
    dprintf("SonardyneDriver - Read %d bytes.\n",nbytes);

    sprintf(cmd, "PL%d\xd\xa", _pl);  // Power Level setting
    _device->write(cmd, strlen(cmd));
    _response[0] = '\0';
    nbytes = _device->readUntil(_response,sizeof(_response),RECORD_TERMINATOR, _timeout);
    dprintf("SonardyneDriver - PL response: \n%s", _response);
    dprintf("SonardyneDriver - Read %d bytes.\n",nbytes);

    sprintf(cmd, "LW%d\xd\xa", _lw);  // Lbl Wait setting
    _device->write(cmd, strlen(cmd));
    _response[0] = '\0';
    nbytes = _device->readUntil(_response,sizeof(_response),RECORD_TERMINATOR, _timeout);
    dprintf("SonardyneDriver - LW response: \n%s", _response);
    dprintf("SonardyneDriver - Read %d bytes.\n",nbytes);

    sprintf(cmd, "MRPL%d\xd\xa", _mrpl);  // Max Pulse Length setting
    _device->write(cmd, strlen(cmd));
    _response[0] = '\0';
    nbytes = _device->readUntil(_response,sizeof(_response),RECORD_TERMINATOR, _timeout);
    dprintf("SonardyneDriver - MRPL response: \n%s", _response);
    dprintf("SonardyneDriver - Read %d bytes.\n",nbytes);

    // Turn on jitter supression if configured as such
    //
    if (_vjse)
    {
      sprintf(cmd, "VJSE\xd\xa");
      _device->write(cmd, strlen(cmd));
      nbytes = _device->readUntil(_response,sizeof(_response),RECORD_TERMINATOR, _timeout);
      dprintf("SonardyneDriver - Read %d bytes.\n",nbytes);
      dprintf("SonardyneDriver - VJSE response: \n%s", _response);
    }

    sprintf(cmd, "SV%.1f\xd\xa", _esv); // Sound velocity setting
    _device->write(cmd, strlen(cmd));
    nbytes = _device->readUntil(_response,sizeof(_response),RECORD_TERMINATOR, _timeout);
    dprintf("SonardyneDriver - SV response: \n%s", _response);
    dprintf("SonardyneDriver - Read %d bytes.\n",nbytes);
  }
  catch (Exception e)
  {
    Syslog::write("SonardyneDriver::init - caught exception on %s: %s", cmd, e.msg);
    _output->data.deviceReady = False;
    _output->write();
    exit(-1);
  }
  
  _output->data.deviceReady = True;
  _output->write();

}

// Request device to interrogate beacon and parse range values (if any).
// Place values in output object.
//
void SonardyneDriver::acquireRangeData()
{
  Boolean debug = debug_flag;

  dprintf("SonardyneDriver::acquireRangeData.");

  //_device->commsDebugMode(SerialDevice::DebugOn);

  _device->flushReceiveBuf();
  _device->clearPort();
  _device->write(_interrogation, strlen(_interrogation));
  _output->data.status = UsblIF::Undefined;

  _response[0] = '\0';
  int nbytes = 0;
  int ansTime = 0, askTime = 0;

  try {
    askTime = Time::milliseconds();
    nbytes = _device->readUntil(_response, sizeof(_response),
                                RECORD_TERMINATOR, _timeout);
    ansTime = Time::milliseconds();
    dprintf("SonardyneDriver - POS response: %s\n", _response);
  }
  catch (Exception e) {
    Syslog::write("SonardyneDriver - caught exception during POS cycle: %s %d", e.msg, nbytes);
    ansTime = Time::milliseconds();
  }

  printf("Received %s, which is %d bytes, in %d ms.\n", _response, nbytes, 
	 ansTime-askTime);

//  strcpy(_response, "R608.37,X12.3,Y45.6,Z78.9");
//  strcpy(_response, "R608.37");
  char *response = strchr(_response, 'R'); // discard echoed command
  int nvals = 0;
  
  if (nbytes > 0 && response)
  {
    dprintf("SonardyneDriver - parsing %s", response);
    _r = _x = _y = _z = 0.;
    // Parse data string and update output
    //
    if ((_r = atof(++response)) > 0.0001)
    {
      nvals = 1;
      dprintf("SonardyneDriver - read R = %f from %s", _r, response);
    }
    else
    {
      nvals = 0;
      dprintf("SonardyneDriver - Beacon not detected.");
    }

    // If the XYZ values are there, parse the values out of the reponse
    // Then convert from Usbl coordinates to vehicle coords.  The Usbl has z
    // ahead along the case centerline, and x and y perpendicular and marked
    // on the case.  The Usbl is mounted with Usbl_x to starboard, or vehicle
    // y, and Usbl_y up or along vehicle -z.  The Usbl has a LEFT handed
    // coordinate system, so we'll correct that here.
    //
    // 8 April 2009 Coordinate system flipped based on sea trial results:
    //  Old             New
    //  xVeh = -zUsbl   xVeh = -zUsbl
    //  yVeh =  xUsbl   yVeh = -xUsbl
    //  zVeh = -yUsbl   zVeh =  yUsbl
    //
    if (nvals > 0 && (response = strchr(_response, 'X')))
    {
      nvals += 3;
      _y =  atof(++response);                                        //yB= xUsbl
      if (response = strchr(_response, 'Y')) _z = -atof(++response); //zB=-yUsbl
      if (response = strchr(_response, 'Z')) _x = -atof(++response); //xB=-zUsbl
    }
    
    // Set outputs depending on how many values were parsed
    //
    if (nvals > 1)
    {
      // Looks like we got all data - best case scenario
      _output->data.range  = _r;
      _output->data.x      = _x;
      _output->data.y      = _y;
      _output->data.z      = _z;
      _output->data.status = UsblIF::AllGood;
      _output->data.tat    = ansTime - askTime;
      Time::gettime(&(_output->data.updateTime));
      dprintf("SonardyneDriver - got all data, Vehicle coordinates,\n"
	      "    r=%f, x=%f, y=%f, z=%f tat = %d nvals = %d\n",
              _r, _x, _y, _z, _output->data.tat, nvals);
    }
    else if (1 == nvals)
    {
      // OK, we only got the range value
      //
      _output->data.range  = _r;
      _output->data.x      = 0.;
      _output->data.y      = 0.;
      _output->data.z      = 0.;
      _output->data.status = UsblIF::RangeOnly;
      _output->data.tat    = ansTime - askTime;
      Time::gettime(&(_output->data.updateTime));
      dprintf("SonardyneDriver - got range only (%f), tat = %d", _r, _output->data.tat);
    }
    else if (0 == nvals)
    {
       // If tat = _timeout (5 sec), the readUntil timed out.  The usbl
       // probably isn't responding.  If tat = lw (3 sec) the usbl responded,
       // but it can't hear the beacon.
       // 
       _output->data.status = UsblIF::Stale;
       _output->data.tat    = ansTime - askTime;
       Time::gettime(&(_output->data.updateTime));
     
       Syslog::write("SonardyneDriver::acquireRangeData() - Beacon not detected!"
		     " tat = %.2f", _output->data.tat);
    }
    //
    // end the "if" where a usbl response (but not necessarily the beacon)
    // was detected.
  }
  else if (0 == nvals)
  {
     // If tat = _timeout (5 sec), the readUntil timed out.
     // 
     _output->data.status = UsblIF::Undefined;
     _output->data.tat    = ansTime - askTime;
     Time::gettime(&(_output->data.updateTime));
     
     Syslog::write("SonardyneDriver::acquireRangeData() - Beacon not detected!"
		   " tat = %.2f", _output->data.tat);
  }
  // Acquire and log diagnostics from transceiver on every interrogation:
  //
  acquireDiagData();

}

// Ask device for diagnostics about the previous interrogation. Parse return
// data and place values in *this for logging.
//
void SonardyneDriver::acquireDiagData()
{
  Boolean debug = debug_flag;
  char posdString[10];

  dprintf("SonardyneDriver::acquireDiagData.");

  _device->flushReceiveBuf();
  _device->clearPort();
  sprintf(posdString, "POSD\xa", 5);
  dprintf("posd string %s\n", posdString);
  _device->write(posdString, 5); 

  int nbytes;
  try {
    nbytes = _device->readUntil(_response, sizeof(_response),
                                RECORD_TERMINATOR, 5000);
    dprintf("SonardyneDriver - POSD response: \n%s", _response);
    dprintf("SonardyneDriver - nbytes = %d\n", nbytes);
  }
  catch (Exception e) {
    Syslog::write("SonardyneDriver - caught exception during DIAG cycle: %s %d", e.msg, nbytes);
  }

  char *response = strstr(_response, " S"); // discard echoed command

  _sigLevel = _detectLevel = _quality  = 0.;
  _doppler = _acceptance = 0;
  if (response)
  {
    // Looks like we got a valid POSD response, parse data
    //
    response += 2;
    
    _sigLevel = fabs(atof(response));
    _clipped = (strchr(_response, '+') != (char*)0);
    if (response = strchr(response, 'N')) _detectLevel = atof(++response);
    if (response = strchr(response, 'D')) _doppler = atof(++response);
    if (response = strchr(response, 'U')) _acceptance = atof(++response);
    if (response = strchr(response, 'Q')) _quality = atof(++response);

    dprintf("SonardyneDriver::acquireDiagData() - S:%f, N:%f, D:%f, U:%f, Q:%f\n",
	    _sigLevel, _detectLevel, _doppler, _acceptance, _quality);
  }
}


void SonardyneDriver::run()
{
  Boolean debug = debug_flag;
  //
  // Check for any commands.  Execute them if requested.
  //
  SonardyneCommand::Command cmdIn;
  while(1)
  {
     while ((_command->read(&cmdIn)) > 0)
     {
	int ii;
	switch (cmdIn._cmd) {
	   case SonardyneCommand::Start:
	      _running = True;
	      Syslog::write("SonardyneDriver::Start\n");
 	      break;

	   case SonardyneCommand::Stop:
	      if( _alwaysOn == 0 ) _running = False;
	      Syslog::write("SonardyneDriver::Stop\n");
	      break;
	   case SonardyneCommand::SetBeacon:
	      strcpy(_beacon, cmdIn.string);
	      // Since the config file parser will not accept a comma in the
	      // beacon string, we use a underscore instead. Now, replace all
	      // the _ with ,
	      // 
	      for (ii = 0; ii < strlen(_beacon); ii++)
	      {
		 if ('_' == _beacon[ii]) _beacon[ii] = ',';
	      }
	      // Set the interrogation string to use for continuous operation
	      //
	      strcpy(_interrogation, "POS ");
	      strcat(_interrogation, _beacon);
	      strcat(_interrogation, RECORD_TERMINATOR);
	      Syslog::write("SonardyneDriver:: New transponder interrogation "
			    "address is %s",  _interrogation);

 	      break;

	   default:
	      break;
	}
     }

     if( !_running )
     {
	System::milliSleep(200);
     }
     else
     {
	acquireRangeData();  
	_output->write();
	_log->write();
	if( _pingDelay != 0 ) 
	{
	   System::milliSleep(_pingDelay);
	   dprintf("Sleeping for %d milliseconds", _pingDelay);
	}
     }
  }
}

// ########### Configuration file attributes section ###########

// Create attributes in our attricutes member
//
void SonardyneDriver::createCfgAttributes()
{
  _attributes.add( new IntegerAttribute("PingDelay",
                   "Sonardyne Polling Period", &_pingDelay, 0 ) );
  _attributes.add( new IntegerAttribute("PresetGain",
                   "Preset Gain Level", &_pg, 3 ) );
  _attributes.add( new IntegerAttribute("PowerLevel",
                   "Power Level", &_pl, 3 ) );
  _attributes.add( new IntegerAttribute("LblWait",
                   "LBL Wait Period", &_lw, 3 ) );
  _attributes.add( new IntegerAttribute("MRPL",
                   "Max Pulse Length in ms", &_mrpl, 2 ) );
  _attributes.add( new FloatAttribute  ("EstSoundVel",
                   "Estimated Sound Velocity", &_esv, 1485. ) );
  _attributes.add( new StringAttribute  ("Beacon",
                   "Name of beacon to interrogate", &_beacon ) );
  _attributes.add( new IntegerAttribute("VJSE",
                   "Jitter Suppression", &_vjse, 0 ) );
  _attributes.add( new IntegerAttribute("AlwaysOn",
                   "Keep iUsbl on", &_alwaysOn, 1 ) );

}

// Load the config attributes and do some sanity checking
//
void SonardyneDriver::loadConfigFile(const char* configFile)
{
  AttributeParser::parse(configFile, &_attributes); 

  // Since the config file parser will not accept a comma in the
  // beacon string, we use a underscore instead. Now, replace all
  // the _ with ,
  // 
  for (int ii = 0; ii < strlen(_beacon); ii++)
  {
    if ('_' == _beacon[ii]) _beacon[ii] = ',';
  }
  
  // Check attribute values against limits
  //
  if (_pingDelay < 0)
  {
    Syslog::write("Sonardyne driver: PingDelay attr=%d is out of bounds, ",
      "defaulting to 0", _pingDelay);
    _pingDelay = 0;
  }
  if (_pg < 0 || _pg > 3)
  {
    Syslog::write("Sonardyne driver: PresetGain attr=%d is out of bounds, ",
      "defaulting to 3 (hi)", _pg);
    _pg = 3;
  }

  if (_pl < 0 || _pl > 1)
  {
    Syslog::write("Sonardyne driver: PowerLevel attr=%d is out of bounds, ",
      "defaulting to 1 (hi)", _pl);
    _pl = 1;
  }

  if (_lw < 1)
  {
    Syslog::write("Sonardyne driver: LblWait attr=%d is out of bounds, ",
      "defaulting to 3 seconds", _lw);
    _lw = 3;
  }
  else if (_lw > MAX_LW)
  {
    Syslog::write("Sonardyne driver: LblWait attr=%d is out of bounds, ",
      "defaulting to MAX_LW seconds", _lw);
    _lw = MAX_LW;
  }
  
  if (_esv > (DEFAULT_ESV+0.1*DEFAULT_ESV))
  {
    double esv = _esv;
    _esv = (DEFAULT_ESV+0.1*DEFAULT_ESV);
    Syslog::write("Sonardyne driver: ESV attr=%f is out of bounds, ",
      "defaulting to %f", esv, _esv);
  }
  else if (_esv < (DEFAULT_ESV-0.1*DEFAULT_ESV))
  {
    double esv = _esv;
    _esv = (DEFAULT_ESV-0.1*DEFAULT_ESV);
    Syslog::write("Sonardyne driver: ESV attr=%f is out of bounds, ",
      "defaulting to %f", esv, _esv);
  }

  if ( _mrpl < 1 || _mrpl > 5)
  {
    double mrpl = _mrpl;
    _mrpl = 2;
    Syslog::write("Sonardyne driver: MRPL attr=%f is out of normal bounds, ",
      "defaulting to %fms", mrpl, _mrpl);
  }

  if ( _alwaysOn ) _running = True;
}

// Record the attributes in Syslog
//
void SonardyneDriver::reportCfgAttributes()
{
     Syslog::write("SonardyneDriver -- configuration:\n"
		   "\tPingDelay  = %d ms\n"
		   "\tPresetGain = %d\n"
		   "\tPowerLevel = %d\n"
		   "\tLblWait    = %d s\n"
		   "\tESV        = %f m/s\n"
		   "\tBeacon     = %s\n"
                   "\tMRPL       = %d ms\n",
		   _pingDelay, _pg, _pl, _lw, _esv, _beacon, _mrpl);
}