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

#include "AHRS_sp3003D.h"
#include "AHRS_sp3003DIF.h"

#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.017453)      // pi/180 for 1 degree accuracy. See spec
#define TEMP_ACCURACY (3.0)           // Temperature accurate to +/- 3 degree C. See spec
#define PITCH_ROLL_ACCURACY (.003490) // <.2 degree accuracy. See spec
#define GRAVITY_MILLI_G (.00980665)   // equal to one milli-G

AHRS_sp3003D* AHRS_sp3003D::Instance_( NULL );

AHRS_sp3003D::AHRS_sp3003D( const Module* module )
    : SyncSensorComponent( AHRS_sp3003DIF::NAME, module ),
      calMode_( CAL_OFF ),
      calibrationFailure_( false ),
      debug_( false ),
      configSet_( false ),
      verbosity_( 0 ),
      loadControl_( AHRS_sp3003DIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      usingSimWarned_( false ),
      magDeviation_( 0.0 ),
      magVariation_( 0.0 ),
      pitchOffset_( 0.0 ),
      rollOffset_( 0.0 ),
      poTimeout_( 0.8 ), // spec states 600 msec. Adding some margin
      startTime_( Timestamp::NOT_SET_TIME ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      readAccelerationsCfg_( false ),
      readMagneticsCfg_( false ),
      readMountingCfg_( false ),
      uart_( AHRS_sp3003DIF::UART, AHRS_sp3003DIF::BAUD, 0.3, logger_ )

{

    // Slate inputs
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    // Slate outputs
    compassHeadingWriter_ = newDataWriter( AHRS_sp3003DIF::COMPASS_ORIENTATION_READING );
    compassTemperatureWriter_ = newDataWriter( AHRS_sp3003DIF::COMPASS_TEMPERATURE_READING );

    mxWriter_ = newDataWriter( AHRS_sp3003DIF::MX_READING );
    myWriter_ = newDataWriter( AHRS_sp3003DIF::MY_READING );
    mzWriter_ = newDataWriter( AHRS_sp3003DIF::MZ_READING );
    mtWriter_ = newDataWriter( AHRS_sp3003DIF::MT_READING );

    axWriter_ = newDataWriter( AHRS_sp3003DIF::AX_READING );
    ayWriter_ = newDataWriter( AHRS_sp3003DIF::AY_READING );
    azWriter_ = newDataWriter( AHRS_sp3003DIF::AZ_READING );
    atWriter_ = newDataWriter( AHRS_sp3003DIF::AT_READING );

    magneticHeadingWriter_ = newUniversalWriter( UniversalURI::PLATFORM_MAGNETIC_ORIENTATION, Units::DEGREE, AHRS_ACCURACY );
    trueHeadingWriter_ = newUniversalWriter( UniversalURI::PLATFORM_ORIENTATION, Units::DEGREE, AHRS_ACCURACY );
    pitchWriter_ = newUniversalWriter( UniversalURI::PLATFORM_PITCH_ANGLE, Units::DEGREE, PITCH_ROLL_ACCURACY );
    rollWriter_ = newUniversalWriter( UniversalURI::PLATFORM_ROLL_ANGLE, Units::DEGREE, PITCH_ROLL_ACCURACY );
    Point3D accuracyVector( AHRS_ACCURACY, PITCH_ROLL_ACCURACY, PITCH_ROLL_ACCURACY );
    rotationMatrixWriter_ = newUniversalBlobWriter( UniversalURI::PLATFORM_ORIENTATION_MATRIX, Units::NONE, accuracyVector.getMagnitude() );

    magDeviationCfgReader_ = newConfigReader( AHRS_sp3003DIF::MAG_DEVIATION_CFG );
    pitchOffsetCfgReader_ = newConfigReader( AHRS_sp3003DIF::PITCH_OFFSET_CFG );
    rollOffsetCfgReader_ = newConfigReader( AHRS_sp3003DIF::ROLL_OFFSET_CFG );
    readAccelerationsCfgReader_ = newConfigReader( AHRS_sp3003DIF::READ_ACCELERATIONS_CFG );
    readMagneticsCfgReader_ = newConfigReader( AHRS_sp3003DIF::READ_MAGNETICS_CFG );
    readMountingCfgReader_ = newConfigReader( AHRS_sp3003DIF::READ_MOUNTING_CFG );


    Instance_ = this;

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


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

void AHRS_sp3003D::run()
{
}

void AHRS_sp3003D::readConfig()
{
    magDeviationCfgReader_->read( Units::RADIAN, magDeviation_ );
    pitchOffsetCfgReader_->read( Units::RADIAN, pitchOffset_ );
    rollOffsetCfgReader_->read( Units::RADIAN, rollOffset_ );
    readAccelerationsCfgReader_->read( Units::BOOL, readAccelerationsCfg_ );
    readMagneticsCfgReader_->read( Units::BOOL, readMagneticsCfg_ );
    readMountingCfgReader_->read( Units::BOOL, readMountingCfg_ );
}

void AHRS_sp3003D::uninitialize()
{
    if( debug_ ) logger_.syslog( "uninitialize", Syslog::INFO );
    if( !simulateHardware() )
    {
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.isolateLoad() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        uart_.close();
    }
}

bool AHRS_sp3003D::SetCalMode( CalibrateSpartonMode calMode )
{
    if( NULL == Instance_ )
    {
        return false;
    }
    return Instance_->setCalMode( calMode );
}

bool AHRS_sp3003D::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, Syslog::INFO );
        return true;
    }
}

