/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : KvhInclinoDriver.cc                                           */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*
  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 "Syslog.h"
#include "KvhInclinoDriver.h"
#include "Math.h"

#define MaxRecordBytes 512
#define ReadTimeout 4000

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

KvhInclinoDriver::KvhInclinoDriver( SerialDevice *device,
				    int updateRate)
  : KvhDriver("kvhInclinoDriver", device, MaxRecordBytes, "\xa",
	      updateRate, ReadTimeout)
{
  _log = new KvhInclinoLog(this, DataLog::BinaryFormat);
  _output = new KvhInclinoOutput(SharedData::Write);
}


KvhInclinoDriver::~KvhInclinoDriver()
{
  delete _log;
}


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

  _device->setLineFormat( &InclinoLineFormat );

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

  // Stop the port
  stopData();

  _device->clearPort();

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

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

  // Set Inclino heading update rate    
  if (setConfirmUpdateRate( _updateRate ) == ERROR) {
    Syslog::write("KvhInclinoDriver()::setConfirmUpdateRate() failed");
    return DeviceIF::Error;
  }

  _device->clearPort();
     
  startData();

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

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

  return DeviceIF::Ok;
}


DeviceIF::Status KvhInclinoDriver::processRecord(unsigned char *record, 
						 int nRecordBytes)
{
  Boolean debug = False;
  int pitchRaw, rollRaw, pitchDeltaRaw, rollDeltaRaw, nconv;
  char *sor, *eor;
  double pitch, roll, pitchDelta, rollDelta;
  timespec updateTime;

  char *kvhBuf = (char *)record;

  if (sor = strrchr(kvhBuf, '%')) {

    dprintf("KvhInclinoDriver::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,%d,%d", 
		    &pitchRaw, &rollRaw, 
		    &pitchDeltaRaw, &rollDeltaRaw );

    if ( nconv == 4 ) {
      // Inclino data string parsed OK  
      clock_gettime(CLOCK_REALTIME, &updateTime);
      // For explanation of the negation of pitch, see
      // the coord system note above.

      pitch      = -((double)pitchRaw     / 10.0 );
      roll       =  ((double)rollRaw      / 10.0 );
      pitchDelta =  ((double)pitchDeltaRaw / 100.0 );
      rollDelta  = -((double)rollDeltaRaw  / 100.0 );

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

      _output->data.pitch       = Math::degToRad( pitch );
      _output->data.roll        = Math::degToRad( roll  );

      _output->data.pitchRate = Math::degToRad( pitchDelta ) * _updateRate;
      _output->data.rollRate = Math::degToRad( rollDelta ) * _updateRate;

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

      _log->write();

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

      return DeviceIF::Error;
    }
  }
  else
    return DeviceIF::Error;
}