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

  DESCRIPTION: Device interface for Crossbow DMU-AHRS.

  $Id: Crossbow.cc,v 1.16 2001/06/02 21:33:38 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 "Crossbow.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 CrossbowConfigFileName "crossbow.cfg"

SerialDevice::lineFormat CrossbowLineFormat = { 38400, 8, 1, "NONE" };

//========================================================================= 
Crossbow::Crossbow( SerialDevice *device ) 
     : PeriodicTask(CrossbowTaskName),
       _output( SharedData::Write ),
       _command( SharedData::ReadWrite )
{
     Boolean debug = True;
     VehicleConfigurationIF::Port port;
     short priority = 10;

     _device = NULL;
     _log = NULL;

     // Default values
     _crossbowPeriod = 200;
     _crossbowDebugMode = False;
     _dumpHex = False;
    
     // Load value from the configuration file
     loadConfig( CrossbowConfigFileName );

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

     _device = device;

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

     // Initialize the device itself
     initialize();

     // Add the periodic callback
     addPeriodicCallback( _crossbowPeriod, 
			  (CallbackMethod)Crossbow::crossbowCallback );

}

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

     if( _log )
	  delete _log;
}

//==========================================================================
void Crossbow::loadConfig( const char *configFile ) {
     char fullConfigFile[256];
     Attributes attributes( "CrossbowConstants" );

     strcpy( fullConfigFile, System::configurationFile(configFile));

     attributes.add( new IntegerAttribute("crossbowPeriod",
					  "Period of crossbow queries (ms)",
					  &_crossbowPeriod,
					  200 ) );
     attributes.add( new BooleanAttribute("debug",
					  "Extra debugging output",
					  &_crossbowDebugMode,
					  False ) );
     attributes.add( new BooleanAttribute("dumpHex",
					  "Provides a hex dump of Crossbow input",
					  &_dumpHex,
					  False ) );

     
     try {
          AttributeParser::parse(fullConfigFile, &attributes);
     }
     catch(...)
     {
          Syslog::write("VehicleConstants -- Caught exception while "
                           "loading vehicle constants");
          return;
     }

}

//========================================================================= 
int Crossbow::initialize( void ) 
{
  //     _device->commsDebugMode( SerialDevice::DebugOn );

     if( _crossbowDebugMode )
	  Syslog::write( "Crossbow::initialize : Initializing Crossbow driver");

     _device->setLineFormat( &CrossbowLineFormat );
     _device->raw();
     _device->blocking();

     setPolledMode();
     setAngleMode();

     _device->clearPort();

     if( _crossbowDebugMode )
	  Syslog::write( "Crossbow::initialize : \t ...done!");

     _device->blocking();

     getSerialNumber();

     setTFactor( defaultTFactor );

    return OK;
}

//========================================================================= 
void Crossbow::log(int millisec)
{

}

//========================================================================= 
void Crossbow::crossbowDebugMode( Boolean newMode ) {
     _crossbowDebugMode = newMode;
}
  
//========================================================================= 
Boolean Crossbow::setHexDump( Boolean newVal ) {
     return _dumpHex = newVal;
}

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

}

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

}

//========================================================================= 
void Crossbow::crossbowCallback()
{
     _device->clearPort();
     
     // Place a data request with the Crossbow
     requestData();
     
     // Small delay -- this may not be necessary on the PC/104
     // We really had to fiddle with this value to avoid the
     // error from readNChars() in readData()
     //     System::milliSleep( 10 );
     System::milliSleep( 25 );

     // Blocking read...and parse
     readData();
     
     // Write the current data out
     _output.write();
     
     // Log the new data
     _log->write();

     // Check for a command, then process
     _command.read();
     if( _command.data.Request ) {
	  switch( _command.data.Request ) {
	  case CrossbowCommand::SerialNumber:
	       getSerialNumber();
	       break;
	  case CrossbowCommand::FirmwareVersion:
		    
	       break;
	  case CrossbowCommand::SetZero:
	       setZero( _command.data.ZeroSamples );
	       break;
	  case CrossbowCommand::SetTFactor:
	       setTFactor( _command.data.TFactor );
	       break;
	  }

	  _command.clear( );
	  _command.write( );

	  // Post the newly acquired data
	  _output.write();
     }
}