// Returns magnetic heading or nan if not available/error
float AHRS_sp3003D::getCompassHdg()
{
    float retVal = nanf( "" );

    enum
    {
        InLabel,
        InHdg,
        Finished
    };

    if( !simulateHardware() )
    {
        int state = InLabel;
        uart_.setStartTimeout( 0.75 );
        bool commsOk = tryUartComms( "$xxHDM\r\n", "getCompassHdg uart error " );
        uart_.resetStartTimeout();

        // Check and parse response
        if( commsOk )
        {
            // Checksum is at the end
            char *endPtr = strchr( deviceResponse_, '*' );
            //char *checkSumPtr;

            if( endPtr )
            {
                //checkSumPtr = endPtr;
            }
            else
            {
                // No checksum on this sentence.
                //checkSumPtr = 0;
                endPtr = ( char * )deviceResponse_ + strlen( deviceResponse_ );
            }

            const size_t bufSize( 256 );
            char buf[bufSize];
            unsigned int nBytes = MIN( ( unsigned int )( endPtr - deviceResponse_ ), bufSize - 1 );
            strncpy( buf, deviceResponse_, nBytes );
            buf[nBytes] = '\0';
            char *token = ( char * )buf;

            // Now, parse the heading string
            while( state != Finished && NULL != token )
            {
                switch( state )
                {
                case InLabel:
                    // Throw away invalid leading characters
                    token = strstr( token, "HCHDM" );
                    // Match to the expected response per interface spec
                    if( NULL != token && strncmp( token, "HCHDM", 5 ) != 0 )
                    {
                        state = Finished;
                        logger_.syslog( Str( "Invalid response from SP3003D: " + ( Str ) token ), Syslog::ERROR );
                    }
                    break;

                case InHdg:
                    retVal = atof( token );
                    break;
                }

                // Advance to next state
                state++;

                // Point to next token
                if( NULL != token )
                {
                    token = strchr( token, ',' );
                    if( NULL != token )
                    {
                        ++token;
                    }
                }
            }
        }
    }
    return retVal;
}

// Returns true heading or nan if not available/error
float AHRS_sp3003D::getTrueHdg()
{
    float retVal = nanf( "" );

    enum
    {
        InLabel,
        InHdg,
        Finished
    };

    if( !simulateHardware() )
    {

        int state = InLabel;

        // Check and parse response
        if( tryUartComms( "$xxHDT\r\n", "getTrueHdg uart error " ) )
        {
            // Checksum is at the end
            char *endPtr = strchr( deviceResponse_, '*' );
            //char *checkSumPtr;

            if( endPtr )
            {
                //checkSumPtr = endPtr;
            }
            else
            {
                // No checksum on this sentence.
                //checkSumPtr = 0;
                endPtr = ( char * )deviceResponse_ + strlen( deviceResponse_ );
            }

            const size_t bufSize( 256 );
            char buf[bufSize];
            unsigned int nBytes = MIN( ( unsigned int )( endPtr - deviceResponse_ ), bufSize - 1 );
            strncpy( buf, deviceResponse_, nBytes );
            buf[nBytes] = '\0';
            char *token = ( char * )buf;

            // Now, parse the heading string
            while( state != Finished && NULL != token )
            {
                switch( state )
                {
                case InLabel:
                    // Throw away invalid leading characters
                    token = strstr( token, "HCHDT" );
                    // Match to the expected response per interface spec
                    if( NULL == token || strncmp( token, "HCHDT", 5 ) != 0 )
                    {
                        state = Finished;
                        logger_.syslog( Str( "Invalid response from SP3003D: " + ( Str ) token ), Syslog::ERROR );
                    }
                    break;

                case InHdg:
                    retVal = atof( token );
                    break;
                }

                // Advance to next state
                state++;

                // Point to next token
                if( NULL != token )
                {
                    token = strchr( token, ',' );
                    if( NULL != token )
                    {
                        ++token;
                    }
                }
            }
        }
    }
    return retVal + magDeviation_;
}


