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

  DESCRIPTION: Driver from the Kvh Base

  $Id: KvhBaseDriver.cc,v 1.4 2000/02/08 00:00:54 pean Exp $
  *-----------------------------------------------------------------------*/

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


KvhBaseDriver::KvhBaseDriver( SerialDevice *newBaseDev,
				    KvhOutput::Data *newData )
{

     _device = newBaseDev;
     _outputData = newData;

     _kvhUpdateRate = 0;

}


KvhBaseDriver::~KvhBaseDriver()
{
     printf ("~KVHBaseDriver()\n");
}


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

int KvhBaseDriver::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 KvhBaseDriver::setKvhDefaultMsgType(void)
{
     _device->write("=t,0\xd", 5);
     return _device->confirm(">\n", KvhTimeout);
}


int KvhBaseDriver::setKvhFifoLongTermAvg(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 KvhBaseDriver::setKvhUpdateRate(int 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("KvhBaseDriver::setKvhUpdateRate() - "
		     "serial device timed out");
       return ERROR;
     }
     catch (SerialDevice::BufferFull e) {
       Syslog::write("KvhBaseDriver::setKvhUpdateRate() - "
		     "serial device filled buffer");
       return ERROR;
     }
     catch (...) {
       Syslog::write("KvhBaseDriver::setKvhUpdateRate() - bad juju!");
       return ERROR;
     }
}


int KvhBaseDriver::getKvhUpdateRate(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("KvhBaseDriver::getKvhUpdateRate() - "
		       "SerialDevice::readUntil() returned ERROR");
	 return ERROR;
       }

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

     dprintf("KvhBaseDriver::getKvhUpdateRate() - nread=%d, reply=\"%s\"",
	     nread, reply);

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

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

     return OK;
}

int KvhBaseDriver::setConfirmKvhUpdateRate(int newRate ) 
{
  Boolean debug = False;
     int rate;

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

     if (setKvhUpdateRate(newRate) == ERROR) {
       Syslog::write("KvhBaseDriver: setKvhUpdateRate() failed");
       return ERROR;
     }
    
     // Confirm data update rate           
     if (getKvhUpdateRate(&rate) == ERROR) {
       Syslog::write("KvhBaseDriver: getKvhUpdateRate() failed");
       return ERROR;
     }

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

     return OK;
}