//========================================================================= 
int Crossbow::readData( void )
{
     rawAnglePacket *dataIn;
     char xbowBuf[anglePacketLen+2], *start;

     // Assume error until proven otherwise
     _output.data.status = DeviceIF::Error;
     
     int timeout;
     timeout = 1000;
     if( _device->readNChars( xbowBuf, anglePacketLen, timeout) == ERROR ) {
	  Syslog::write("Crossbow::readData: Insufficient characters");
	  return ERROR;
     }

     start = xbowBuf;

     if( start == NULL )
	  return ERROR;

     dataIn = (struct rawAnglePacket *)start;

     // Check the checksum on the packet
     if( checkChecksum( dataIn ) ) {
	  Syslog::write("Crossbow::readData: Checksum does not agree");
	  Syslog::dumpHex("Crossbow::readData:", xbowBuf, anglePacketLen );

	  _device->clearPort();
	  return ERROR;
     }

     // Generate real numbers from the binary in
     reverseByteOrder( dataIn );
     scaleRawAnglePacket( dataIn  );

     // At this point, we got good data.
     _output.data.status = DeviceIF::Ok;

     if( _crossbowDebugMode ) {
	  if( _dumpHex )
	       Syslog::dumpHex( "Crossbow Data", start, anglePacketLen );
	  
	  Syslog::write( "R = %7.2lf : P = %7.2lf : Y = %7.2lf",
			 Math::radToDeg(_output.data.roll), 
			 Math::radToDeg(_output.data.pitch), 
			 Math::radToDeg(_output.data.yaw) );
     }

     return OK;
}

//========================================================================= 
int Crossbow::setPolledMode( void )
{
     Syslog::write("Crossbow.cc: Setting polled mode");
     _device->write("P", 1);
     DataMode = POLLED;
     _device->flushReceiveBuf();

     return OK;
}

//========================================================================= 
int Crossbow::setContinuousMode( void )
{
     // Don't depend on the state variable DataMode.
     // Just maintain it.
     Syslog::write("Crossbow.cc: Setting continuous mode");
     _device->flushReceiveBuf();
     _device->write("C", 1);
     DataMode = CONTINUOUS;
     
     return OK;
}

//========================================================================= 
int Crossbow::setAngleMode( void )
{
     _device->write("a",1);

     // Needs to check for "A" response

     return OK;
}

//========================================================================= 
int Crossbow::requestData( void )
{
     _device->flushReceiveBuf();
     _device->write("G",1);
     return OK;
}

//========================================================================= 
int Crossbow::getSerialNumber( void ) 
{
     char xbowBuf[8];
     char *start;
     unsigned long serialNumber;
     int result = ERROR;
     CrossbowDataMode PrevMode;

     PrevMode = DataMode;
     setPolledMode();

     Syslog::write("Crossbow.cc: In getSerialNumber()");

     _device->write("S", 1);

     _device->clearPort();

     //     System::milliSleep( 100 );
     
     if( _device->readNChars( xbowBuf, 6, 500 ) == ERROR ) {
	  Syslog::write("Crossbow.cc: _device->read failed with error" );
	  return ERROR;
     }

     Syslog::write("Crossbow.cc: device->read succeeded");

     if( _dumpHex )
	  Syslog::dumpHex( "Crossbow", xbowBuf, 6 );

     start = xbowBuf;

     if( start ) {

	  start++;

	  if( _dumpHex )
	       Syslog::dumpHex( "Crossbow", start, 4 );
	  
	  swab( start, (char *)&serialNumber+2, 2 );
	  swab( start+2, (char *)&serialNumber, 2 );

	  if( _dumpHex )
	       Syslog::dumpHex( "Crossbow", (char *)&serialNumber, 4 );

	  sprintf(_output.data.serialNumber, "%d", serialNumber);

	  Syslog::write("Crossbow.cc: I think the serial number is %lu (%x)",
			serialNumber, serialNumber );	  

	  result = OK;

     }
	  
     setDataMode( PrevMode );

     return result;
}