// Returns pitch/roll or nan if not available/error
void AHRS_sp3003D::getPitchRoll( float &pitch, float &roll )
{
    enum
    {
        InLabel,
        InPitch,
        InRoll,
        Finished
    };

    if( !simulateHardware() )
    {

        int state = InLabel;

        // Check and parse response
        if( tryUartComms( "$PSPA,PR\r\n", "getPitchRoll uart error " ) )
        {
            // Checksum is at the end
            char *endPtr = strchr( deviceResponse_, '*' );
            //char *checkSumPtr;

            if( endPtr )
            {
                //checkSumPtr = endPtr;
            }
            else
            {
                // No checksum on this sentence.
                //checkSumPtr = 0;
                endPtr = ( char * )deviceResponse_ + strlen( deviceResponse_ );
            }

            const size_t bufSize( 256 );
            char buf[bufSize];
            unsigned int nBytes = MIN( ( unsigned int )( endPtr - deviceResponse_ ), bufSize - 1 );
            strncpy( buf, deviceResponse_, nBytes );
            buf[nBytes] = '\0';
            char *token = ( char * )buf;

            // Now, parse the heading string
            while( state != Finished && NULL != token )
            {
                switch( state )
                {
                case InLabel:
                    // Throw away invalid leading characters
                    token = strstr( token, "PSPA" );
                    // Match to the expected response per interface spec
                    if( NULL == token || strncmp( token, "PSPA", 4 ) != 0 )
                    {
                        state = Finished;
                        logger_.syslog( Str( "Invalid response from SP3003D: " + ( Str ) token ), Syslog::ERROR );
                    }
                    break;

                case InPitch:
                    pitch = atof( ( strchr( token, '=' ) ) + 1 );
                    break;

                case InRoll:
                    roll = atof( strchr( token, '=' ) + 1 );
                    break;
                }

                // Advance to next state
                state++;

                // Point to next token
                if( NULL != token )
                {
                    token = strchr( token, ',' );
                    if( NULL != token )
                    {
                        ++token;
                    }
                }
            }
        }
    }
}

void AHRS_sp3003D::requestTransducers()
{
    if( !simulateHardware() )
    {
        uart_.flush();
        uart_ << "$xxXDR\r\n";
    }
}

bool AHRS_sp3003D::receiveTransducers( float &compassHeading,
                                       float &pitch, float &roll, float &temperature )
{
    if( !simulateHardware() )
    {
        // Check and parse response
        uart_.readLine( deviceResponse_, sizeof( deviceResponse_ ) );
        // Check response
        if( uart_.hasError() )
        {
            logger_.syslog( "receiveTransducers uart error: ", uart_.errorString(), Syslog::ERROR );
        }
        else
        {
            return ParseHCXDR( deviceResponse_, compassHeading, pitch, roll, temperature );
        }
    }
    return false;

}

bool AHRS_sp3003D::readTransducers( float &compassHeading,
                                    float &pitch, float &roll, float &temperature )
{
    if( !simulateHardware() )
    {
        // Check and parse response
        if( tryUartComms( "$xxXDR\r\n", "readTransducers uart error: " ) )
        {
            return ParseHCXDR( deviceResponse_, compassHeading, pitch, roll, temperature );
        }
    }
    return false;

}


void AHRS_sp3003D::requestHeadingMagBin()
{
    if( simulateHardware() )
    {
        return;
    }
    // Check and parse response
    uart_.flush();
    uart_ << "\xA4\x09\xA0";
}


bool AHRS_sp3003D::receiveHeadingMagBin( float &compassHeading )
{
    bool retVal = false;
    if( simulateHardware() )
    {
        return true;
    }
    // Check and parse response
    uart_.read( deviceResponse_, 5 );
    if( !uart_.hasError()
            && ( ( unsigned char )deviceResponse_[0] == 0xA4 )
            && ( deviceResponse_[1] == 0x09 )
            && ( ( unsigned char )deviceResponse_[4] == 0xA0 ) )
    {
        compassHeading = ( float )extractShort( &deviceResponse_[2] );
        compassHeading = compassHeading * 360.0 / 4096.0;
        compassHeading = D2R( compassHeading );
        retVal = true;
    }
    else
    {
        if( uart_.hasError() )
        {
            logger_.syslog( "readHeadingMagBin UART error: ", uart_.errorString(), Syslog::ERROR );
        }
        if( uart_.bytesRead() > 0 )
        {
            logger_.syslog( "readHeadingMagBin got 0x" + Str( deviceResponse_, uart_.bytesRead() ).asHex(), Syslog::ERROR );
        }
    }
    return retVal;
}


