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

  DESCRIPTION: Device interface for Ixsea Octans gyrocompass

  $Id: Octans.cc,v 1.13 2001/08/06 22:15:41 hthomas 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 "SemaphoreP.h"
#include "MathP.h"

#include "Octans.h"

#include "BooleanAttribute.h"
#include "IntegerAttribute.h"
#include "AttributeParser.h"

#include "VehicleConfigurationIF.h"

#include "SerialDevice.h"
#include "Syslog.h"

#include "AHRSOutput.h"
#include "SharedData.h"

#define OctansConfigFileName "octans.cfg"
SerialDevice::lineFormat OctansLineFormat = { 19200, 8, 1, "NONE" };


//========================================================================= 
Octans::Octans( SerialDevice *device , enum OctansProtocol protocol) 
  :SerialDeviceDriver("Octans",
		      device,
		      100, "Q\x25\xc", 1000),
  _output(SharedData::Write)
{
     Boolean debug = True;
     VehicleConfigurationIF::Port port;
     short priority = 10;

     _device = NULL;
     _log = NULL;
     _octansDebugMode = False;
     _dumpHex = False;
     _protocol = protocol;
     // Load value from the configuration file
     //     loadConfig( OctansConfigFileName );

     // Clear out the output buffer
     _output.clear();
     _output.data.status = DeviceIF::Initializing;
     _output.write();

     // Create the log
     _log = new OctansLog( this, DataLog::BinaryFormat );

     _device = device;
     // Initialize the device itself
     initialize();

}

//========================================================================= 
Octans::~Octans() 
{
     if( _device )
	  delete _device;

     if( _log )
	  delete _log;
}

//========================================================================= 
DeviceIF::Status Octans::initialize() 
{
     if( _octansDebugMode )
	  Syslog::write( "Octans::initialize : Initializing Octans driver");

     _device->setLineFormat( &OctansLineFormat );
     switch (_protocol) {
     case gyrocompass1:
     case ptvf:
       _device->nonRaw(); _device->blocking(); break;

     case victor:
     case imbat:
      _device->raw(100,2); _device->nonBlocking(); break;
     }     

     printf("clearing port\n");
     _device->clearPort();

     printf("done clearing port\n");

     if( _octansDebugMode )
	  Syslog::write( "Octans::initialize : \t ...done!");

     _device->nonBlocking();
     _device->commsDebugMode(SerialDevice::DebugOff);
     _device->clearPort();
     _device->clearPort();
    return DeviceIF::Ok;
}

//========================================================================= 

//========================================================================= 
void Octans::octansDebugMode( Boolean newMode ) {
     _octansDebugMode = newMode;
}
  
//========================================================================= 
Boolean Octans::setHexDump( Boolean newVal ) {
     return _dumpHex = newVal;
}

//========================================================================= 
void Octans::status(char *str)
{

}

//========================================================================= 
void Octans::currentData(char *str)
{

}

//========================================================================= 
#define VICTOR_HDR "Q\x25\x0C"
#define VICTOR_HDR_LENGTH (3)
#define VICTOR_RECORD_LENGTH (37)

DeviceIF::Status Octans::readASCIIRecord(unsigned char *record,int maxBytes,
					 int readTimeout,
					 int *nbytesRead)
{
  Boolean readError = False;

  try {
    //    *nbytesRead = _device->readUntil((char *)record, maxBytes,"\xd\xa",
    //				     readTimeout);
    *nbytesRead = ::read(_device->getFd(), record, maxBytes);
  }

  catch (SerialDevice::TimedOut) {
    readError = True;
    Syslog::write("%s::readRecord() - serial device timed out", name());
  }
  catch (SerialDevice::BufferFull) {
    readError = True;
    _device->clearPort();
    Syslog::write("%s::readRecord() - buf=", name(), record);
    Syslog::write("%s::readRecord() - serial device buffer full", name());
  }
  catch (Exception e) {
    readError = True;
    Syslog::write("%s::readRecord() - caught exception:  abort", name());
    throw;
  }

  if (readError)
    return DeviceIF::Error;
  else
    return DeviceIF::Ok;
}

void swapBytes(unsigned char *dest, unsigned char *src, int nbytes)
{
  int i;
  for (i = 0; i < nbytes; i++) {
    dest[i] = src[nbytes-1-i];
  }
}

