/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : KvhDriver.cc                                                  */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

#include <i86.h>
#include "KvhDriver.h"
#include "Math.h"
#include "Syslog.h"

KvhDriver::KvhDriver(const char *name,
		     SerialDevice *device,
		     unsigned maxRecordBytes,
		     const char *recordTerminator,
		     unsigned updateRate,
		     unsigned readTimeout)

  : StreamSerialDriver(name, device, maxRecordBytes, 
		       recordTerminator, readTimeout)
{
  _updateRate = updateRate;
}


KvhDriver::~KvhDriver()
{
}


int KvhDriver::startData( void )
{
  _device->write("s\xd", 2);
  return _device->confirm(">\n", KvhTimeout);
}


int KvhDriver::stopData(void)
{
  int status;

  do {
    _device->clearPort();	// Flush input port                   

    _device->write("h\xd", 2);
    status = _device->confirm(">\n", KvhTimeout);
    
  } while ((status != OK) || (_device->nRecvdBytes() > 0));

  return status;
}


int KvhDriver::setDefaultMsgType(void)
{
  _device->write("=t,0\xd", 5);
  return _device->confirm(">\n", KvhTimeout);
}


int KvhDriver::setFifoLongTermAvg(int sampleSize)
{
  char cmdBuf[16];

  sprintf(cmdBuf, "=s,%d\xd", sampleSize);
  _device->write(cmdBuf, strlen(cmdBuf));

  // Confirm not required here, device sends data
  // Note there is actually a device reset in here, with
  // a boot-up message (which we ignore) --- this means
  // the halt may take a couple of loops

  // Pausing here avoids timeout in stopData()'s call to 
  // SerialDevice::confirm()
  sleep(1);

  return stopData();	// Stop device sending data
}


int KvhDriver::setUpdateRate(int rate)
{
  Boolean debug = True;
  dprintf("KvhDriver::setUpdateRate(%d)", rate);

  char cmdBuf[16];

  sprintf( cmdBuf,  "=r,%0d\xd", rate );

  try {
    _device->write(cmdBuf, strlen(cmdBuf));

    return _device->confirm(">\n", KvhTimeout);
  }
  catch (SerialDevice::TimedOut e) {
    Syslog::write("KvhDriver::setUpdateRate() - "
		     "serial device timed out");
    return ERROR;
  }
  catch (SerialDevice::BufferFull e) {
    Syslog::write("KvhDriver::setUpdateRate() - "
		  "serial device filled buffer");
    return ERROR;
  }
  catch (...) {
    Syslog::write("KvhDriver::setUpdateRate() - bad juju!");
    return ERROR;
  }
}


int KvhDriver::getUpdateRate(int *rate)
{
  Boolean debug = False;

  int nread;

  char reply[16];

  try {
    _device->write("?r\xD\xA", 4);

    // Wait for start of reply
    if ( _device->readUntil(">\n", KvhTimeout) == ERROR ) {
      Syslog::write("KvhDriver::getUpdateRate() - "
		    "SerialDevice::readUntil() returned ERROR");
      return ERROR;
    }

    // Read reply
    nread = _device->readUntil(reply, sizeof(reply), "\n", KvhTimeout);
  }
  catch (SerialDevice::TimedOut e) {
    Syslog::write("KvhDriver::getUpdateRate() - "
		  "serial device timed out");
    return ERROR;
  }
  catch (SerialDevice::BufferFull e) {
    Syslog::write("KvhDriver::getUpdateRate() - "
		  "serial device filled buffer");
    return ERROR;
  }
  catch (...) {
    Syslog::write("KvhDriver::getUpdateRate() - bad juju!");
    return ERROR;
  }

  dprintf("KvhDriver::getUpdateRate() - nread=%d, reply=\"%s\"",
	  nread, reply);

  if ( nread < 1 ) {
    Syslog::write("KvhDriver::getUpdateRate() - nread =%d bytes",
		  nread);
    return ERROR;
  }

  if (sscanf(reply, "?r,%d", rate) != 1) {
    Syslog::write("KvhDriver::getUpdateRate() - "
		  "couldn't parse rate from \"%s\"", reply);
    return ERROR;
  }

  return OK;
}


int KvhDriver::setConfirmUpdateRate(int newRate ) 
{
  Boolean debug = False;
  int rate;

  // Let's assume we get the new rate!
  _updateRate = newRate;

  if (setUpdateRate(newRate) == ERROR) {
    Syslog::write("KvhDriver: setUpdateRate() failed");
    return ERROR;
  }
    
  // Confirm data update rate           
  if (getUpdateRate(&rate) == ERROR) {
    Syslog::write("KvhDriver: getUpdateRate() failed");
    return ERROR;
  }

  if (rate != newRate) {
    Syslog::write("KvhDriver: Error setting data update rate\n");
    return ERROR;
  }

  return OK;
}