//========================================================================= 
int Crossbow::setTFactor( int newFactor )
{
     char response[3];

     sprintf( response, "%c", (char)(newFactor & 0xFF ) );
     _device->write("T",1);
     _device->write( response, 1 );

     return OK;
}

//========================================================================= 
int Crossbow::setZero( int samples )
{
     int timeout;
     char convertedSamples[2];
     char response[3];

     samples /= 10;
     sprintf( convertedSamples, "%c", (char)(samples & 0xFF) );

     _device->write("z",1);
     _device->write(convertedSamples,1);

     timeout = samples * 50; // Estimate 30-40 ms per 10 samples

     if( _device->readNChars( response, 1, timeout ) == ERROR ) {
	  
	  Syslog::write("Crossbow.cc: _device->read failed with error");
	  
	  return ERROR;
	  
     }

     return OK;
     
}

//========================================================================= 
void Crossbow::clearCommand( void ) {
     _command.data.Request = CrossbowCommand::None;
     _command.write( );
}

//========================================================================= 
int Crossbow::checkChecksum( struct rawAnglePacket *testPacket )
{
     unsigned int i, sum=0;
     unsigned char *point = (unsigned char *)testPacket;

     for( i = 1; i < anglePacketLen-1; i++ ) 
	  sum += point[i];

     sum &= 0xFF;

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

     return (sum == testPacket->Checksum) ? OK : ERROR;
}

//========================================================================= 
int Crossbow::reverseByteOrder( struct rawAnglePacket *work ) {


     /* The Crossbow packet comes in MSB, LSB, which must be reversed
	on the ol' x86.  Pairs to be reversed are 1-2, 3-4, ...27-28
	See Crossbow.h for packet format */
     swab( (char *)work+1, (char *)work+1, anglePacketLen-2 );
    
     return OK;
}

//========================================================================= 
// angle (deg) = data * (SCALE)/2^15
//
//  for  roll, SCALE = 180 deg
//      pitch, SCALE =  90 deg
//        yaw, SCALE = 180 deg
//
// The manual reports the pitch scale as 90deg, but results come in
// correct with a scale of 180deg.  ???
//
const double scaleRoll   = 0.00549316406;
const double scalePitch  = 0.00549316406;
//const double scalePitch = 0.00274658203;
const double scaleYaw    = 0.00549316406;
//
// Angular Rate (deg/sec) = data * (AR*1.5)/2^15
//
//  where   AR = angular rate sensor limit
//               (+/- 100 deg/sec typical = use 100)
const double scaleAngRate = 0.00457763672;
//
// Acceleration (G) = data * (GR * 1.5)/2^15
//
//  where   GR = accelerometer limit (+/-2G typical = use 2)
const double scaleAccel = 0.0000915527344;
//
// mag field (gauss) = data * (MR * 1.5)/2^15
//
//  where MR = 1.25 for DMU_AHRS
//
const double scaleMag = 0.000057220459;
//
// temperature (degC) = 44.4 * ((data * 5/4096) - 1.375)
//
int Crossbow::scaleRawAnglePacket( struct rawAnglePacket *in )
{
     _output.data.roll  = Math::degToRad( scaleRoll  * in->Roll );
     _output.data.pitch = Math::degToRad( scalePitch * in->Pitch );
     _output.data.yaw   = Math::degToRad( scaleYaw   * in->Yaw );
     
     _output.data.rollRate  = Math::degToRad(scaleAngRate * in->RollRate);
     _output.data.pitchRate = Math::degToRad(scaleAngRate * in->PitchRate);
     _output.data.yawRate   = Math::degToRad(scaleAngRate * in->YawRate);

     _output.data.accelX = scaleAccel * in->AccelX;
     _output.data.accelY = scaleAccel * in->AccelY;
     _output.data.accelZ = scaleAccel * in->AccelZ;

     _output.data.magX = scaleMag * in->MagX;
     _output.data.magY = scaleMag * in->MagY;
     _output.data.magZ = scaleMag * in->MagZ;

     _output.data.temperature = 44.4 * ((in->Temp * 0.00122070313) - 1.375);

     return OK;
}