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

  DESCRIPTION: Driver from the Kvh Compass

  $Id: KvhCompassDriver.cc,v 1.10 2000/02/25 19:56:04 oreilly Exp $
  *-----------------------------------------------------------------------*/

/*-----------------------------------------------------------------------*
  A NOTE ON COORDINATE SYSTEMS:

  This driver has been programmed to be consistent with the following 
  right-hand coordinate system:
  
  +X toward the nose of the AUV
  +Y to the starboard
  +Z down

  In this coord. system:

  Positive pitch is nose up
  Positive roll is toward the starboard (starboard down)
  Positive yaw is toward the starboard
 *-----------------------------------------------------------------------*/

#include <i86.h>

#include "Syslog.h"
#include "KvhCompassDriver.h"
#include "Math.h"

#define MaxRecordBytes 512
#define ReadTimeout 4000

SerialDevice::lineFormat compassLineFormat = { 9600, 8, 1, "NONE" };


KvhCompassDriver::KvhCompassDriver(SerialDevice *device,
				   int updateRate)
     : KvhDriver("kvhCompassDriver", device, MaxRecordBytes, "\xa",
		 updateRate, ReadTimeout)
{
  _log = new KvhCompassLog(this, DataLog::BinaryFormat);
  _output = new KvhCompassOutput(SharedData::Write);
}


KvhCompassDriver::~KvhCompassDriver()
{
  delete _log;
  delete _output;
}


DeviceIF::Status KvhCompassDriver::initialize()
{
  Boolean debug = True;

  int rate;
    
  _device->setLineFormat( &compassLineFormat );

  Syslog::write("KvhCompassDriver::initialize()");

  // Just in case it's going
  stopData(); 

  _device->clearPort();

  //  setAutocal(Off);		// Disable Auto calibrate mode        

  setDefaultMsgType();	// Select default mesage type         

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

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

  // Set compass heading update rate    
  setConfirmUpdateRate(_updateRate);

  startData();

  _output->data.deviceReady = True;
  _output->write();
  dprintf("KvhCompassDriver::initialize() - ready!");

  Syslog::write("KvhCompassDriver::initialize() - finished");

  return DeviceIF::Ok;
}


DeviceIF::Status KvhCompassDriver::processRecord(unsigned char *record, 
						 int nBytes)
{
  Boolean debug = False;

  char *kvhBuf = (char *)record;

  int a, b, nconv;

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

  timespec updateTime;

  // 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, '%')) {

    dprintf("KvhCompassDriver::processRecord() - %s", 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;

      _output->data.updateTime.tv_sec  = updateTime.tv_sec;
      _output->data.updateTime.tv_nsec = updateTime.tv_nsec;

      _output->data.heading     = Math::degToRad(mag_yaw_d);
      // convert delta yaw to yaw rate  
      _output->data.headingRate = Math::degToRad(del_yaw_d) * _updateRate;

      // Write data to shared memory
      _output->write();

      _log->write();

      return DeviceIF::Ok;
    }
    else {
      Syslog::write("KvhCompassDriver::processRecord() - error parsing %s",
		    kvhBuf);
      return DeviceIF::Error;
    }
  }
  else
    return DeviceIF::Error;
}


int KvhCompassDriver::setAutocal(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,\n", 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);
}