/****************************************************************************/
/* Copyright (c) 2000 MIT Sea Grant                                         */
/* MIT Proprietary Information. All rights reserved.                        */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : PS8000.cc                                                     */
/* Author   : Don Eickstedt                                                 */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 05/25/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include "PS8000.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "System.h"
#include "AttributeParser.h"
#include "FloatAttribute.h"
#include "BooleanAttribute.h"

PS8000::PS8000(SerialParameters *serialParams):Task("ps8000")
{
  int status;
  double easting,northing,depth;
  long turnaround,channel,result;
  short num_beacons;
  char fullConfigFile[256];
  Attributes attributes( "ps8000constants" );
  Boolean debug = False;

  dprintf(" Begin PS8000 Constructor\n");

  //create LBL log.  This is an ascii file of text strings, like syslog.
  _textLog = new LblLog();
  //
  // Create the binary Matlab log
  //
  _log = new PS8000Log(this);


  // ShareData object to hold output (read by LblServer)
  _output = new LblDriverOutput(SharedData::Write);

  _rt = &_output->data.tofs;

  try{
    _ps8000 = new SerialDevice(serialParams);
  }
  catch(...){
    throw("PS8000 - Could not open serial port->abort,abort");
  }

  dprintf(" PS8000: Opened Serial Port %s\n", serialParams->portName);

  _ps8000->commsDebugMode(SerialDevice::DebugOff);

  _ps8000->setLineFormat(BAUD, DATA_BITS, STOP_BITS, PARITY);

  if(debug) _ps8000->printTermios();

  _ps8000->raw();

  _ps8000->blocking();
  
  _ps8000->clearPort();

  _ps8000->flushReceiveBuf();

  dprintf(" PS8000: Finished Serial Port Setup.\n");

  WorkSiteIF workSite("workSite");

  dprintf(" PS8000: Instantiated workSite Object.\n");
 
  num_beacons = workSite.nLblTransponders();

  dprintf(" PS8000: There are %d beacons.\n", num_beacons);

  for (short i = 1; i <= num_beacons; i++) {

    dprintf(" PS8000: Reading in Transponder %d Parameters.\n", i);

    //read transponder information for each transponder
    if ((result = workSite.getTransponder(i-1, &easting, &northing, &depth,
					  &turnaround,&channel)) == -1)
      Syslog::write ("Lbl Nav Sensor  -- getTransponder returned NULL");

    else

      //this is used to set the timeouts for each of the ten channels
      _beacon_channel [i] = (short) channel;
  }

  strcpy( fullConfigFile, System::configurationFile("ps8000.cfg"));

  attributes.add( new FloatAttribute("ping_lockout",
				     "Ping lockout (s)",
				     &ping_lockout,
				     0.03 ) );
  attributes.add( new FloatAttribute("range_gate",
				     "Upper range gate",
				     &range_gate,
				     1.75 ) );
  attributes.add( new BooleanAttribute("verbose",
				       "Verbosity",
				       &verbose,
				       True ) );
  try {
    AttributeParser::parse(fullConfigFile, &attributes);
  }
  catch(...) {
    Syslog::write("PS8000 -- error loading configuration file");
  }

  dprintf("PS8000: ping_lockout = %.5f\n", ping_lockout );
  dprintf("PS8000: range_gate   = %.5f\n", range_gate );

  try{
    status = initialize(num_beacons);
  }

  catch(char* message){
    throw(message);
  }

  catch(...){
    throw ("PS8000 initialization failed...");
  }

  if (status == -1)
    throw("PS8000 initialization failed...");

  dprintf(" PS8000: Constructor Finished!\n");

}


PS8000::~PS8000()
{
  //cleanup on exit
  //
  delete _output;
  delete _ps8000;
  delete _textLog;
  delete _log;
}