DeviceIF::Status Octans::readVictorRecord(unsigned char *record, int maxBytes,
					  int readTimeout, int *nbytesRead)
{
  Boolean debug = False;
  Boolean readError = False;

  try {
    // Read until we get the header from octans
    *nbytesRead = _device->readUntil((char *)record, maxBytes, 
				     (char *)VICTOR_HDR, 
				     readTimeout);
    printf("read %d bytes trying to get header\n", *nbytesRead);
  }
  catch (SerialDevice::TimedOut) {
    readError = True;
    Syslog::write("%s::readRecord() - serial device timed out", name());
  }
  catch (SerialDevice::BufferFull) {
    readError = True;
    Syslog::write("%s::readRecord() - buf=", name(), record);
    Syslog::write("%s::readRecord() - serial device buffer full", name());
  }
  catch (Exception e) {
    readError = True;
    Syslog::write("%s::readRecord() - caught exception:  abort", name());
    throw;
  }

  //read reset of record into record
  float vals[9];
  memcpy(record, record+(*nbytesRead-VICTOR_HDR_LENGTH), VICTOR_HDR_LENGTH);
  while (_device->nRecvdBytes() <= VICTOR_RECORD_LENGTH) {};
  *nbytesRead = ::read(_device->getFd(), (char *)vals, 
		       36);
  printf("read %d bytes trying to get reset of packet\n", *nbytesRead);
  *nbytesRead = 40;
  float val;
  int i; 
  for (i=0; i<9; i++) {
    printf("val %d is %f\n", i, vals[i]);
  }
  printf("%d bytes read\n", *nbytesRead);
  memcpy((unsigned char *)&val, record+15, 4);
  printf("yaw is %f 0x%x\n", val*180.0/3.14, val);
  printf("bytes are 0x%2x%2x%2x%2x\n", record[15], record[16], record[17], record[18]);
  printf("checksum is %d\n", record[39]);
  dprintf("StreamSerialDriver::readRecord() - done");

  if (readError)
    return DeviceIF::Error;
  else
    return DeviceIF::Ok;
}

DeviceIF::Status Octans::readImbatRecord(unsigned char *record, int maxBytes,
					 int readTimeout, int *nbytesRead)
{
  Boolean debug = False;
  Boolean readError = False;
  unsigned char char1, char2;

  try {
    // Read until we get the header from octans
    _device->read((char *)&char1, 1, 1000);
    do {
      _device->read((char *)&char2, 1,1000);
      if (char1==0x00 && char2==0x90) break;
      Syslog::write("looking for header\n");
      char1 = char2;
    } while(1);
  }
  catch (SerialDevice::TimedOut) {
    readError = True;
    Syslog::write("%s::readRecord() - serial device timed out", name());
  }
  catch (SerialDevice::BufferFull) {
    readError = True;
    Syslog::write("%s::readRecord() - buf=", name(), record);
    Syslog::write("%s::readRecord() - serial device buffer full", name());
  }
  catch (Exception e) {
    readError = True;
    Syslog::write("%s::readRecord() - caught exception:  abort", name());
    throw;
  }

  //read reset of record into record
  record[0] = char1; record[1] = char2;
  //while (_device->nRecvdBytes() <= 22) {};
  try {
    _device->read((char *)record+2,22, 100);
  }
  catch (SerialDevice::TimedOut) {
    readError = True;
    Syslog::write("%s::readRecord() - timed out getting packet", name());
  }
  catch (SerialDevice::BufferFull) {
    readError = True;
    Syslog::write("%s::readRecord() - buf=", name(), record);
    Syslog::write("%s::readRecord() - serial device buffer full", name());
  }
  catch (Exception e) {
    readError = True;
    Syslog::write("%s::readRecord() - caught exception:  abort", name());
    throw;
  }

  *nbytesRead = 24;

  if (readError)
    return DeviceIF::Error;
  else
    return DeviceIF::Ok;
}

DeviceIF::Status Octans::readRecord(unsigned char *record, int maxBytes,
				    const char *recordTerminator,
				    unsigned readTimeout, int *nbytesRead)
{
  Boolean debug = False;
  dprintf("Octans::readRecord()");
  Boolean readError = False;
  DeviceIF::Status readStatus;

  try {
    switch (_protocol) {
    case gyrocompass1:
    case ptvf:
      readStatus = readASCIIRecord(record, maxBytes, readTimeout, nbytesRead);

      break;
    case victor:
      readStatus = readVictorRecord(record, maxBytes, readTimeout, nbytesRead);
      break;
    case imbat:
      readStatus = readImbatRecord(record, maxBytes, readTimeout, nbytesRead);
      break;
    }
  }

  catch (SerialDevice::TimedOut) {
    readError = True;
    Syslog::write("%s::readRecord() - serial device timed out", name());
    return DeviceIF::Error;
  }

  catch (SerialDevice::BufferFull) {
    readError = True;
    Syslog::write("%s::readRecord() - buf=", name(), record);
    Syslog::write("%s::readRecord() - serial device buffer full", name());
    _device->clearPort();
    _device->clearPort();
    return DeviceIF::Error;
  }

  catch (Exception e) {
    readError = True;
    Syslog::write("%s::readRecord() - caught exception:  abort", name());
    throw;
  }

  if (readError || readStatus == DeviceIF::Error)
    return DeviceIF::Error;
  else
    return DeviceIF::Ok;

}