bool AHRS_sp3003D::readHeadingMagBin( float &compassHeading )
{
    requestHeadingMagBin();
    return receiveHeadingMagBin( compassHeading );
}

void AHRS_sp3003D::requestPitchRollBin()
{
    if( simulateHardware() )
    {
        return;
    }
    // Check and parse response
    uart_.flush();
    uart_ << "\xA4\x06\xA0";
}

bool AHRS_sp3003D::receivePitchRollBin( float &pitch, float &roll )
{
    bool retVal = false;
    if( simulateHardware() )
    {
        return true;
    }
    // Check and parse response
    uart_.read( deviceResponse_, 7 );
    if( !uart_.hasError()
            && ( ( unsigned char )deviceResponse_[0] == 0xA4 )
            && ( deviceResponse_[1] == 0x06 )
            && ( ( unsigned char )deviceResponse_[6] == 0xA0 ) )
    {
        pitch = ( float )extractShort( &deviceResponse_[2] );
        pitch = pitch * 90.0 / 4096.0;

        roll = ( float )extractShort( &deviceResponse_[4] );
        roll = roll * 180.0 / 4096.0;

        pitch = D2R( pitch );
        roll = D2R( roll );

        retVal = true;
    }
    else
    {
        if( uart_.hasError() )
        {
            logger_.syslog( "readPitchRollBin UART error: ", uart_.errorString(), Syslog::ERROR );
        }
        if( uart_.bytesRead() > 0 )
        {
            logger_.syslog( "readPitchRollBin got 0x" + Str( deviceResponse_, uart_.bytesRead() ).asHex(), Syslog::ERROR );
        }
    }
    return retVal;
}

bool AHRS_sp3003D::readPitchRollBin( float &pitch, float &roll )
{
    requestPitchRollBin();
    return receivePitchRollBin( pitch, roll );
}

bool AHRS_sp3003D::readMagnetics( float &mX, float &mY, float &mZ, float &mT )
{
    if( !simulateHardware() )
    {
        // Check and parse response
        if( tryUartComms( "$PSPA,M\r\n", "readMagnetics uart error: " ) )
        {
            return ParsePSPAM( deviceResponse_, mX, mY, mZ, mT );
        }
    }
    return false;

}

bool AHRS_sp3003D::readMagneticsBin( float &mX, float &mY, float &mZ )
{
    bool retVal = false;
    if( simulateHardware() )
    {
        return true;
    }
    // Check and parse response
    uart_.flush();
    uart_ << "\xA4\x01\xA0";
    uart_.read( deviceResponse_, 9 );
    if( !uart_.hasError()
            && ( ( unsigned char )deviceResponse_[0] == 0xA4 )
            && ( deviceResponse_[1] == 0x01 )
            && ( ( unsigned char )deviceResponse_[8] == 0xA0 ) )
    {
        mX = ( float )extractShort( &deviceResponse_[2] );
        mY = ( float )extractShort( &deviceResponse_[4] );
        mZ = ( float )extractShort( &deviceResponse_[6] );
        // Check for failed magnetometer
        if( ( ( mX == 0 ) && ( mY == 0 ) && ( mZ == 0 ) ) || ( ( mX <= -4096 ) && ( mY <= -4096 ) && ( mZ <= -4096 ) ) )
        {
            retVal = false;
        }
        else
        {
            retVal = true;
        }
    }
    else
    {
        if( uart_.hasError() )
        {
            logger_.syslog( "readMagneticsBin UART error: ", uart_.errorString(), Syslog::ERROR );
        }
        if( uart_.bytesRead() > 0 )
        {
            logger_.syslog( "readMagneticsBin got 0x" + Str( deviceResponse_, uart_.bytesRead() ).asHex(), Syslog::ERROR );
        }
    }
    return retVal;
}