int PS8000::initialize(int num_beacons)
{
  int  confirmed;
  char strbuf[200];
  float timeout;
  Boolean used;
  Boolean debug = True;

  _ps8000->write("$\n\r",3);
  //  _ps8000->confirm("$ER\n\r",TIMEOUT);
  _ps8000->clearPort();

  if (verbose)
    _textLog->write("Setting Idle mode..."); 
  _ps8000->write("$M,0\r\n",6);
  confirmed = _ps8000->confirm("$OK\r\n",TIMEOUT);
  if (confirmed != ERROR)
    {
      if (verbose)
	_textLog->write("Set Idle Mode Command Acknowledged");
    }
  else
    {
      if (verbose)
	_textLog->write("Command not acknowledged, aborting");  
      throw("Set Idle mode command not acknowledged -- aborting");
    }


  if (verbose)
    _textLog->write("Disabling Emergency Slave");
  _ps8000->write("$V,4,1\r\n",8);
  confirmed = _ps8000->confirm("$OK\r\n",TIMEOUT);
  if (confirmed != ERROR)
    {
      if (verbose)
	_textLog->write("Disable emergency slave Command Acknowledged");
    }
  else
    {
      if (verbose)
	_textLog->write("Disable emergency slave Command not acknowledged, aborting");
      throw("Disable emergency slave command not acknowledged -- aborting");
    }

  if (verbose)
    _textLog->write("Setting ping lockout...");
  sprintf(strbuf,"$V,1,%1.2f\r\n",ping_lockout);
  _ps8000->write(strbuf,11);
  confirmed = _ps8000->confirm("$OK\r\n",TIMEOUT);
  if (confirmed != ERROR)
    {
      if (verbose)
	_textLog->write("Set ping lockout Command Acknowledged");
    }
  else
    {
      if (verbose)
	_textLog->write("Set ping lockout Command not acknowledged, aborting");
      throw ("Set ping lockout command not acknowledged -- aborting");
    }


  //
  //Set timeouts on active channels -- timeouts on inactive channels are 
  //set to 0.0 because you don't want to block waiting for returns that 
  //will never come.
  //
  dprintf(" The beacon Array is: \n");
  strcpy(strbuf,"$V, 2");
  for (int i = 1;i <= 10; i++)
    {
      for (int jj = 1; jj <= num_beacons; jj++) 
	dprintf("   %d", _beacon_channel[jj]);
      dprintf("\n");
      used = False;
      for (int j = 1; j <= num_beacons; j++)
	if (_beacon_channel[j] == i || i==3)
          used = True;

      if (used == True)  
        timeout = range_gate;
      else
        timeout = 0.0;
      sprintf(strbuf+strlen(strbuf),",%07.4f,%07.4f",0.0,timeout);
    }
  strcat(strbuf,"\r\n");

  dprintf(" PS8000: Inactivate unused channels: \n%s\n",strbuf);

  if (verbose)
    _textLog->write("Setting range Gates");
  _ps8000->write(strbuf,strlen(strbuf));
  confirmed = _ps8000->confirm("$OK\r\n",TIMEOUT);
  if (confirmed != ERROR)
    {
      if (verbose)
	_textLog->write("Set range gates Command Acknowledged");
    }
  else
    {
      if (verbose)
	_textLog->write("Set range gates Command not acknowledged -- aborting");
      throw("Set range gates command not acknowledged -- aborting");
    }

  if (verbose)
    _textLog->write("Setting Output Power -> High");
  _ps8000->write("$V,3,2\r\n",8);
  confirmed = _ps8000->confirm("$OK\r\n",TIMEOUT);
  if (confirmed != ERROR)
    {
      if (verbose)
	_textLog->write("Set output power Command Acknowledged");
    }
  else
    {
      if (verbose)
	_textLog->write("Set oputput power Command not acknowledged, aborting");
      throw("set output power command not acknowledged -- aborting");
    }


  if (verbose)
    _textLog->write("Setting PS8000 to Master Mode...");
  _ps8000->write("$M,1\r\n",6);
  confirmed = _ps8000->confirm("$OK\r\n",TIMEOUT);
  if (confirmed != ERROR)
    {
      if (verbose)
	_textLog->write("Set master mode Command Acknowledged");
    }
  else
    {
      if (verbose)
	_textLog->write("Set master mode Command not acknowledged, aborting");
      throw("Set master mode command not acknowledged -- aborting");
    }

  return 0;
}