DeviceIF::Status Octans::processASCIIRecord(unsigned char *record, int nbytes)
{

  if (!strncmp("$HEHDT", (char *)record, 6)) {
    float heading;
    unsigned char checksum;
    sscanf((char *)record,"$HEHDT,%f,T*%x", &heading, &checksum);
    if (heading > 180.0) heading -= 360;
    if (checkChecksum(record+1, checksum)==ERROR) {
      printf("checksum invalid\n");
      _output.data.status = DeviceIF::Error;
      return DeviceIF::Error;
    }
    _output.data.yaw = Math::degToRad(heading);
    _output.data.status = DeviceIF::Ok;
    return DeviceIF::Ok;
  } else if (!strncmp("$PHTRO", (char *)record, 6)) {
    float pitch, roll;
    char pitchSign, rollSign;
    unsigned char checksum;
    sscanf((char *)record,"$PHTRO,%f,%c,%f,%c*%x",
	   &pitch, &pitchSign, &roll, &rollSign, &checksum);
    if (checkChecksum(record+1, checksum)==ERROR) {
      printf("checksum invalid\n");
      _output.data.status = DeviceIF::Error;
      return (DeviceIF::Error);
    }
    if (pitchSign == 'P') pitch *= -1.0;
    if (rollSign == 'B') roll *= -1.0;
    _output.data.roll = Math::degToRad(roll);
    _output.data.pitch = Math::degToRad(pitch);
    _output.data.status = DeviceIF::Ok;
    return DeviceIF::Ok;
  } else if (!strncmp("$PHINF", (char *)record, 6)) {
    unsigned long status;
    unsigned char checksum;
    sscanf((char *)record,"$PHINF,%x*%x",&status, &checksum);
    if (checkChecksum(record+1, checksum)==ERROR) {
      printf("checksum invalid\n");
    }
    _output.data.octansStatus = status;
    return DeviceIF::Ok;
  } else if (!strncmp("$PTVF", (char *)record,5)) {
    int pitch, roll;
    float pitchRate, yawRate, rollRate;
    float heading, speed;
    unsigned int mode, status;
    unsigned char checksum;
    float val;
    int num;

    num = sscanf((char *)record,
		 "$PTVF, %dP, %dR,%fT,%fPR,%fRR,%fAR, %fN,%dMD,%4xAL*%2x",
		 &pitch, &roll, &heading,
		 &pitchRate, &rollRate, &yawRate, &speed,
		 &mode, &status, &checksum);
    if (num != 10) {
      Syslog::write("Octans::num is %d, unable to parse string %s\n",
		    num, record);
      return DeviceIF::Error;
    }

    //we have to flip the sign of roll and pitch to follow the dorado coord
    //system
    val = (float)(roll/100) + (float)(roll%100)/60.0;
    _output.data.roll = Math::degToRad(val); //degrees
    val = (float)(pitch/100) + (float)(pitch%100)/60.0;
    _output.data.pitch = Math::degToRad(-val); //degrees

    //now do the rest
    _output.data.yaw = Math::degToRad(heading); //degrees
    _output.data.accelX = 0.0; //micro-g's
    _output.data.accelY = 0.0; //micro-g's
    _output.data.accelZ = 0.0; //micro-g's
    _output.data.rollRate = Math::degToRad(1.0*rollRate); //degrees per sec
    _output.data.pitchRate = Math::degToRad(-1.0*pitchRate); //degrees per sec
    _output.data.yawRate = Math::degToRad(yawRate); //degrees per sec
    _output.data.octansStatus = status;

    if (checkChecksum(record+1, checksum)==ERROR) {
      Syslog::write("checksum is invalid, checksum is %d\n", checksum);
      _output.data.status = DeviceIF::Error;
    } else {
      _output.data.status = DeviceIF::Ok;
    }
    return _output.data.status;
  }

  return DeviceIF::Error;

}

