/** \file
 *
 *  Contains the AHRS_sp3003D class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

//// AHRS_M2 DATA-STREAM CONFIGURATION ////
//
// Stop data-stream (if on):
//      chan0TriggerDivisor 0 set drop
//
// Configure rotation matrix for LRAUV (flat-rib back, connector down):
//      boresightMatrix m[ decimal 0 0 2 2 f0.0 f0.0 f-1.0 f0.0 f-1.0 f0.0 f-1.0 f0.0 f0.0 ]m set drop
//
// Configure the data-stream:
//      chan0Format 2 set drop
//      chan0Trigger 5 set drop
//      chan0Enables array[ 0 15 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]array set drop
//      chan0EnableBit pitch dvid@ set drop
//      chan0EnableBit roll dvid@ set drop
//      chan0EnableBit yaw dvid@ set drop
//      chan0EnableBit magp dvid@ set drop
//      chan0EnableBit accelp dvid@ set drop
//      chan0EnableBit gyrop dvid@ set drop
//      chan0EnableBit yawErrEst dvid@ set drop
//      chan0EnableBit temperature dvid@ set drop
//      chan0EnableBit magBufferActiveIndex dvid@ set drop
//
//// * ////

#include "AHRS_M2.h"
#include "AHRS_M2IF.h"

#include <string>       // std::string
#include <iostream>     // std::cout
#include <sstream>      // std::stringstream

#include <cstdlib>
#include <unistd.h>  // include for the sleep method

#include "data/ConfigReader.h"
#include "data/Matrix3x3.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "utils/MagneticVariation.h"


// Primary compass
#define AHRS_ACCURACY (0.003490)       // pi/180 for 0.2 degree accuracy. See spec
#define PITCH_ROLL_ACCURACY (0.003490) // <.2 degree accuracy. See spec
#define GRAVITY_MILLI_G (0.00980665) // equal to one milli-G
#define MAGNETIC_FLUX_MILLIGAUSS ( 0.1 ) // equal to one Microtesla [uT]

const float AHRS_M2::HEADING_LIMIT[] = { -M_2PI, M_2PI };
const float AHRS_M2::PITCH_LIMIT[]   = { -M_PI, M_PI };
const float AHRS_M2::ROLL_LIMIT[]    = { -M_PI, M_PI };

const unsigned short AHRS_M2::MAX_DEVICE_MSG_QUEUE_SIZE( 5 );

AHRS_M2* AHRS_M2::Instance_( NULL );


AHRS_M2::AHRS_M2( const Module* module )
    : SyncSensorComponent( AHRS_M2IF::NAME, module ),
      loadControl_( AHRS_M2IF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      uart_( AHRS_M2IF::UART, AHRS_M2IF::BAUD, 0.3, logger_ ),
      calMode_( CAL_OFF ),
      calModeRequest_( CAL_OFF ),
      calSequance_( NUM_POINT_CAL ),
      calibrationFailure_( false ),
      dataSetup_( BORESIGHT ),
      dataStreamConfigured_( false ),
      dataStreamActive_( false ),
      pitch_( 0.0 ),
      roll_( 0.0 ),
      compHeading_( 0.0 ),
      magHeading_( 0.0 ),
      trueHeading_( 0.0 ),
      accel_( 0.0 ),
      gyro_( 0.0 ),
      mag_( 0.0 ),
      yawErrEst_( 0.0 ),
      temperature_( 0.0 ),
      numPointsCal_( 0 ),
      rotationFromVehicleToNavigationFrame_( 0.0 ),
      rotationFromDeviceToVehicleFrame_( 0.0 ),
      magDeviation_( 0.0 ),
      magVariation_( 0.0 ),
      poTimeout_( 3.0 ), // spec states a few sec.
      timeout_( 10.0 ), // Valid data from the unit should be expected within this time (sec). Long timeout accounts for startup config.
      startTime_( Timestamp::NOT_SET_TIME ),
      poTime_( Timestamp::NOT_SET_TIME ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      readAccelerationsCfg_( false ),
      readAngularVelocitiesCfg_( false ),
      readMagneticsCfg_( false ),
      numPointsCalCfg_( 4 ),
      debug_( false ),
      verbosity_( 0 )
{
    // Slate inputs
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    // Universal Slate outputs
    magneticHeadingWriter_ = newUniversalWriter( UniversalURI::PLATFORM_MAGNETIC_ORIENTATION, Units::RADIAN, AHRS_ACCURACY );
    pitchWriter_           = newUniversalWriter( UniversalURI::PLATFORM_PITCH_ANGLE, Units::RADIAN, PITCH_ROLL_ACCURACY );
    rollWriter_            = newUniversalWriter( UniversalURI::PLATFORM_ROLL_ANGLE, Units::RADIAN, PITCH_ROLL_ACCURACY );
    trueHeadingWriter_     = newUniversalWriter( UniversalURI::PLATFORM_ORIENTATION, Units::RADIAN, AHRS_ACCURACY );

    Point3D accuracyVector( AHRS_ACCURACY, PITCH_ROLL_ACCURACY, PITCH_ROLL_ACCURACY );
    rotationMatrixWriter_ = newUniversalBlobWriter( UniversalURI::PLATFORM_ORIENTATION_MATRIX, Units::NONE, accuracyVector.getMagnitude() );

    // Slate outputs
    compassCalStateWriter_    = newDataWriter( AHRS_M2IF::COMPASS_CAL_STATE );
    compassHeadingErrWriter_  = newDataWriter( AHRS_M2IF::COMPASS_ORIENTATION_ERR );
    compassHeadingWriter_     = newDataWriter( AHRS_M2IF::COMPASS_ORIENTATION );
    compassTemperatureWriter_ = newDataWriter( AHRS_M2IF::COMPASS_TEMPERATURE );

    accelWriter_ = newBlobWriter( AHRS_M2IF::ACCELEROMETER_READAING );
    gyroWriter_  = newBlobWriter( AHRS_M2IF::GYRO_READAING );
    magWriter_   = newBlobWriter( AHRS_M2IF::MAGNETOMETER_READAING );

    numPointCalWriter_ = newDataWriter( AHRS_M2IF::NUM_POINTS_CAL );

    // Configuration inputs
    boresightMatrixCfgReader_       = newConfigReader( AHRS_M2IF::BORESIGHT_MATRIX_CFG );
    magDeviationCfgReader_          = newConfigReader( AHRS_M2IF::MAG_DEVIATION_CFG );
    numPointsCalCfgReader_          = newConfigReader( AHRS_M2IF::NUM_POINTS_CAL_CFG );
    readAccelerationsCfgReader_     = newConfigReader( AHRS_M2IF::READ_ACCELERATIONS_CFG );
    readAngularVelocitiesCfgReader_ = newConfigReader( AHRS_M2IF::READ_ANGULAR_VELOCITIES_CFG );
    readMagneticsCfgReader_         = newConfigReader( AHRS_M2IF::READ_MAGNETICS_CFG );
    verbosityCfgReader_             = newConfigReader( AHRS_M2IF::VERBOSITY_CFG );

    Instance_ = this;

    // This configures the advanced run modes.
    setRunState( START );
}


AHRS_M2::~AHRS_M2()
{
    Instance_ = NULL;
}

void AHRS_M2::run()
{
}


/// Similar to initialize, in old init/run/uninit sequence
Component::RunState AHRS_M2::start()
{
    logger_.syslog( "Initializing AHRS_M2." );
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

    if( !simulateHardware() )
    {
        deviceResponse_[0] = '\0';
        this->setAllowableFailures( 5 );
        this->setRetryTimeout( 300 );
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "AHRS_M2 load controller failed to power up.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return START;
        }

        // Open the uart
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error opening port: ", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
    }

    // Init magnetic variation with location from the configuration
    float latitude, longitude;
    if( !Slate::ReadOnce( "Config/workSite", "initLat", Units::RADIAN, latitude, logger_ ) ) logger_.syslog( "Failed to read workSite initLat", Syslog::FAULT );
    if( !Slate::ReadOnce( "Config/workSite", "initLon", Units::RADIAN, longitude, logger_ ) ) logger_.syslog( "Failed to read workSite initLon", Syslog::FAULT );
    magVariation_ = MagneticVariation::Radian( Timestamp::Now(), latitude, longitude );

    // Init and log calibration variables
    calMode_ = CAL_OFF;
    calModeRequest_ = CAL_OFF;
    compassCalStateWriter_->write( Units::COUNT, calMode_, Timestamp::Now() );

    startTime_ = Timestamp::Now();
    poTime_ = Timestamp::Now();
    return STARTING;
}


/// Might follow a STOP...START sequence
Component::RunState AHRS_M2::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    // Wait until PO reset has occurred
    if( poTime_.elapsed() < poTimeout_ )
    {
        startTime_ = Timestamp::Now();
        return STARTING;
    }

    if( readConfig()
            && configureDataStream()
            && startDataStream() ) // Note: simulated hardware case is handled by tryUartComms
    {
        startTime_ = Timestamp::Now();
        return RUNNABLE;
    }

    if( startTime_.elapsed() > timeout_ )
    {
        logger_.syslog( "Failed to initialize within timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    return STARTING;
}

/// Pause for a short period
Component::RunState AHRS_M2::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    if( !simulateHardware() )
    {
        if( !loadControl_.powerDown() ) // NOTE: old Sparton sp3003D required isolateLoad()
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        uart_.close();
    }
    return PAUSED;
}

/// Should eventually follow a PAUSE request
Component::RunState AHRS_M2::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isDataRequested() )
    {
        return resume();
    }
    return PAUSED;
}

Component::RunState AHRS_M2::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    if( !simulateHardware() )
    {
        // Open the uart
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error opening port: ", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }

        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }
    startTime_ = Timestamp::Now();
    return RESUMING;
}


Component::RunState AHRS_M2::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );

    // Wait until PO reset has occurred
    if( startTime_.elapsed() < poTimeout_ )
    {
        return RESUMING;
    }

    if( readConfig() && startDataStream() )
    {
        startTime_ = Timestamp::Now();
        return RUNNABLE;
    }

    if( startTime_.elapsed() > timeout_ )
    {
        logger_.syslog( Str( "Failed to resume within timeout." ), Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    return RESUMING;
}

Component::RunState AHRS_M2::runnable()
{
    //if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );

    readConfig();

    if( handleCalRequest() )
    {
        if( calibrationFailure_ )
        {
            calSequance_ = NUM_POINT_CAL;
            calibrationFailure_ = false;
            return STOP;
        }
        return RUNNABLE;
    }

    if( !simulateHardware() )
    {
        // Log voltage and current and check for any faults
        loadControl_.requestVoltageAndCurrent();
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }

    // Compute magnetic variation
    getMagneticVariation();

    bool validData( false );
    if( dataStreamActive_ && receiveRFSData() )
    {
        // Data is valid if primary values are in range
        validData = ( AuvMath::InLimit( compHeading_, HEADING_LIMIT[ 0 ], HEADING_LIMIT[ 1 ] )
                      || AuvMath::InLimit( pitch_, PITCH_LIMIT[ 0 ], PITCH_LIMIT[ 1 ] )
                      || AuvMath::InLimit( roll_, ROLL_LIMIT[ 0 ], ROLL_LIMIT[ 1 ] ) );

        setWritersInvalid( !validData ); // The writers are invalid if primary values are out of range
        processData();
        writeData();

        if( validData )
        {
            startTime_ = Timestamp::Now(); // Good data, reset the clock
            this->resetFailCount(); // and the fail count
        }
    }

    // Check for recent valid data
    if( startTime_.elapsed() > timeout_ )
    {
        logger_.syslog( "Failed to acquire valid data within timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::DATA );
        return STOP;
    }

    // Pause if we don't want data
    if( !isDataRequested() )
    {
        return PAUSE;
    }

    return RUNNABLE;
}

Component::RunState AHRS_M2::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );
    uninitialize(); // First power down then query for faults next cycle
    startTime_ = Timestamp::Now();
    return STOPPING;
}

Component::RunState AHRS_M2::stopping()
{
    if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO );
    if( !simulateHardware() )
    {
        loadControl_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }

    }

    return STOPPED;
}

Component::RunState AHRS_M2::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );
    if( isDataRequested() )
    {
        return START;
    }
    if( !simulateHardware() )
    {
        if( ( loadControl_.getPowerState() != LoadControl::OFF )
                && ( loadControl_.getPowerState() != LoadControl::POWER_DOWN ) )
        {
            return stop();
        }

        // Close if the uart if it is open
        if( uart_.isReadable() )
        {
            uart_.close();
        }
    }
    return STOPPED;
}


void AHRS_M2::uninitialize()
{
    if( debug_ ) logger_.syslog( "Uninitializing.", Syslog::INFO );

    if( !simulateHardware() )
    {
        // stopDataStream(); // NOTE: uncomment when debugging using NDS Dev-Board
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        uart_.close();
    }
}


// Sets calibration state and commands the Sparton
bool AHRS_M2::SetCalMode( CalibrateSpartonMode calMode )
{
    if( NULL == Instance_ )
    {
        return false;
    }
    return Instance_->setCalMode( calMode );
}


/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI AHRS_M2::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        return AHRS_M2IF::POWER;
    case CONFIG_SIMULATE_HARDWARE:
        return AHRS_M2IF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}


bool AHRS_M2::isDataRequested( void )
{
    // lcmSlateWriter_->isDataRequested()
    return magneticHeadingWriter_->isDataRequested()
           || trueHeadingWriter_->isDataRequested()
           || pitchWriter_->isDataRequested()
           || rollWriter_->isDataRequested()
           || rotationMatrixWriter_->isDataRequested()
           || compassHeadingWriter_->isDataRequested()
           || compassHeadingErrWriter_->isDataRequested()
           || compassTemperatureWriter_->isDataRequested()
           || magWriter_->isDataRequested()
           || accelWriter_->isDataRequested()
           || gyroWriter_->isDataRequested()
           || numPointCalWriter_->isDataRequested();
}


void AHRS_M2::writeData( void )
{
    magneticHeadingWriter_->write( Units::RADIAN, magHeading_, dataTimestamp_ );
    trueHeadingWriter_->write( Units::RADIAN, trueHeading_, dataTimestamp_ );
    pitchWriter_->write( Units::RADIAN, pitch_, dataTimestamp_ );
    rollWriter_->write( Units::RADIAN, roll_, dataTimestamp_ );
    rotationMatrixWriter_->write2DClass( Units::NONE, rotationFromVehicleToNavigationFrame_, dataTimestamp_ );
    compassHeadingWriter_->write( Units::RADIAN, compHeading_, dataTimestamp_ );
    compassHeadingErrWriter_->write( Units::RADIAN, yawErrEst_, dataTimestamp_ );
    compassTemperatureWriter_->write( Units::CELSIUS, temperature_, dataTimestamp_ );

    if( readMagneticsCfg_ )
    {
        magWriter_->write1DClass( Units::MICROTESLA, mag_, dataTimestamp_ );
    }

    if( readAccelerationsCfg_ )
    {
        accelWriter_->write1DClass( Units::METER_PER_SECOND_SQUARED, accel_, dataTimestamp_ );
    }

    if( readAngularVelocitiesCfg_ )
    {
        gyroWriter_->write1DClass( Units::RADIAN_PER_SECOND, gyro_, dataTimestamp_ );
    }

    if( calMode_ != CAL_OFF )
    {
        numPointCalWriter_->write( Units::COUNT, numPointsCal_, dataTimestamp_ );
    }

    if( verbosity_ > 3 )
        logger_.syslog( "rotation FSK->NED: " + rotationFromVehicleToNavigationFrame_.toString(), Syslog::INFO );
}


void AHRS_M2::setWritersInvalid( bool invalid )
{
    magneticHeadingWriter_->setInvalid( invalid );
    trueHeadingWriter_->setInvalid( invalid );
    pitchWriter_->setInvalid( invalid );
    rollWriter_->setInvalid( invalid );
    rotationMatrixWriter_->setInvalid( invalid );
}


bool AHRS_M2::tryUartComms( const char* textToSend, const char* errorPrefix,
                            Syslog::Severity severity )
{
    if( !simulateHardware() )
    {
        uart_.flush();
        uart_ << textToSend;
        uart_.readLine( deviceResponse_, sizeof( deviceResponse_ ) );
        // Check response
        if( uart_.hasError() )
        {
            logger_.syslog( errorPrefix, uart_.errorString(), severity );
        }
        return !uart_.hasError();
    }
    else
    {
        logger_.syslog( textToSend, ( verbosity_ > 0 ) ? Syslog::INFO : Syslog::DEBUG );
        startTime_ = Timestamp::Now();
        return true;
    }
}


bool AHRS_M2::readConfig( void )
{
    bool ok( true );
    ok &= magDeviationCfgReader_->read( Units::RADIAN, magDeviation_ );
    ok &= numPointsCalCfgReader_->read( Units::COUNT, numPointsCalCfg_ );
    ok &= readAccelerationsCfgReader_->read( Units::BOOL, readAccelerationsCfg_ );
    ok &= readAngularVelocitiesCfgReader_->read( Units::BOOL, readAngularVelocitiesCfg_ );
    ok &= readMagneticsCfgReader_->read( Units::BOOL, readMagneticsCfg_ );
    ok &= verbosityCfgReader_->read( Units::COUNT, verbosity_ );

    return ok;
}

// Configures the Sparton data-stream to output desired variables
bool AHRS_M2::configureDataStream( void )
{

    if( dataStreamConfigured_ ) return true;

    switch( dataSetup_ )
    {
    case BORESIGHT:
        if( setBoresightMatrix() )
            dataSetup_ = FORMAT;
        break;
    case FORMAT:
        // Format BitStream (2 = BitStreamBinary)
        if( tryUartComms( "chan0Format 2 set drop\r\n",
                          "Format BitStream UART error: " ) )
            dataSetup_ = TRIGGER;
        break;
    case TRIGGER:
        // Set Trigger to CompassData
        if( tryUartComms( "chan0Trigger 5 set drop\r\n",
                          "Set Trigger UART error: " ) )
            dataSetup_ = CLEAR;
        break;
    case CLEAR:
        // Clear any previous data selections for channel
        if( tryUartComms( "chan0Enables array[ 0 15 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]array set drop\r\n"
                          , "Clear channel UART error: " ) )
            dataSetup_ = PITCH;
    case PITCH:
        // Subscribe to Pitch data-stream
        if( tryUartComms( "chan0EnableBit pitch dvid@ set drop\r\n",
                          "Subscribe Pitch UART error: " ) )
            dataSetup_ = ROLL;
        break;
    case ROLL:
        // Subscribe to Roll data-stream
        if( tryUartComms( "chan0EnableBit roll dvid@ set drop\r\n",
                          "Subscribe Roll UART error: " ) )
            dataSetup_ = YAW;
        break;
    case YAW:
        // Subscribe to Yaw data-stream
        if( tryUartComms( "chan0EnableBit yaw dvid@ set drop\r\n",
                          "Subscribe Yaw UART error: " ) )
            dataSetup_ = MAG;
        break;
    case MAG:
        // Subscribe to Magnetics data-stream
        if( tryUartComms( "chan0EnableBit magp dvid@ set drop\r\n",
                          "Subscribe Magnetometer UART error: " ) )
            dataSetup_ = ACCEL;
        break;
    case ACCEL:
        // Subscribe to Accelerations data-stream
        if( tryUartComms( "chan0EnableBit accelp dvid@ set drop\r\n",
                          "Subscribe Accelerations UART error: " ) )
            dataSetup_ = GYRO;
        break;
    case GYRO:
        // Subscribe to Accelerations data-stream
        if( tryUartComms( "chan0EnableBit gyrop dvid@ set drop\r\n",
                          "Subscribe Gyro UART error: " ) )
            dataSetup_ = YAWERR;
        break;
    case YAWERR:
        // Subscribe to Yaw data-stream
        if( tryUartComms( "chan0EnableBit yawErrEst dvid@ set drop\r\n",
                          "Subscribe yawErrEst UART error: " ) )
            dataSetup_ = TEMPERATURE;
        break;
    case TEMPERATURE:
        // Subscribe to Temperature data-stream
        if( tryUartComms( "chan0EnableBit temperature dvid@ set drop\r\n",
                          "Subscribe Temperature UART error: " ) )
            dataSetup_ = NUMPOINTCAL;
        break;
    case NUMPOINTCAL:
        // Calibratin mode: subscribe to MagBufferActiveIndex data-stream (num. of cal points acquired)
        if( tryUartComms( "chan0EnableBit magBufferActiveIndex dvid@ set drop\r\n",
                          "Subscribe MagBufferActiveIndex UART error: " ) )
            dataSetup_ = SETUPDONE;
        break;
    case SETUPDONE:
        dataStreamConfigured_ = true;
        dataSetup_ = BORESIGHT;
        return true;
        break;
    default:
        dataSetup_ = BORESIGHT;
        break;
    }

    return false;
}


// Configurs the AHRS-M2 to match LRAUV mounting orientation
bool AHRS_M2::setBoresightMatrix( void )
{
    bool ok( true );
    boresightMatrixCfg_ = "";
    ok &= boresightMatrixCfgReader_->read( boresightMatrixCfg_ );

    if( ok )
    {
        // Extract 3x3 rotation matrix values
        double *r = rotationFromDeviceToVehicleFrame_.getPtr1d();
        if( 9 != sscanf( boresightMatrixCfg_.cStr(), "f%lf f%lf f%lf f%lf f%lf f%lf f%lf f%lf f%lf",
                         &r[0], &r[1], &r[2], &r[3], &r[4], &r[5], &r[6], &r[7], &r[8] ) )
        {
            logger_.syslog( "Failed to parse boresight matrix values.", Syslog::CRITICAL );
        }

        if( verbosity_ > 0 ) logger_.syslog( "rotationFromDeviceToVehicleFrame: " + rotationFromDeviceToVehicleFrame_.toString(), Syslog::INFO );

        boresightMatrixCfg_ = "boresightMatrix m[ decimal 0 0 2 2 " + boresightMatrixCfg_ + " ]m set drop\r\n";
        ok &= tryUartComms( boresightMatrixCfg_.cStr(), "setBoresightMatrix UART error: " );
    }

    return ok;
}


bool AHRS_M2::startDataStream( void )
{
    if( tryUartComms( "chan0TriggerDivisor 40 set drop\r\n",  // 100Hz/40=2.5Hz
                      "Start data-stream UART error: " ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Data-stream active.", Syslog::INFO );
        dataStreamActive_ = true;
        return true;
    }
    return false;
}


bool AHRS_M2::stopDataStream( void )
{
    if( tryUartComms( "chan0TriggerDivisor 0 set drop\r\n",
                      "Stop data-stream UART error: " ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Data-stream disabled.", Syslog::INFO );
        dataStreamActive_ = false;
        setWritersInvalid( true );
        return true;
    }
    return false;
}


void AHRS_M2::getMagneticVariation( void )
{
    if( latitudeReader_->isActive() && longitudeReader_->isActive() )
    {
        float latitude, longitude;
        if( latitudeReader_->read( Units::RADIAN, latitude ) && latitude == latitude
                && longitudeReader_->read( Units::RADIAN, longitude ) && longitude == longitude )
        {
            magVariation_ = MagneticVariation::Radian( Timestamp::Now(), latitude, longitude );
        }
    }
}


bool AHRS_M2::setCalMode( CalibrateSpartonMode calMode )
{
    if( calMode_ == calMode )
    {
        return true;
    }
    else
    {
        calModeRequest_ = calMode;
    }

    return false;
}


// Return true if handling a calibration request. False otherwise.
bool AHRS_M2::handleCalRequest( void )
{
    if( calMode_ != calModeRequest_ )
    {
        if( dataStreamActive_ ) stopDataStream();

        bool success( false );
        switch( calSequance_ )
        {
        case NUM_POINT_CAL:
            char cmdString[30];
            sprintf( cmdString, "minCalPoints %d set drop\r\n", numPointsCalCfg_ );

            success = tryUartComms( cmdString, "minCalPoints UART error: " );
            if( success )
            {
                if( verbosity_ > 0 ) logger_.syslog( Str( "Set minCalPoints to " ) + Str( numPointsCalCfg_ ) + Str( "." ), Syslog::INFO );
                calSequance_ = CLEAR_POINT_CAL;
            }
            break;

        case CLEAR_POINT_CAL:
            success = tryUartComms( "clearPointCal 1 set drop\r\n", "clearPointCal UART error: " );
            if( success )
            {
                if( verbosity_ > 0 ) logger_.syslog( "Set clearPointCal.", Syslog::INFO );
                calSequance_ = INIT_POINT_CAL;
            }
            break;

        case INIT_POINT_CAL:
            success = tryUartComms( "initCalPointBuffer 1 set drop\r\n", "initCalPointBuffer UART error: " );
            if( success )
            {
                if( verbosity_ > 0 ) logger_.syslog( "Set initCalPointBuffer.", Syslog::INFO );
                calSequance_ = ACTIVATE_CAL;
            }
            break;

        case ACTIVATE_CAL:
            if( tryUartComms( "autoFieldCalActive 1 set drop\r\n", "autoFieldCalActive ON UART error: " ) )
            {
                if( verbosity_ > 0 ) logger_.syslog( "Set autoFieldCalActive.", Syslog::INFO );
                calSequance_ = REPORT_NUM_POINT_CAL;
                calMode_ = CAL_AUTO;
                compassCalStateWriter_->write( Units::COUNT, calMode_, Timestamp::Now() );
                startDataStream(); // Reactivate the data-stream
                success = true;
            }
            break;

        case REPORT_NUM_POINT_CAL:
            success = tryUartComms( "$PSRFS,magBufferActiveIndex,get\r\n", "Read magBufferActiveIndex UART error: " );
            if( success )
            {
                if( !simulateHardware() )
                {
                    int checksum = 0xFF;
                    int calBufferActive;
                    if( 2 == sscanf( deviceResponse_, "$PSRFS,magBufferActiveIndex,%d*%02X", &calBufferActive, &checksum ) )
                    {
                        logger_.syslog( "Acquired " + Str( calBufferActive ) + " calibration points.", Syslog::IMPORTANT );
                    }
                    else
                    {
                        logger_.syslog( "Number of acquired calibration points is unknown. Response was: " + Str( deviceResponse_, uart_.bytesRead() ), Syslog::IMPORTANT );
                    }
                }
                calSequance_ = DEACTIVATE_CAL;
            }
            break;

        case DEACTIVATE_CAL:
            success = tryUartComms( "autoFieldCalActive 0 set drop\r\n", "autoFieldCalActive OFF UART error: " );
            if( success )
            {
                if( verbosity_ > 0 ) logger_.syslog( "Un-set autoFieldCalActive.", Syslog::INFO );
                calSequance_ = REPORT_ERR_CAL;
            }
            break;

        case REPORT_ERR_CAL:
            if( tryUartComms( "$PSRFS,magFieldCalErr,get\r\n", "Read MagFieldCalErr UART error: " ) )
            {
                if( !simulateHardware() )
                {
                    int checksum = 0xFF;
                    float calErr;
                    if( 2 == sscanf( deviceResponse_, "$PSRFS,magFieldCalErr,%f*%02X", &calErr, &checksum ) )
                    {
                        logger_.syslog( "Magnetic calibration quality (0[best] to 10000) is " + Str( calErr ) + ".", Syslog::IMPORTANT );
                    }
                    else
                    {
                        logger_.syslog( "Magnetic calibration quality is unknown. Response was " + Str( deviceResponse_, uart_.bytesRead() ), Syslog::IMPORTANT );
                    }
                }

                calSequance_ = NUM_POINT_CAL;
                calMode_ = CAL_OFF;
                compassCalStateWriter_->write( Units::COUNT, calMode_, Timestamp::Now() );
                startDataStream(); // Reactivate the data-stream
                success = true;
            }
            break;

        default:
            break;
        }

        // Reset timeout following a successful operation
        if( success ) startTime_ = Timestamp::Now();

        // Check for recent valid data
        if( startTime_.elapsed() > timeout_ )
        {
            logger_.syslog( "Failed to handle calibration request within timeout.", Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            calibrationFailure_ = true;
        }

        return true;
    }

    return false;
}


bool AHRS_M2::receiveRFSData( void )
{
    if( simulateHardware() )
    {
        SimAhrsStruct ahrsSimData;

        // Get the latests simulated AHRS data
        SimSlate::Read( ahrsSimData );

        if( !isnan( ahrsSimData.roll )
                && !isnan( ahrsSimData.pitch )
                && !isnan( ahrsSimData.yaw ) )
        {
            // Grab a timestamp for the data
            dataTimestamp_ = Timestamp::Now();

            roll_ = ahrsSimData.roll;
            pitch_ = ahrsSimData.pitch;
            trueHeading_ = ahrsSimData.yaw;

            // Linear accelaraions along FSK in m/s2
            accel_ = ahrsSimData.linearAcceleration;
            // Angular velocities along FSK in rad/s
            gyro_  = ahrsSimData.angularVelocity;
            // Magnetic field along FSK in milligauss
            mag_   = ahrsSimData.magneticField;

            return true;
        }
    }
    else
    {
        unsigned char response[256];
        if( readRFSPacket( response ) )
        {

            pAhrsM2Data m2Data = ( pAhrsM2Data )response;

            pitch_ = D2R( m2Data->pitch_ );
            roll_  = D2R( m2Data->roll_ );
            compHeading_ = D2R( m2Data->yaw_ );

            if( verbosity_ > 1 ) logger_.syslog( "PITCH: " + Str( R2D( pitch_ ) ) + ", ROLL: " + Str( R2D( roll_ ) ) + ", YAW: " + Str( R2D( compHeading_ ) ) + " (deg)", Syslog::INFO );

            if( readMagneticsCfg_ )
            {
                Point3D rawMag( m2Data->magX_, m2Data->magY_, m2Data->magZ_ );

                mag_ = 0;  // must set to 0 before addProduct
                mag_.addProduct( rotationFromDeviceToVehicleFrame_, rawMag );
                mag_ *= MAGNETIC_FLUX_MILLIGAUSS;

                if( verbosity_ > 1 ) logger_.syslog( "Mag:     X: " + Str( mag_.getX() ) + ", Y: " + Str( mag_.getY() ) + ", Z: " + Str( mag_.getZ() ) + " (uT)", Syslog::INFO );
            }

            if( readAccelerationsCfg_ )
            {
                Point3D rawAccel( m2Data->accelX_, m2Data->accelY_, m2Data->accelZ_ );

                accel_ = 0;  // must set to 0 before addProduct
                accel_.addProduct( rotationFromDeviceToVehicleFrame_, rawAccel );
                accel_ *= GRAVITY_MILLI_G;

                if( verbosity_ > 1 ) logger_.syslog( "Accel:   X: " + Str( accel_.getX() ) + ", Y: " + Str( accel_.getY() ) + ", Z: " + Str( accel_.getZ() ) + " (m/s2)", Syslog::INFO );
            }

            if( readAngularVelocitiesCfg_ )
            {
                Point3D rawGyro( m2Data->gyroX_, m2Data->gyroY_, m2Data->gyroZ_ );

                gyro_ = 0;  // must set to 0 before addProduct
                gyro_.addProduct( rotationFromDeviceToVehicleFrame_, rawGyro );

                if( verbosity_ > 1 ) logger_.syslog( "Ang.Vel: X: " + Str( gyro_.getX() ) + ", Y: " + Str( gyro_.getY() ) + ", Z: " + Str( gyro_.getZ() ) + " (rad/s)", Syslog::INFO );
            }

            yawErrEst_ = D2R( m2Data->yawErrEst_ );
            temperature_ = m2Data->temperature_;

            if( verbosity_ > 1 ) logger_.syslog( "YAW ERR: " + Str( R2D( yawErrEst_ ) ) + " (deg), TEMP: " + Str( temperature_ ) + " (degC)", Syslog::INFO );

            if( calMode_ != CAL_OFF )
            {
                numPointsCal_ = m2Data->numPointsCal_;

                if( verbosity_ > 1 ) logger_.syslog( "FIELD CAL ACTIVE. numPointsCal: " + Str( numPointsCal_ ), Syslog::INFO );
            }

            return true;
        }
    }

    return false;
}


bool AHRS_M2::readRFSPacket( unsigned char* response )
{
    int readCount( 0 );
    bool processPacket( false );
    unsigned int length( 0 );

    // Cycle through the data available on the serial port and get to the most recent packet
    while( uart_.dataAvailable() && ( readCount < MAX_DEVICE_MSG_QUEUE_SIZE ) )
    {
        // Look for packet termintation char ETX
        if( uart_.readUntil( deviceResponse_, MAX_DEVICE_RESPONSE, ETX ).hasError() )
        {
            if( uart_.getError() != UartStream::TIMEOUT )
            {
                logger_.syslog( "Read RFS packet UART error: ", uart_.errorString(), Syslog::ERROR );
                if( uart_.bytesRead() > 0 && verbosity_ > 0 )
                {
                    printBufferHex( "Received:", deviceResponse_, ( unsigned )uart_.bytesRead() );
                }
            }
            processPacket = false;
            break;
        }
        else
        {
            // Got device response
            if( debug_ && verbosity_ > 1 ) logger_.syslog( "Found valid termintation char ETX. Skip count: " + Str( readCount ) + ".", Syslog::ERROR );
            processPacket = readCount < MAX_DEVICE_MSG_QUEUE_SIZE;
        }

        ++readCount;
    }

    if( processPacket )
    {
        // Grab a timestamp for the data
        dataTimestamp_ = Timestamp::Now();

        if( verbosity_ > 2 ) printBufferHex( "Tx", deviceResponse_, ( unsigned int )uart_.bytesRead(), Syslog::INFO );

        // Strip the SAPP frame off incoming packet
        length = removeSAPPFrame( response, deviceResponse_, ( unsigned int )uart_.bytesRead() );

        if( verbosity_ > 2 ) printBufferHex( "Tx post-dle", ( const char * )response, length, Syslog::INFO );

        if( length >= MIN_DEVICE_RESPONSE ) // TODO: add up MIN_DEVICE_RESPONSE when subscribing to data-streams
        {
            // Grab the CRC and check it against our calculation
            unsigned short crc = getCRC( &response[1], length - 3 ); // CRC data response (skip 1st char and the CRC)
            if( crc != extractShort( &response[length - 2] ) )
            {
                if( verbosity_ > 1 ) logger_.syslog( "CRC does not match. Expected: " + Str( crc ) + " got: " + Str( extractShort( &response[length - 2] ) ), Syslog::ERROR );
            }
            else
            {
                // CRC match!
                return true;
            }
        }
    }

    // Flush the buffer in case we're out of sync with the device
    uart_.flush();
    return false;
}


void AHRS_M2::processData( void )
{
    if( simulateHardware() )
    {
        // Remove variation and deviation
        magHeading_ = AuvMath::NormalizeAngle( trueHeading_ - magVariation_ );
        compHeading_ = AuvMath::NormalizeAngle( magHeading_ - magDeviation_ );
    }
    else
    {
        // Apply variation and deviation
        magHeading_ = AuvMath::NormalizeAngle( compHeading_ + magDeviation_ );
        trueHeading_ = AuvMath::NormalizeAngle( magHeading_ + magVariation_ );
    }

    // make the orientation matrix
    Point6D zzzrph( 0, 0, 0, roll_, pitch_, AuvMath::ModPi( trueHeading_ ) );
    rotationFromVehicleToNavigationFrame_ = Matrix3x3( zzzrph );

}


unsigned int AHRS_M2::removeSAPPFrame( unsigned char *output, const char *input, unsigned int length )
{
    unsigned char *origin = output;

    // skip any non SOH at the front (SYN?)
    while( static_cast<void>( length-- ), *input++ != SOH )
    {
        if( length <= 0 )
        {
            if( debug_ ) logger_.syslog( "SAPP frame error: SOH not found.", Syslog::ERROR );
            return( 0 );
        }
    }

    // now reduce all DLE'd pairs
    while( ( length-- > 0 ) && ( *input != ETX ) )
    {
        if( *input == DLE )
        {
            length--;
            input++;
            *output++ = *input++ & MASK_OFF;
        }
        else
        {
            *output++ = *input++;
        }
    }
    // if packet is not well formed
    if( *input != ETX )
    {
        if( debug_ ) logger_.syslog( "SAPP frame error: ETX not found.", Syslog::ERROR );
        return( 0 );
    }
    return( ( unsigned int )( output - origin ) );

}


unsigned char AHRS_M2::isControl( unsigned char value )
{
    // short circuit most characters
    if( value > SYN )
    {
        return( 0 );
    }
    switch( value )
    {
    case SYN:
    case SOH:
    case DLE:
    case ETX:
    case ACK:
    case NAK:
        return( 1 );
    default :
        return( 0 );
    }
}


unsigned short AHRS_M2::getCRC( const unsigned char* data, unsigned int len )
{
    unsigned char * dataPtr = ( unsigned char * )data;
    unsigned int index = 0;
    unsigned short crc = 0xFFFF;
    while( len-- )
    {
        crc = ( unsigned char )( crc >> 8 ) | ( crc << 8 );
        crc ^= dataPtr[index++];
        crc ^= ( unsigned char )( crc & 0xff ) >> 4;
        crc ^= ( crc << 8 ) << 4;
        crc ^= ( ( crc & 0xff ) << 4 ) << 1;
    }
    return crc;

}


// Converts 2 bytes to short
unsigned short AHRS_M2::extractShort( unsigned char *convertFloat, bool flipEndian )
{
    unsigned short retVal;
    char *retFloat = ( char* ) & retVal;

    if( flipEndian )
    {
        // flip endian
        retFloat[0] = convertFloat[1];
        retFloat[1] = convertFloat[0];
    }
    else
    {
        retFloat[0] = convertFloat[0];
        retFloat[1] = convertFloat[1];
    }

    return retVal;
}


void AHRS_M2::printBufferHex( const char * prefix, const char * buffer, unsigned int length, Syslog::Severity severity )
{
    std::stringstream ss;
    ss << prefix << " (" << length << "):";
    for( unsigned int i = 0; i < length; ++i )
        ss << " 0x" << std::hex << ( int )buffer[i];
    std::string str = ss.str();
    const char *cstr = str.c_str();
    logger_.syslog( Str( cstr ), severity );
}