/** \file
 *
 *  Contains the PNI_TCM class implementation.
 *
 *  Copyright (c) 2014 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

// PNI configuration prior to installation //
// Mounting: Standard
// North Reference: Magnetic
// Endian: Big
// Output: Degrees
// Acquisition: Continuous
// getDataResp returning Heading (magnetic), Pitch, Roll, MagX, MagY, MagZ, Temperature.
// Delays: 0.0
// Calibration Settings:
// 		- Auto sampling
//		- 32 points
//		- Full range calibration
//
// PNI configuration prior to installation //

#include "PNI_TCM.h"
#include "PNI_TCMIF.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" // needed for worksite.cfg
#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.034907)      // 2 degree accuracy in radians. See spec for limited tilt cal even though we're doing full it will still be <45 degrees. 
#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



char PNI_TCM::SetConfigAutoCal_[CMD_SIZE] = "\x00\x07\x06\x0D\x01\x85\xF0"; // Sets kUserCalAutoSampling to True
char PNI_TCM::SetConfigCalNumPts_[CMD_SIZE] = "\x00\x07\x06\x0C\x20\x82\x82"; // Sets kUserCalNumPoints to 32
char PNI_TCM::SetConfigMagCoeff_[CMD_SIZE] = "\x00\x07\x06\x12\x01\x96\xBD"; // Sets kMagCoeffSet to 1
//char PNI_TCM::SetConfigMountingRef_[CMD_SIZE] = "\x00\x07\x06\x0A\x07\x7C\xA1"; // Sets mounting reference to Z down 0 degree. Frame ID 6, Config ID 10 decimal
//char PNI_TCM::SetConfigMountingRef_[CMD_SIZE] = "\x00\x07\x06\x0A\x09\x9D\x6F"; // Sets mounting reference to X up 180 degree. Frame ID 6, Config ID 10 decimal, value 9.
char PNI_TCM::SetConfigMountingRef_[CMD_SIZE] = "\x00\x07\x06\x0A\x02\x2C\x04"; // Sets mounting reference to X up 0 degree. Frame ID 6, Config ID 10 decimal, value 2.
char PNI_TCM::SetConfigBaudRate_[CMD_SIZE] = "\x00\x07\x06\x0E\x0C\x01\x0E"; // Sets baud rate to 38400 (default). Frame ID 6, Config ID 14 decimal
char PNI_TCM::SetBigEndian_[CMD_SIZE] = "\x00\x07\x06\x06\x01\x59\0Ax"; // Sets Endian to big (default). Frame ID 6, Config ID 6 decimal
char PNI_TCM::SetDataComponents_[CMD_SIZE] = "\x00\x0D\x03\x07\x05\x18\x19\x1B\x1C\x1D\x07\x96\x52"; // Frame ID 3d. Requests heading, pitch, roll, Mag x/y/z, temp
char PNI_TCM::StartCalFullRange_[CMD_SIZE] = "\x00\x09\x0A\x00\x00\x00\x0A\xAF\x06"; // Frame ID 10d with full range calibration to the maximum tilt angles we can acheive.
char PNI_TCM::StartContinuousMode_[CMD_SIZE] = "\x00\x05\x15\xBD\x61"; // Frame ID 21d
char PNI_TCM::StopContinuousMode_[CMD_SIZE] = "\x00\x05\x16\x8D\x02";  // Frame ID 22d

char PNI_TCM::SetAcqParams_[CMD_SIZE] = "\x00\x0F\x18\x00\x00\x00\x00\x00\x00\x3E\xCC\xCC\xCD\x62\xAD";  // Frame ID 24d sets Sample delay to 0.4 seconds in continuous mode.
// Why is Acquisition mode set to 0? The spec indicates "Continuous Mode is TRUE and is the default" for the uInt8 labeled "Acquisition Mode" in kSetAcqParams.
// However, vehicle trials indicate that acquisition mode == 0 is actually continuous mode and that if a 01 is provided, the PNI respones in polled mode only.
// This comment added against DOC#1014688 r06 page 55 of the TCM User Manual - July 2013. Note that while the footer reads revision 6, the title shows revision 7.

char PNI_TCM::Save_[CMD_SIZE] = "\x00\x05\x09\x6E\xDC";  // Frame ID 9d

//"\x00\x0F\x18\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x8B\x15"; // 0 sample delay

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


PNI_TCM* PNI_TCM::Instance_( NULL );

PNI_TCM::PNI_TCM( const Module* module )
    : SyncSensorComponent( PNI_TCMIF::NAME, module ),
      startup_( SETDATA ),
      debug_( false ),
      verbosity_( 0 ),
      loadControl_( PNI_TCMIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      checksum_matches_( false ),
      heading_( 0.0 ),
      pitch_( 0.0 ),
      roll_( 0.0 ),
      magHeading_( 0.0 ),
      trueHeading_( 0.0 ),
      mX_( 0.0 ),
      mY_( 0.0 ),
      mZ_( 0.0 ),
      temperature_( 0.0 ),
      magDeviation_( 0.0 ),
      magVariation_( 0.0 ),
      // TODO: add a heading offset
      pitchOffset_( 0.0 ),
      rollOffset_( 0.0 ),
      poTimeout_( 0.5 ), // spec states 210 msec
      validTimeout_( 5.0 ), // Valid data from the unit should be expected within this time
      uart_( PNI_TCMIF::UART, PNI_TCMIF::BAUD, 0.3, logger_, 4095 )

{

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

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

    mxWriter_ = newDataWriter( PNI_TCMIF::MX_READING );
    myWriter_ = newDataWriter( PNI_TCMIF::MY_READING );
    mzWriter_ = newDataWriter( PNI_TCMIF::MZ_READING );

    magneticHeadingWriter_ = newUniversalWriter( UniversalURI::PLATFORM_MAGNETIC_ORIENTATION, Units::RADIAN, AHRS_ACCURACY );
    trueHeadingWriter_ = newUniversalWriter( UniversalURI::PLATFORM_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 );
    Point3D accuracyVector( AHRS_ACCURACY, PITCH_ROLL_ACCURACY, PITCH_ROLL_ACCURACY );
    rotationMatrixWriter_ = newUniversalBlobWriter( UniversalURI::PLATFORM_ORIENTATION_MATRIX, Units::NONE, accuracyVector.getMagnitude() );

    verbosityCfgReader_ = newConfigReader( PNI_TCMIF::VERBOSITY_CFG );
    magDeviationCfgReader_ = newConfigReader( PNI_TCMIF::MAG_DEVIATION_CFG );
    pitchOffsetCfgReader_ = newConfigReader( PNI_TCMIF::PITCH_OFFSET_CFG );
    rollOffsetCfgReader_ = newConfigReader( PNI_TCMIF::ROLL_OFFSET_CFG );

    Instance_ = this;

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


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


void PNI_TCM::run()
{
}


void PNI_TCM::readConfig()
{
    verbosityCfgReader_->read( Units::COUNT, verbosity_ );
    magDeviationCfgReader_->read( Units::RADIAN, magDeviation_ );
    pitchOffsetCfgReader_->read( Units::RADIAN, pitchOffset_ );
    rollOffsetCfgReader_->read( Units::RADIAN, rollOffset_ );
}


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

    startup_ = SETDATA;

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

/// Do what needs to be done to run
/// Similar to initialize, in old init/run/uninit sequence
Component::RunState PNI_TCM::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( "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::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    startTime_ = Timestamp::Now();
    return STARTING;
}


Component::RunState PNI_TCM::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

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

    readConfig();

    if( !simulateHardware() )
    {
        switch( startup_ )
        {
        case SETDATA:
            // flush the uart and command what data to bring back
            uart_.flush();
            uart_.write( SetDataComponents_, SetDataComponents_[1] );
            startup_ = MOUNTING;
            break;
        case MOUNTING:
            // Set the mounting configuration
            uart_.write( SetConfigMountingRef_, SetConfigMountingRef_[1] );
            startup_ = ENDIAN;
            break;
        case ENDIAN:
            // Set the endian
            uart_.write( SetBigEndian_, SetBigEndian_[1] );
            startup_ = PARAMS;
            break;
        case PARAMS:
            uart_.flush();
            // Set the acquisition parameters
            uart_.write( SetAcqParams_, SetAcqParams_[1] );
            startup_ = PARAMRESPONSE;
            startTime_ = Timestamp::Now();
            break;
        case PARAMRESPONSE:
            // Waits for response from setting params
            if( uart_.dataAvailable() )
            {
                uart_.read( deviceResponse_, 5 );
                if( uart_.hasError() )
                {
                    logger_.syslog( "No parameter response UART error: ", uart_.errorString(), Syslog::ERROR );
                    if( uart_.bytesRead() > 0 )
                    {
                        logger_.syslog( "Received 0x" + Str( deviceResponse_, uart_.bytesRead() ).asHex(), Syslog::ERROR );
                    }
                }
                startup_ = DONE;
            }
            else
                startup_ = PARAMRESPONSE;

            // response not received
            if( startTime_.elapsed() > validTimeout_ )
            {
                logger_.syslog( "Failed to set parameters during initialization.", Syslog::FAULT );
                this->setFailure( FailureMode::DATA );
                return STOP;
            }

            break;
        case DONE:
            startup_ = SETDATA;
            uart_.flush(); // Start clean
            startTime_ = Timestamp::Now();

            uart_.write( StartContinuousMode_, StartContinuousMode_[1] ); // Command continuous mode to begin
            return RUNNABLE;
            break;
        }
        return STARTING; // Come back until we finish this case statement.
    }
    else // simulated
    {
        startTime_ = Timestamp::Now();
        return RESUMING;
    }

}


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


Component::RunState PNI_TCM::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::ERROR );
            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 PNI_TCM::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    readConfig();


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

    readConfig();

    if( !simulateHardware() )
    {
        // flush the uart and command data to start flowing
        uart_.flush();
        uart_.write( StartContinuousMode_, StartContinuousMode_[1] );
    }
    startTime_ = Timestamp::Now();
    return RUNNABLE;
}


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

    readConfig();

    // If not simulating, check the load control board.
    // TODO: This is boilerplate for all sensors. It should be part of a superclass. (There is a matching TODO in the non-simulation branch of the if statement below.)
    if( !simulateHardware() ) // checkLoadControl();
    {
        // 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;
        }
    }

    // First get the magnetic variation
    getMagneticVariation();

    if( receiveGetDataResp() )
    {
        startTime_ = Timestamp::Now(); // Restart the timer (and timestamp the data -- TODO: consider separating timestamps, since this is a bit _after_ the data was read from the serial port...)
        // Check for values in range before applying offsets, which could push things a little out of range
        setWritersInvalid(
            !AuvMath::InLimit( heading_, HEADING_LIMIT[ 0 ], HEADING_LIMIT[ 1 ] )
            || !AuvMath::InLimit( pitch_, PITCH_LIMIT[ 0 ], PITCH_LIMIT[ 1 ] )
            || !AuvMath::InLimit( roll_, ROLL_LIMIT[ 0 ], ROLL_LIMIT[ 1 ] )
        ); // The writers are valid if primary values are in range
        processData();
        writeData();
        this->resetFailCount(); // Good data, reset the fail count
    }

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


Component::RunState PNI_TCM::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 PNI_TCM::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 PNI_TCM::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 PNI_TCM::isDataRequested()
{
    return compassHeadingWriter_->isDataRequested()
           || magneticHeadingWriter_->isDataRequested()
           || trueHeadingWriter_->isDataRequested()
           || pitchWriter_->isDataRequested()
           || rollWriter_->isDataRequested()
           || rotationMatrixWriter_->isDataRequested()
           || mxWriter_->isDataRequested()
           || myWriter_->isDataRequested()
           || mzWriter_->isDataRequested();
}


// getDataResp returning Heading (magnetic), Pitch, Roll, MagX, MagY, MagZ, Temperature. Values are float32
// Heading, pitch, and roll are in degrees.
//
// Payload Expected
// ID Count: 7 (UInt8)
// Heading ID: 5 (UInt8)
// Headin Value (float32)
// Pitch ID: 24 (UInt8)
// Pitch Value (float 32)
// Roll ID: 25 (UInt8)
// Roll Value (float32)
// MagX ID: 27 (UInt8)
// MagX Value (float32)
// MagY ID: 28 (UInt8)
// MagY Value (float32)
// MagZ ID: 29 (UInt8)
// MagZ Value (float32)
// Temp ID: 7 (UInt8)
// Temp Value (float32)
bool PNI_TCM::receiveGetDataResp( void )
{
    if( simulateHardware() )
    {
        SimAhrsStruct ahrsSimData;

        SimSlate::Read( ahrsSimData );

        if( !isnan( ahrsSimData.roll )
                && !isnan( ahrsSimData.pitch )
                && !isnan( ahrsSimData.yaw ) )
        {
            // do not convert the simulated to degrees, convert the compass output to radians instead TODO: see if we can get compass to output radians, quaternion, or rotation matrix
            pitch_ = ahrsSimData.pitch - pitchOffset_;
            roll_ = ahrsSimData.roll - rollOffset_;
            trueHeading_ = ahrsSimData.yaw;

            magHeading_ = trueHeading_ - magVariation_;
            heading_ = magHeading_ - magDeviation_;
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        while( DATA_RESP_SIZE <= uart_.dataAvailable() ) // using while loop here to cycle through all of the data available on the serial port and get to the most recent packet -- the compass really should be an asyc component
        {
            uart_.read( deviceResponse_, DATA_RESP_SIZE );
            if( uart_.hasError() )
            {
                logger_.syslog( "GetDataResp UART error: ", uart_.errorString(), Syslog::ERROR );
                if( uart_.bytesRead() > 0 )
                {
                    logger_.syslog( "Received 0x" + Str( deviceResponse_, uart_.bytesRead() ).asHex(), Syslog::ERROR );
                }
                checksum_matches_ = false;
            }
            else
            {
                // Grab the CRC and check it against our calculation
                unsigned short crc = getCRC( deviceResponse_, DATA_RESP_SIZE - 2 ); // Passing all of data response but leaving off the CRC in the length (-2)
                if( crc == extractShort( &deviceResponse_[39] ) )
                {
                    checksum_matches_ = true;
                }
                else
                {
                    checksum_matches_ = false;
                    if( debug_ ) logger_.syslog( "CRC does not match. Expected:0x" + Str( crc ) + " got:0x" + Str( extractShort( &deviceResponse_[39] ) ), Syslog::ERROR );
                }
            }
        }

        if( checksum_matches_ )
        {
            // If we're here, the CRC is good. Load up the values
            heading_ = D2R( extractFloat( &deviceResponse_[5] ) );
            pitch_ = D2R( extractFloat( &deviceResponse_[10] ) );
            roll_ = D2R( extractFloat( &deviceResponse_[15] ) );
            mX_ = extractFloat( &deviceResponse_[20] );
            mY_ = extractFloat( &deviceResponse_[25] );
            mZ_ = extractFloat( &deviceResponse_[30] );
            temperature_ = extractFloat( &deviceResponse_[35] );
            checksum_matches_ = false; // Clean up for next time through the loop (or not through the loop). This fixes the bug that keeps writing the last value after the compass gets unpluged.
            return true;
        }
        else
        {
            uart_.flush(); // Try flushing what's here if we got out of step
            return false;
        }
    }
}

void PNI_TCM::processData( void )
{
    // Apply variation, deviation, and offsets
    magHeading_ = heading_ + magDeviation_;
    trueHeading_ = magHeading_ + magVariation_;
    pitch_ += pitchOffset_;
    roll_ += rollOffset_;
    // TODO: use offsets in a rotation matrix (rotationFromInstrumentToVehicleFrame_) to rotate the rotationFromInstrumentToNavigationFrame_ into rotationFromVehicleToNavigationFrame_
    // make the orientation matrix
    Point6D zzzrph( 0, 0, 0, roll_, pitch_, AuvMath::ModPi( trueHeading_ ) );
    rotationFromVehicleToNavigationFrame_ = Matrix3x3( zzzrph );
    // TODO: try this one-liner: rotationFromVehicleToNavigationFrame_ = Matrix3x3( Point6D( 0, 0, 0, roll_, pitch_, AuvMath::ModPi( trueHeading_ ) ) );
    // TODO: rotate the magnetic field vector into vehicle frame
}

void PNI_TCM::writeData( void )
{
    magneticHeadingWriter_->write( Units::RADIAN, magHeading_, startTime_ );
    trueHeadingWriter_->write( Units::RADIAN, trueHeading_, startTime_ );
    // TODO: strongly suggest writing the heading as read from the compass!
    pitchWriter_->write( Units::RADIAN, pitch_, startTime_ );
    rollWriter_->write( Units::RADIAN, roll_, startTime_ );
    rotationMatrixWriter_->write2DClass( Units::NONE, rotationFromVehicleToNavigationFrame_, startTime_ );
    mxWriter_->write( Units::MICROTESLA, mX_, startTime_ );
    myWriter_->write( Units::MICROTESLA, mY_, startTime_ );
    mzWriter_->write( Units::MICROTESLA, mZ_, startTime_ );
    compassTemperatureWriter_->write( Units::CELSIUS, temperature_, startTime_ );
    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
}

// Parses the calibration score which may take > 1 minute to be calculated.
// Acceptable ranges:
// magScore: <=1 for full, <=2 for other methods
// AccelScore: for mag-only calibration, the score will be 99.99 so it it not reported here
// distError: 0
// tiltError: 0 but sufficient tilt is reported given the calibration method
// tiltRange: Whatever pitch is commanded (i.e. 30 degrees)
bool PNI_TCM::parseCalScore( float &magScore, float &distError, float &tiltError, float &tiltRange )
{
    logger_.syslog( "Cal score parsing is not yet implemented!", Syslog::ERROR );
    return false;
}


//
// Update the CRC for transmitted and received data using the
// CCITT 16bit algorithm (X^16 + X^12 + X^5 + 1).
//
unsigned short PNI_TCM::getCRC( const char* data, unsigned int len )
{
    unsigned char * dataPtr = ( unsigned char * )data;
    unsigned int index = 0;
    unsigned short crc = 0;
    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;
}


// Flips endian and converts 4 bytes to floating point
float PNI_TCM::extractFloat( char * convertFloat )
{
    float retVal;
    char *retFloat = ( char* ) & retVal;

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


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

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


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


void PNI_TCM::setWritersInvalid( bool invalid )
{
    mxWriter_->setInvalid( invalid );
    myWriter_->setInvalid( invalid );
    mzWriter_->setInvalid( invalid );

    magneticHeadingWriter_->setInvalid( invalid );
    trueHeadingWriter_->setInvalid( invalid );
    pitchWriter_->setInvalid( invalid );
    rollWriter_->setInvalid( invalid );
    rotationMatrixWriter_->setInvalid( invalid );
}


void PNI_TCM::getMagneticVariation()
{
    float latitude, longitude;
    if( latitudeReader_->isActive() && latitudeReader_->read( Units::RADIAN, latitude ) && latitude == latitude
            &&  longitudeReader_->isActive() && longitudeReader_->read( Units::RADIAN, longitude ) && longitude == longitude )
    {
        magVariation_ = MagneticVariation::Radian( Timestamp::Now(), latitude, longitude );
    }
    else
    {
        // Or we'll take our location from the configuration and do the best we can.
        // TODO: Decide whether it would be better to just not change magVariation_. It would certainly be simpler.
        if( !Slate::ReadOnce( "Config/workSite", "initLat", Units::RADIAN, latitude, logger_ ) ) logger_.syslog( "did not find initLat", Syslog::FAULT );
        if( !Slate::ReadOnce( "Config/workSite", "initLon", Units::RADIAN, longitude, logger_ ) ) logger_.syslog( "did not find initLon", Syslog::FAULT );
        magVariation_ = MagneticVariation::Radian( Timestamp::Now(), latitude, longitude );
    }
}