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

#include "AHRS_3DMGX3.h"
#include "AHRS_3DMGX3IF.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"

// Secondary compass
#define AHRS_ACCURACY ( 2.0 ) // 2 degree accuracy. See spec
// TODO: Does this accuracy ever change? Does the instrument report it? If so, use setAccuracy or writeWithAccuracy.
#define AHRS_RATE_ACCURACY ( 2.0 ) // 2 radians per second: larger than anything else which would specify a real accuracy
// For rates spec sheet has bias and nonlinearity spec, but not accuracy

AHRS_3DMGX3::AHRS_3DMGX3( const Module* module )
    : SyncSensorComponent( AHRS_3DMGX3IF::NAME, module ),
      debug_( false ),
      loadControl_( AHRS_3DMGX3IF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      ahrsTimeout_( 10.0 ),
      usingSimWarned_( false ),
      magDeviation_( 0.0 ),
      magVariation_( 0.0 ),
      pitchOffset_( 0.0 ),
      rollOffset_( 0.0 ),
      gyroGain_( 0.0 ),
      uart_( AHRS_3DMGX3IF::UART, AHRS_3DMGX3IF::BAUD, 0.3, logger_ ),
      fault_( FailureMode::NONE )
{
    // Slate inputs
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );

    // Slate outputs
    compassHeadingWriter_ = newDataWriter( AHRS_3DMGX3IF::COMPASS_ORIENTATION_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, AHRS_ACCURACY );
    rollWriter_ = newUniversalWriter( UniversalURI::PLATFORM_ROLL_ANGLE, Units::DEGREE, AHRS_ACCURACY );
    rotationMatrixWriter_ = newUniversalBlobWriter( UniversalURI::PLATFORM_ORIENTATION_MATRIX, Units::NONE, AHRS_ACCURACY );

    rollRateWriter_ = newUniversalWriter( UniversalURI::PLATFORM_ROLL_RATE, Units::RADIAN_PER_SECOND, AHRS_RATE_ACCURACY ); // spec sheet has bias and nonlinearity spec, but not accuracy
    pitchRateWriter_ = newUniversalWriter( UniversalURI::PLATFORM_PITCH_RATE, Units::RADIAN_PER_SECOND, AHRS_RATE_ACCURACY ); // spec sheet has bias and nonlinearity spec, but not accuracy
    yawRateWriter_ = newUniversalWriter( UniversalURI::PLATFORM_YAW_RATE, Units::RADIAN_PER_SECOND, AHRS_RATE_ACCURACY ); // spec sheet has bias and nonlinearity spec, but not accuracy

    magDeviationCfgReader_ = newConfigReader( AHRS_3DMGX3IF::MAG_DEVIATION_CFG );
    pitchOffsetCfgReader_ = newConfigReader( AHRS_3DMGX3IF::PITCH_OFFSET_CFG );
    rollOffsetCfgReader_ = newConfigReader( AHRS_3DMGX3IF::ROLL_OFFSET_CFG );

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


AHRS_3DMGX3::~AHRS_3DMGX3()
{
}

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

void AHRS_3DMGX3::run()
{
}

void AHRS_3DMGX3::uninitialize()
{
    if( !simulateHardware() )
    {
        uart_.close();

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


// Flips endian and converts 4 bytes to floating point
float AHRS_3DMGX3::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;
}


FailureMode::FailType AHRS_3DMGX3::readTemperature( float &temperature )
{
    uart_.flush();
    uart_ << "\x07";
    uart_.waitForBufferEmpty();
    uart_.read( deviceResponse_, 7 );

    if( uart_.hasError() )
    {
        logger_.syslog( "readTemperature uart error: ", uart_.errorString(), Syslog::ERROR );
        return FailureMode::COMMUNICATIONS;
    }
    else
    {
        unsigned short calcChecksum = deviceResponse_[0];
        unsigned short usTemperature = ( ( ( unsigned short )deviceResponse_[1] ) << 8 ) + ( unsigned short )deviceResponse_[2];
        unsigned short usTimerTicks = ( ( ( unsigned short )deviceResponse_[3] ) << 8 ) + ( unsigned short )deviceResponse_[4];
        unsigned short checksum = ( ( ( unsigned short )deviceResponse_[5] ) << 8 ) + ( unsigned short )deviceResponse_[6];

        calcChecksum += usTemperature + usTimerTicks;

        //printf("readTemperature checksum=0x%04hX, calcChecksum=0x%04hX\n", checksum, calcChecksum);
        if( checksum != calcChecksum )
        {
            return FailureMode::DATA;
        }

        temperature = ( ( usTemperature * 5.0 / 65536.0 ) - 0.5 ) * 100;
    }
    return FailureMode::NONE;
}

FailureMode::FailType AHRS_3DMGX3::readTransducers( float &roll, float &pitch, float &compHeading,
        float &rollRate, float &pitchRate, float &yawRate )
{
    uart_.flush();
    uart_ << "\xCF";
    uart_.read( deviceResponse_, 31 );
    if( uart_.hasError() )
    {
        logger_.syslog( "readTransducers uart error: ", uart_.errorString(), Syslog::ERROR );
        return FailureMode::COMMUNICATIONS;
    }
    else
    {
        // Calculate the cksum
        unsigned short calcChecksum = deviceResponse_[0];
        unsigned short checksum = ( ( ( unsigned short )deviceResponse_[29] ) << 8 ) + ( unsigned short )deviceResponse_[30];
        for( int i = 1; i < 29; i++ )
        {
            calcChecksum += deviceResponse_[i];
        }
        //printf( "readTransducers checksum=0x%04hX, calcChecksum=0x%04hX\n", checksum, calcChecksum ); //*** DEBUG
        if( checksum != calcChecksum )
        {
            return FailureMode::DATA;
        }

        roll = extractFloat( &deviceResponse_[1] );
        pitch = extractFloat( &deviceResponse_[5] );
        compHeading = extractFloat( &deviceResponse_[9] );
        rollRate = extractFloat( &deviceResponse_[13] );
        pitchRate = extractFloat( &deviceResponse_[17] );
        yawRate = extractFloat( &deviceResponse_[21] );
    }
    return FailureMode::NONE;
}

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

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

    logger_.syslog( "Initializing AHRS_3DMGX3." );

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

}


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

    float roll = nanf( "" );
    float pitch = nanf( "" );
    float compHeading = nanf( "" );
    float rollRate = nanf( "" );
    float pitchRate = nanf( "" );
    float yawRate = nanf( "" );

    // 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

    readConfig();

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    fault_ = readTransducers( roll, pitch, compHeading, rollRate, pitchRate, yawRate );
    if( fault_ == FailureMode::NONE )
    {
        return RUNNABLE;
    }
    else
    {
        bool ignoreFailure = false;
        // 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( "3DMGX3 failed to initialize -- using Simulator" ), Syslog::ERROR );
            usingSimWarned_ = true;
        }
        if( !ignoreFailure )
        {
            logger_.syslog( Str( "3DMGX3 failed to initialize" ), Syslog::FAULT );
            this->setFailure( fault_ );
        }
        return STOP;
    }
}


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


