/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : PSA916.cc                                                     */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/uio.h>
#include <fcntl.h>
#include <errno.h>
#include <iostream.h>

#include "PSA916.h"
#include "RangeFinderIF.h"
#include "Syslog.h"

#define BAUD 9600
#define PARITY "NONE"
#define STOP_BITS 1
#define DATA_BITS 8
#define READ_TIMEOUT 750

#define PSA916PERIOD 2000

#define MaxRecordBytes 200

PSA916::PSA916(SerialDevice *device)
  : StreamSerialDriver("PSA916", device, MaxRecordBytes, "\xa",
		       READ_TIMEOUT )
{
  Syslog::write("PSA916: constructing...\n");
  _device = device;
  _input = new PSA916Input();
  _output = new PSA916Output();

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


PSA916::~PSA916()
{
  delete _input;
  delete _output;
}


/* Init function simply clears serial port and then confirms that
   lines beginning with the character "R" are streaming from the
   device */

DeviceIF::Status PSA916::initialize(void)
{
  char buf[30];
  
  Syslog::write("PSA916: initializing...\n");

  _output->data.range = 0.00;
  _output->data.updateTime.tv_sec = 0.0;
  _output->data.updateTime.tv_nsec = 0.0;
  _valid = False;
  Syslog::write("PSA916: trying to write data...\n");
  try
    {
      _output->write();
    }
  catch (SharedData::AccessError error)
    {
      fprintf(stderr, "%s",error.msg);
      return DeviceIF::Error;
    }
  //  Syslog::write("PSA916: data written...\n");
  _device->commsDebugMode(SerialDevice::DebugOff);
  //  _device->commsDebugMode(SerialDevice::DebugOn);
  Syslog::write("PSA916: debug mode on...\n");
  // initialize serial port
  _device->setLineFormat(BAUD, DATA_BITS, STOP_BITS, PARITY);
  //  Syslog::write("PSA916: format set...\n");
  _device->printTermios();
  //  Syslog::write("PSA916: termios printed...\n");
  // clear serial port
  
  _device->clearPort();
  _device->flushReceiveBuf();
  
  // look for R to confirm data is streaming
  
  Syslog::write("PSA916: confirming data stream...\n");
  if (_device->confirm("R",1000) == -1)
    {
      Syslog::write("PSA916: data not streaming...\n");
      return DeviceIF::Error;
    }
  else
    {
      Syslog::write("PSA916: data is streaming...\n");
    }

  _output->data.deviceReady = True;
  _output->write();

  return DeviceIF::Ok;
} 


DeviceIF::Status PSA916::processRecord(unsigned char *record, int nBytes)
{
  // Read any input from server
  _input->read();

  // If disabled, just return
  //  if (!_input->data.enabled)
  //    return DeviceIF::Ok;

  /* loop through buffer until port is cleared to ensure most recent hit */
  /* determine if data is good or "E" for no echo */
  /* data is of the form "R<float><cr><lf> if good, */
  /* and of the form     "R<float>E<cr><lf> if no echo */

  int nconv;
  int i;
  double read_range = 0.0;
  char R, code;
  char reply2[5];

  Boolean debug = False;

  dprintf("processRecord() - %s", record);

  // the below is a hack as a replacement to sscanf
  // because sscanf isn't behaving properly...
  

  char *reply = (char *)record;

  //  nconv = sscanf(reply2,"%lf",&read_range);
  read_range = atof(reply+1);
  code = reply[strlen(reply)-1];

  Boolean error = False;

  if ((reply[0] == 'R') && code != 'E' )
    {
      /* NOTE: We might have the range reading as "99.99", which
	 really indicates no echo returned, even though the
	 error code is okay.
      */
      _range = read_range;
      clock_gettime(CLOCK_REALTIME, &_readtime);
      _valid = True;
    }
  else if (code  == 'E')
    {
      _range = read_range;
      clock_gettime(CLOCK_REALTIME, &_readtime);
      _valid = False;
    }
  else //we shouldn't get here
    {
      _range = -999.9;
      clock_gettime(CLOCK_REALTIME, &_readtime);
      _valid = False;
      Syslog::write("PSA916::processRecord() - "
		    "this should not have happened: record=%s", record);
      error = True;
    }


  _output->data.range = _range;
  _output->data.echoReceived = _valid;
  _output->data.updateTime.tv_sec = _readtime.tv_sec;
  _output->data.updateTime.tv_nsec = _readtime.tv_nsec;
   try
    {
      _output->write();
    }
  catch (SharedData::AccessError error)
    {
      fprintf(stderr, "%s",error.msg);
      exit(1);
    }
  // and now log the data
  _log->write();

  if (error)
    return DeviceIF::Error;
  else
    return DeviceIF::Ok;
}