DeviceIF::Status Octans::processVictorRecord(unsigned char *record, int nbytes)
{
    
  struct victorDataPacket data;
  int checksum = 0, i;
  float val;

  for (i=0;i<39;i++) checksum += record[i];
  checksum &= 0xff;

  data.Header = *record++;
  data.numBytes = *record++;
  data.userID = *record++;
  memcpy((unsigned char *)&data.roll,record, 4); record+=4;
  memcpy((unsigned char *)&data.pitch, record, 4); record+=4;
  memcpy((unsigned char *)&data.yaw, record, 4); record+=4;
  memcpy((unsigned char *)&data.rollRate, record, 4); record+=4;
  memcpy((unsigned char *)&data.pitchRate, record, 4); record+=4;
  memcpy((unsigned char *)&data.yawRate, record, 4); record+=4;
  memcpy((unsigned char *)&data.rollAxisAcc, record, 4); record+=4;
  memcpy((unsigned char *)&data.pitchAxisAcc, record, 4); record+=4;
  memcpy((unsigned char *)&data.yawAxisAcc, record, 4); record+=4;
  data.checksum = *record;

  printf("Header: %c\n", data.Header);
  printf("numBytes: %d\n", data.numBytes);
  printf("userID: %d\n", data.userID);
  printf("roll: %f pitch: %f yaw: %f\n", (double)data.roll,
	 (double)data.pitch,
	 (double)data.yaw);
  printf("rollRate: %f pitchRate: %f yawRate: %f\n",
	 (double)data.rollRate*180.0/3.14,
	 (double)data.pitchRate*180.0/3.14,
	 (double)data.yawRate*180.0/3.14);
  printf("rollAxisAcc: %f pitchAxisAcc: %f yawAxisAcc: %f\n",
	 data.rollAxisAcc, data.pitchAxisAcc, data.yawAxisAcc);
  printf("checksum: %d should be %d\n", data.checksum, checksum);
  return DeviceIF::Ok;
}

DeviceIF::Status Octans::processImbatRecord(unsigned char *record, int nbytes)
{
  short vals[11];
  unsigned char imbatStatus, trailer;

  memcpy(vals, record, 22);
  imbatStatus = record[22];
  trailer = record[23];

  if ((vals[0]&0x0000ffff) != 0x9000) {
    Syslog::write("header is wrong\n");
    Syslog::write("header is 0x%4x\n", vals[0]);
    return DeviceIF::Error;
  }

  if (trailer != 0x91) {
    Syslog::write("trailer is wrong\n");
    return DeviceIF::Error;
  }

  _output.data.roll = Math::degToRad(vals[1]*.00549); //degrees
  _output.data.pitch = Math::degToRad(vals[2]*.00549); //degrees
  _output.data.yaw = Math::degToRad(vals[4]*.00549); //degrees
  _output.data.accelX = vals[5]*.0000152; //micro-g's
  _output.data.accelY = vals[6]*.0000152; //micro-g's
  _output.data.accelZ = vals[7]*.0000152; //micro-g's
  _output.data.rollRate = Math::degToRad(vals[8]*.00061); //degrees per sec
  _output.data.pitchRate = Math::degToRad(vals[9]*.00061); //degrees per sec
  _output.data.yawRate = Math::degToRad(vals[10]*.00061); //degrees per sec
  _output.data.octansStatus = imbatStatus;
  if (imbatStatus == 0xff) {
      _output.data.status = DeviceIF::Ok;
  } else {
      _output.data.status = DeviceIF::Error;
  }

  return _output.data.status;
}

DeviceIF::Status Octans::processRecord( unsigned char *record, int nbytes)
{
  DeviceIF::Status processStatus;

  switch (_protocol) {
  case gyrocompass1:
  case ptvf: processStatus = processASCIIRecord(record, nbytes); break;
  case victor: processStatus = processVictorRecord(record, nbytes); break;
  case imbat: processStatus = processImbatRecord(record, nbytes); break;
  }

  _output.write();
  _log->write();

  return processStatus;
}

//========================================================================= 
int Octans::checkChecksum( unsigned char *data,
			   unsigned char csum)
{
     unsigned char i, sum=0;
     unsigned char *point = (unsigned char *)data;

     while (*data != '*') {
       sum ^= *data++;
     }

     if( _octansDebugMode )
	  Syslog::write("Checksum = %x", sum );

     return (sum == csum) ? OK : ERROR;
}