int PS8000::ping()
{
  int confirmed;
  TimeIF::TimeSpec ping_time;

  if (verbose)
    _textLog->write("ping command pending");
  _ps8000->write("$X,9.0\r\n",8);

  //get ping time
  clock_gettime(CLOCK_REALTIME,&p_time);
  ping_time.seconds = p_time.tv_sec;
  ping_time.nanoSeconds = p_time.tv_nsec;

  // Make ping time available to LBL server
  _output->data.pingTime = ping_time;
  _output->write();

  confirmed = _ps8000->confirm("$OK\r\n",TIMEOUT);
  if (confirmed != ERROR)
    {
      if (verbose)
	_textLog->write("Ping Command Acknowledged");
    }
  else
    {
      if (verbose)
	_textLog->write("Ping Command not acknowledged");
      return -1;
    }

  if (verbose)
    _textLog->write("Confirming ping sent...");
  confirmed = _ps8000->confirm("$CCEGG,MAT, 9.0\r\n",TIMEOUT);
  if(confirmed != ERROR)
    {
      if (verbose)
	_textLog->write("Master ping transmitted");
      times();
    }
  else
    {
      _textLog->write("Error -- Master ping NOT transmitted");
      return -1;
    }

  return 0;
}


int PS8000::times()

{
  int status,nconv;
  char r_reply [105];
  char *start;
  Boolean debug = False;


  _ps8000->clearPort();
  _ps8000->blocking();

  try{ 
    status = _ps8000->readUntil(r_reply,102,"\n",(range_gate*1000.0+500));
  }

  catch(...){
    if (verbose)  
      _textLog->write("PS8000 -> timed out waiting for return times");
    return -1;
  }

  //exactly 102 characters are returned
  if (status == 102)
    if ((start = strstr(r_reply,"$CCEGG,ARV,"))!= NULL)
      {

	dprintf(" PS8000: The return string is %s\n", start);
      
	nconv=sscanf(start,"$CCEGG,ARV,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
		     &_rt->t1, &_rt->t2, &_rt->t3, &_rt->t4, &_rt->t5, &_rt->t6, 
		     &_rt->t7, &_rt->t8, &_rt->t9, &_rt->t10);
#if 0
	//
	// Temporary Debug Hack.  Only works with the appropriate worksite.cfg.
	//
	nconv = 10;
	_rt->t1 = .2;
	_rt->t10= _rt->t9 = _rt->t8 = _rt->t7 = _rt->t6 = _rt->t5 = _rt->t4 = 
	          _rt->t3 = _rt->t2 = _rt->t1;

#endif
	if (nconv < 10)
	  {
	    if (verbose)
	      _textLog->write("PS8000 -> Scan error reading return times");
	    return -1;
	  }
      }
    else
      {
	if (verbose)
	  _textLog->write("PS8000 -> Error reading return times - could not find start of message");
	return -1;
      }
  else
    {
      if (verbose)
	_textLog->write("PS8000 -> Error reading return times - did not return 102 bytes");
      return -1;
    }

  struct timespec sampleTime;
  clock_gettime(CLOCK_REALTIME, &sampleTime);
  _output->data.tofs.sampleTime.seconds = sampleTime.tv_sec;
  _output->data.tofs.sampleTime.nanoSeconds = sampleTime.tv_nsec;

  // Make the TOFs available to LBL Server
  _output->write();
  //
  // Write to disk after writing to shared memory.
  //
  _log->write();
  return 0;
}

void PS8000::processCommand(Task::Command *command)
{

  char *reply_str = "got it";

  //Make sure server doesn't get reply-blocked
  Reply(getppid(),reply_str,sizeof(reply_str));

  switch(command->code)
    {
      //driver now blocked until TOFs come back or timeout expires 
    case 1: ping();
      break;
      //not implemented yet
    case 2: Syslog::write("received change range gates command");
      break;
    default: Syslog::write("Unknown command received");

    }
}