bool AHRS_sp3003D::readAccelVec( float &aX, float &aY, float &aZ, float &aT )
{
    if( !simulateHardware() )
    {
        // Check and parse response
        if( tryUartComms( "$PSPA,A\r\n", "readAccelVec uart error: " ) )
        {
            return ParsePSPAA( deviceResponse_, aX, aY, aZ, aT );
        }
    }
    return false;

}


//Send: 3 Byte (0xA4, 0x07, 0xA0)
//Response: 11 Bytes (0xA4, 0x07, Ax, Ay, Az, Atotal as 16-bit integers, 0xA0)
bool AHRS_sp3003D::readAccelVecBin( float &aX, float &aY, float &aZ, float &aT )
{
    bool retVal = false;
    if( simulateHardware() )
    {
        return true;
    }
    // Check and parse response
    uart_.flush();
    uart_ << "\xA4\x07\xA0";
    uart_.read( deviceResponse_, 11 );
    if( !uart_.hasError()
            && ( ( unsigned char )deviceResponse_[0] == 0xA4 )
            && ( deviceResponse_[1] == 0x07 )
            && ( ( unsigned char )deviceResponse_[10] == 0xA0 ) )
    {
        aX = ( float )extractShort( &deviceResponse_[2] );
        aY = ( float )extractShort( &deviceResponse_[4] );
        aZ = ( float )extractShort( &deviceResponse_[6] );
        aT = ( float )extractShort( &deviceResponse_[8] );
        retVal = true;
    }
    else
    {
        if( uart_.hasError() )
        {
            logger_.syslog( "readAccelVecBin UART error: ", uart_.errorString(), Syslog::ERROR );
        }
        if( uart_.bytesRead() > 0 )
        {
            logger_.syslog( "readAccelVecBin got 0x" + Str( deviceResponse_, uart_.bytesRead() ).asHex(), Syslog::ERROR );
        }
    }
    return retVal;
}


bool AHRS_sp3003D::ParseHCXDR( const char* response, float &compassHeading,
                               float &pitch, float &roll, float &temperature )
{
    int magError;
    int checksum = 0xFF;
    float trueHeadingTemp;
    // coverity[secure_coding] // indicate that the code below is indeed safe
    if( 7 == sscanf( response,
                     "$HCXDR,A,%5g,D,A,%5g,D,A,%5g,D,A,%6g,D,C,%5g,C,G,%04d*%02X",
                     &compassHeading, &trueHeadingTemp, &pitch, &roll, &temperature, &magError, &checksum ) )
    {
        compassHeading = D2R( compassHeading );
        pitch = D2R( pitch );
        roll = D2R( roll );
        return checksum == CalcNMEAChecksum( response );
    }
    return false;
}


bool AHRS_sp3003D::ParsePSPAM( const char* response, float &mX, float &mY, float &mZ, float &mT )
{
    int checksum = 0xFF;
    // coverity[secure_coding] // indicate that the code below is indeed safe
    if( 5 == sscanf( response,
                     "$PSPA,Mx=%5g,My=%5g,Mz=%5g,Mt=%5g*%02X",
                     &mX, &mY, &mZ, &mT, &checksum ) )
    {
        return checksum == CalcNMEAChecksum( response );
    }
    return false;
}


bool AHRS_sp3003D::ParsePSPAA( const char* response, float &aX, float &aY, float &aZ, float &aT )
{
    int checksum = 0xFF;
    // coverity[secure_coding] // indicate that the code below is indeed safe
    if( 5 == sscanf( response,
                     "$PSPA,Ax=%5g,Ay=%5g,Az=%5g,At=%5g*%02X",
                     &aX, &aY, &aZ, &aT, &checksum ) )
    {
        return checksum == CalcNMEAChecksum( response );
    }
    return false;
}

bool AHRS_sp3003D::ParseMagErr( const char* response, int& magErr )
{
    int checksum = 0xFF;
    // coverity[secure_coding] // indicate that the code below is indeed safe
    if( 2 == sscanf( response,
                     "$PSPA,MagErr=%d*%02X",
                     &magErr, &checksum ) )
    {
        return checksum == CalcNMEAChecksum( response );
    }
    return false;
}


int AHRS_sp3003D::CalcNMEAChecksum( const char* response )
{
    int checksum( 0 );
    while( 0 != *response && '*' != *response )
    {
        if( *response == '$' )
        {
            checksum = 0;
        }
        else
        {
            checksum ^= ( unsigned char ) * response;
        }
        ++response;
    }
    return checksum;
}

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