Component::RunState AHRS_3DMGX3::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );

    if( !simulateHardware() )
    {
        // Open the uart
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error opening port on resume: ", 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;
        }
    }
    return RESUMING; // State not used at this time
}


Component::RunState AHRS_3DMGX3::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    return RUNNABLE; // State not used at this time
}


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

    readConfig();

    // Processing for the compass
    float roll = nanf( "" );
    float pitch = nanf( "" );
    float compHeading = nanf( "" );
    float magHeading = nanf( "" );
    float trueHeading = nanf( "" );
    float rollRate = nanf( "" );
    float pitchRate = nanf( "" );
    float yawRate = 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() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }

        fault_ = readTransducers( roll, pitch, compHeading, rollRate, pitchRate, yawRate );
        if( fault_ == FailureMode::NONE )
        {
            magHeading = compHeading + magDeviation_;
            trueHeading = magHeading + magVariation_;

            pitch += pitchOffset_;
            roll += rollOffset_;

            startTime_ = Timestamp::Now();
        }
        else
        {
            if( startTime_.elapsed() > ahrsTimeout_ )
            {
                logger_.syslog( Str( "Data timeout failure." ), Syslog::FAULT );
                this->setFailure( fault_ );
                return START;
            }
        }
    }

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

    // Write values to the slate
    rollWriter_->write( Units::RADIAN, roll );
    pitchWriter_->write( Units::RADIAN, AuvMath::ModPi( pitch ) );
    compassHeadingWriter_->write( Units::RADIAN, AuvMath::ModPi( compHeading ) );
    magneticHeadingWriter_->write( Units::RADIAN, AuvMath::ModPi( magHeading ) );
    trueHeadingWriter_->write( Units::RADIAN, AuvMath::ModPi( trueHeading ) );
    Point6D vector( 0, 0, 0, roll, pitch, trueHeading );
    Matrix3x3 matrix( vector );
    rotationMatrixWriter_->write2DClass( Units::NONE, matrix );

    if( !simulateHardware() )
    {
        rollRateWriter_->write( Units::RADIAN_PER_SECOND, rollRate );
        pitchRateWriter_->write( Units::RADIAN_PER_SECOND, pitchRate );
        yawRateWriter_->write( Units::RADIAN_PER_SECOND, yawRate );
        this->resetFailCount();
    }



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

    return RUNNABLE;
}


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


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

    }
    // Currently used to delay one more cycle to allow the EZ Servo to fully power down
    return STOPPED;
}

Component::RunState AHRS_3DMGX3::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_3DMGX3::isDataRequested()
{
    return compassHeadingWriter_->isDataRequested()
           || magneticHeadingWriter_->isDataRequested()
           || trueHeadingWriter_->isDataRequested()
           || pitchWriter_->isDataRequested()
           || rollWriter_->isDataRequested()
           || rotationMatrixWriter_->isDataRequested()
           || rollRateWriter_->isDataRequested()
           || pitchRateWriter_->isDataRequested()
           || yawRateWriter_->isDataRequested();
}


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