/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : KvhCompass.cc                                                 */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <i86.h>
#include "KvhCompassDriver.h"
#include "Math.h"

KvhCompassDriver::KvhCompassDriver(const char *ttyName)
   : PeriodicTask("kvhCompassDriver")
{
  _device = new SerialDevice(ttyName);

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

  _baudRate = 9600;
  _dataBits = 8;
  _stopBits = 1;
  _parity   = "NONE";
  
  _kvhUpdateRate = 20;			// Update rate in Hz

  initialize();

  _output = new CompassOutput(SharedData::Write);
}


KvhCompassDriver::~KvhCompassDriver()
{
  printf ("~KVHCompassDriver()\n");
  delete _output;
}


void KvhCompassDriver::log(int millisec)
{
}


void KvhCompassDriver::status(char *str)
{
}


void KvhCompassDriver::currentData(char *str)
{
}


void KvhCompassDriver::run(void)
{
  startData();			// Start updates from compass

  while (True)
  {
    // Blocking read
    readData();

    printf ("KVH Heading %6.1f    Rate  %6.1f\n",
	    Math::radToDeg(_outputData.heading), 
	    Math::radToDeg(_outputData.headingRate));

    // Write to output
    _output->write(&_outputData);
  }
}

int KvhCompassDriver::initialize(void)
{
    int rate;
    
    _device->setLineFormat(_baudRate, _dataBits, _stopBits, _parity);

    setKvhAutocal(Off);		// Disable Auto calibrate mode        

    setKvhDefaultMsgType();	// Select default mesage type         

    setKvhFifoLongTermAvg(1);	// set fifo to 1 to disable averaging  

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

                                // Set compass heading update rate    
    setKvhUpdateRate(_kvhUpdateRate);
    
    getKvhUpdateRate(&rate);	// Confirm data update rate           

    if (rate != _kvhUpdateRate)
    {
      printf ("KVH Compass: Error setting data update rate\n");
      return ERROR;
    }

    return OK;
}

int KvhCompassDriver::readData(void)
{
  char kvhBuf[16];
  int a, b, nconv;

  char *sor, *eor;
  double mag_yaw_d, del_yaw_d;

  timespec updateTime;

  if (_device->read(kvhBuf, 16, 1000) == ERROR)
    return ERROR;

				// first check for autocal
  if (sor = strrchr(kvhBuf, '@'))
  {
    if (eor = strrchr(sor, '\r'))
      *eor = '\0';
  }
				// buf should begin with % but we see a $ 
  if (sor = strrchr(kvhBuf, '$'))
  {				// to convert the string, first test
				// that first char is a % to know it
				// is a valid string 
    nconv = sscanf( kvhBuf, "$%d,%d", &a, &b );

    if ( nconv == 2 )		// compass data string parsed OK  
    {
      clock_gettime(CLOCK_REALTIME, &updateTime);

      mag_yaw_d = ((double) a) / 10.0;
      del_yaw_d = ((double) b) / 100.0;

      _outputData.updateTime.tv_sec  = updateTime.tv_sec;
      _outputData.updateTime.tv_nsec = updateTime.tv_nsec;

      _outputData.heading     = Math::degToRad(mag_yaw_d);
                                // convert delta yaw to yaw rate  
      _outputData.headingRate = Math::degToRad(del_yaw_d) * _kvhUpdateRate;
    }
  }
  return OK;
}

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

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

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

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

    return status;
}

int KvhCompassDriver::setKvhDefaultMsgType(void)
{
    _device->write("=t,0\xd", 5);
    return _device->confirm(">\n", 4000);
}

int KvhCompassDriver::setKvhAutocal(Boolean mode)
{
  char cmdBuf[60];

  stopData();		// Stop automatic updates from compass 

  sprintf (cmdBuf, "$KVHCAL,0,O,0,U,0,D,HCHDM,F,%c,C,\n", 
      (mode == On ? 'O' : 'F')); 

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

  delay(200);

  _device->write("$CQ,\xd", 5);
  _device->confirm(">\n", 1000);

  _device->confirm(">\n", 1000);

  if (_device->read(cmdBuf, 60, 1000) == ERROR)
    return ERROR;

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

  return (strstr(cmdBuf, (mode == On ? "Cal=ON" : "Cal=OFF")) != NULL);
}


int KvhCompassDriver::setKvhFifoLongTermAvg(int sampleSize)
{
    char cmdBuf[16];

    sprintf(cmdBuf, "=s,%d\xd", sampleSize);
    _device->write(cmdBuf, strlen(cmdBuf));
                        // Confirm not required here, device sends data
    return stopData();	// Stop device sending data
}

int KvhCompassDriver::setKvhUpdateRate(int rate)
{
    char cmdBuf[16];

    sprintf( cmdBuf,  "=r,%0d\xd", rate );
    _device->write(cmdBuf, strlen(cmdBuf));

    return _device->confirm(">\n", 1000);
}


int KvhCompassDriver::getKvhUpdateRate(int *rate)
{
    int nread;

    char reply[16];

    _device->write("?r\xd", 3);
    if ( _device->confirm(">\n", 1000) == ERROR )
	return ERROR;

    nread = _device->read(reply, 16, 200);  

    if ( nread < 4 )
	return ERROR;

    if (sscanf(reply, "?r,%d", rate) != 1)
	return ERROR;
    
    return OK;
}