bool AHRS_sp3003D::setCalMode( CalibrateSpartonMode calMode )
{
    if( calMode_ == calMode )
    {
        return true;
    }

    bool success = false;

    switch( calMode )
    {
    case CAL_OFF:
        success = tryUartComms( "$PSPA,CAL=OFF\r\n", "CAL=OFF uart error: ", Syslog::CRITICAL );
        if( success )
        {
            success = tryUartComms( "$PSPA,MAGERR\r\n", "MAGERR uart error: ", Syslog::CRITICAL );
            if( success )
            {
                int magErr = -1;
                bool qualSuccess( true );
                if( !simulateHardware() )
                {
                    qualSuccess = ParseMagErr( deviceResponse_, magErr );
                }
                if( qualSuccess )
                {
                    logger_.syslog( "Magnetic calibration quality (0[best] to 10000) is ", magErr, Syslog::IMPORTANT );
                }
                else
                {
                    logger_.syslog( "Magnetic calibration quality is unknown. Response was ", deviceResponse_, Syslog::IMPORTANT );
                }
            }
        }
        break;
    case CAL_RESET:
        success = tryUartComms( "$PSPA,CAL=RESET\r\n", "CAL=RESET uart error: ", Syslog::CRITICAL );
        break;
    case CAL_AUTOVAR:
    {
        float latitude, longitude, depth;
        if( latitudeReader_->read( Units::DEGREE, latitude )
                && latitude == latitude
                && longitudeReader_->read( Units::DEGREE, longitude )
                && longitude == longitude
                && depthReader_->read( Units::METER, depth )
                && depth == depth )
        {
            double yearDay = Timestamp::Now().asStructTm().tm_yday;
            double year = Timestamp::Now().asStructTm().tm_year;
            char buffer[160] = "";
            snprintf( buffer, sizeof( buffer ),
                      "$PSPA,AUTOVAR,%f,%f,%f,%.2f\n",
                      latitude, longitude, -depth,
                      year + 1900 + ( yearDay / 365 ) ); //*** DEBUG
            success = tryUartComms( buffer, "AUTOVAR uart error: ", Syslog::CRITICAL );
        }
    }
    break;
    case CAL_MANUAL:
        success = tryUartComms( "$PSPA,CAL=MANUAL\r\n", "CAL=MANUAL uart error: ", Syslog::CRITICAL );
        break;
    case CAL_AUTO:
        success = tryUartComms( "$PSPA,CAL=AUTO\r\n", "CAL=AUTO uart error: ", Syslog::CRITICAL );
        break;
    }

    if( success )
    {
        calMode_ = calMode;
        switch( calMode_ )
        {
        case CAL_OFF:
        case CAL_RESET:
        case CAL_AUTOVAR:
            break;
        case CAL_MANUAL:
        case CAL_AUTO:
            setWritersInvalid( true );
            break;
        }
    }
    else
    {
        calibrationFailure_ = true;
    }

    return success;
}

/// Do what needs to be done to run
/// Similar to initialize, in old init/run/uninit sequence
Component::RunState AHRS_sp3003D::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );
    if( simulateHardware() )
    {
        return STARTING;
    }

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

    logger_.syslog( "Initializing AHRS_sp3003D." );

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


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

    bool ignoreFailure = false;

    readConfig();

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

    // Calculate an initial value for magVariation_
    float initLat, initLon;
    Slate::ReadOnce( "Config/workSite", "initLat", Units::RADIAN, initLat, logger_ );
    Slate::ReadOnce( "Config/workSite", "initLon", Units::RADIAN, initLon, logger_ );
    magVariation_ = MagneticVariation::Radian( Timestamp::Now(), initLat, initLon ); // Init magVariation_ with initial lat/lon values

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    if( readMountingCfg_ && !configSet_ )
    {
        setMountVertical();
        configSet_ = true;
        return STARTING;
    }


    float compHeading = nanf( "" );
    if( readHeadingMagBin( compHeading ) )
    {
        if( !readAccelerationsCfg_ && !readMagneticsCfg_ )
        {
            requestHeadingMagBin();
        }
        return RUNNABLE;
    }
    else
    {

        // If we are able to read from the simSlate, use that instead of missing hardware
        float tempPitch;
        ignoreFailure = SimSlate::Read( SimSlate::PITCH_RADIAN, tempPitch );
        if( ignoreFailure && !usingSimWarned_ )
        {
            logger_.syslog( Str( "SP3003D failed to initialize -- using Simulator" ), Syslog::ERROR );
            usingSimWarned_ = true;
        }

        if( !ignoreFailure )
        {
            logger_.syslog( Str( "SP3003D failed to initialize" ), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        return STOP;
    }

}


/// Pause for a short period (indicated by pauseTime)
Component::RunState AHRS_sp3003D::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    if( !simulateHardware() )
    {
        if( !loadControl_.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: should set continueTime
Component::RunState AHRS_sp3003D::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isDataRequested() )
    {
        return resume();
    }
    return PAUSED;
}


Component::RunState AHRS_sp3003D::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;
        }
    }
    return RESUMING; // State not used at this time
}


Component::RunState AHRS_sp3003D::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    readConfig();
    if( !readAccelerationsCfg_ && !readMagneticsCfg_ )
    {
        requestHeadingMagBin();
    }
    return RUNNABLE; // State not used at this time
}


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

    readConfig();

    if( calibrationFailure_ )
    {
        calMode_ = CAL_OFF;
        calibrationFailure_ = false;
        return STOP;
    }

    if( calMode_ == CAL_MANUAL || calMode_ == CAL_AUTO )
    {
        return RUNNABLE;
    }

    float compHeading = nanf( "" );
    float magHeading = nanf( "" );
    float trueHeading = nanf( "" );
    float pitch = nanf( "" );
    float roll = nanf( "" );
    float temperature = nanf( "" );

    float mX = nanf( "" );
    float mY = nanf( "" );
    float mZ = nanf( "" );
    float mT = nanf( "" );
    float aX = nanf( "" );
    float aY = nanf( "" );
    float aZ = nanf( "" );
    float aT = nanf( "" );

    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 );
        }
    }

    if( !simulateHardware() )
    {
        // Log voltage and current and check for any faults
        loadControl_.requestVoltageAndCurrent();
        if( loadControl_.hasError() )
        {
            // Put anything that isn't an actual fault first
            if( loadControl_.errorString().find( "Software Overcurrent" ) )
            {
                // TODO: Run a short mission to determine the offending subsystems.
                if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR ); // TODO: LCB firmware should be updated with software overcurrent values
            }
            // And things that set failures second
            else
            {
                logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
                return STOP;
            }
        }
    }

    if( !simulateHardware() && !readAccelerationsCfg_ && !readMagneticsCfg_ )
    {
        if( receiveHeadingMagBin( compHeading ) )
        {
            magHeading = compHeading + magDeviation_;
            trueHeading = magHeading + magVariation_;
        }
        else
        {
            logger_.syslog( Str( "Read Heading Failure." ), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }

        if( !readPitchRollBin( pitch, roll ) )
        {
            logger_.syslog( Str( "Read Pitch/Roll Failure." ), Syslog::ERROR );
        }
        else
        {
            pitch += pitchOffset_;
            roll += rollOffset_;

        }

        requestHeadingMagBin();
    }
    else if( !simulateHardware() )
    {
//        bool transOk = readTransducers( compHeading, pitch, roll, temperature );
//        bool magOk = transOk && readMagnetics( mX, mY, mZ, mT );
//        bool accelOk = magOk && readAccelVec( aX, aY, aZ, aT );

        dataTimestamp_ = Timestamp::Now();

        bool hdgOk = readHeadingMagBin( compHeading );
        bool pitchRollOk = hdgOk && readPitchRollBin( pitch, roll );
        bool magOk = !readMagneticsCfg_ || ( pitchRollOk && readMagneticsBin( mX, mY, mZ ) );
        bool accelOk = !readAccelerationsCfg_ || ( magOk && readAccelVecBin( aX, aY, aZ, aT ) );

        if( hdgOk && pitchRollOk )
        {
            magHeading = compHeading + magDeviation_;
            trueHeading = magHeading + magVariation_;
            pitch += pitchOffset_;
            roll += rollOffset_;
        }
        else
        {
            logger_.syslog( Str( "Read Heading/Pitch/Roll Failure." ), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }

        if( readMagneticsCfg_ && magOk )
        {
            mxWriter_->write( Units::MILLIGAUSS, mX, dataTimestamp_ );
            myWriter_->write( Units::MILLIGAUSS, mY, dataTimestamp_ );
            mzWriter_->write( Units::MILLIGAUSS, mZ, dataTimestamp_ );
            mtWriter_->write( Units::MILLIGAUSS, mT, dataTimestamp_ );
        }
        else if( !magOk )
        {
            logger_.syslog( Str( "Read Magnetics Failure." ), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }

        if( readAccelerationsCfg_ && accelOk )
        {
            aX = aX * GRAVITY_MILLI_G;
            aY = aY * GRAVITY_MILLI_G;
            aZ = aZ * GRAVITY_MILLI_G;
            aT = aT * GRAVITY_MILLI_G;

            axWriter_->write( Units::METER_PER_SECOND_SQUARED, aX, dataTimestamp_ );
            ayWriter_->write( Units::METER_PER_SECOND_SQUARED, aY, dataTimestamp_ );
            azWriter_->write( Units::METER_PER_SECOND_SQUARED, aZ, dataTimestamp_ );
            atWriter_->write( Units::METER_PER_SECOND_SQUARED, aT, dataTimestamp_ );
        }
        else if( !accelOk )
        {
            logger_.syslog( Str( "Read Acceleration Vector Failure." ), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
    }

    // Always include simulated values, even if hardware exists.
    // If we are in the water, there will be no simulated values available.
    SimSlate::Read( SimSlate::PITCH_RADIAN, pitch );
    SimSlate::Read( SimSlate::ROLL_RADIAN, roll );
    if( SimSlate::Read( SimSlate::HEADING_RADIAN, trueHeading ) )
    {
        magHeading = trueHeading - magVariation_;
        compHeading = magHeading - magDeviation_;
    }

    // TODO: make data into member variables and use a writeData method

    // Write values to the slate
    compassHeadingWriter_->write( Units::RADIAN, AuvMath::ModPi( compHeading ), dataTimestamp_ );
    compassTemperatureWriter_->write( Units::CELSIUS, temperature, dataTimestamp_ );

    setWritersInvalid( false );
    magneticHeadingWriter_->write( Units::RADIAN, AuvMath::ModPi( magHeading ), dataTimestamp_ );
    trueHeadingWriter_->write( Units::RADIAN, AuvMath::ModPi( trueHeading ), dataTimestamp_ );
    pitchWriter_->write( Units::RADIAN, pitch, dataTimestamp_ );
    rollWriter_->write( Units::RADIAN, roll, dataTimestamp_ );
    Point6D vector( 0, 0, 0, roll, pitch, AuvMath::ModPi( trueHeading ) );
    rotationFromVehicleToNavigationFrame_ = Matrix3x3( vector );
    rotationMatrixWriter_->write2DClass( Units::NONE, rotationFromVehicleToNavigationFrame_, dataTimestamp_ );
    if( verbosity_ > 3 )
    {
        logger_.syslog( "rotationFromVehicleToNavigationFrame_ = [ " + Str( rotationFromVehicleToNavigationFrame_( 0, 0 ) ) + "," + Str( rotationFromVehicleToNavigationFrame_( 1, 0 ) ) + "," + Str( rotationFromVehicleToNavigationFrame_( 2, 0 ) ) + ";" + Str( rotationFromVehicleToNavigationFrame_( 0, 1 ) ) + "," + Str( rotationFromVehicleToNavigationFrame_( 1, 1 ) ) + "," + Str( rotationFromVehicleToNavigationFrame_( 2, 1 ) ) + ";" + Str( rotationFromVehicleToNavigationFrame_( 0, 2 ) ) + "," + Str( rotationFromVehicleToNavigationFrame_( 1, 2 ) ) + "," + Str( rotationFromVehicleToNavigationFrame_( 2, 2 ) ) + " ]", Syslog::INFO ); // TODO: check whether this is transposed from the actual representation
    }

    this->resetFailCount();

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


Component::RunState AHRS_sp3003D::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_sp3003D::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 );
        }

    }
    if( startTime_.elapsed() < poTimeout_ )
    {
        return STOPPING;
    }
    return STOPPED;
}


Component::RunState AHRS_sp3003D::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;
}


bool AHRS_sp3003D::isDataRequested()
{
    return compassHeadingWriter_->isDataRequested()
           || magneticHeadingWriter_->isDataRequested()
           || trueHeadingWriter_->isDataRequested()
           || pitchWriter_->isDataRequested()
           || rollWriter_->isDataRequested()
           || rotationMatrixWriter_->isDataRequested()
           || mxWriter_->isDataRequested()
           || mtWriter_->isDataRequested()
           || axWriter_->isDataRequested()
           || atWriter_->isDataRequested();
}

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


// Sets the mounting configuration to vertical instead of the default horizontal
void AHRS_sp3003D::setMountVertical()
{
    uart_ << "$PSPA,MOUNT=V\r\n";
}


// Flips endian and converts 2 bytes to short
short AHRS_sp3003D::extractShort( char * convertFloat )
{
    short retVal;
    char *retFloat = ( char* ) & retVal;

    // flip endian
    retFloat[0] = convertFloat[1];
    retFloat[1] = convertFloat[0];
    return retVal;
}