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

#include "Turbulence_NPS.h"
#include "Turbulence_NPSIF.h"

#include "data/BlobWriter.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "bitModule/CBITIF.h"

#include <stdlib.h>

Turbulence_NPS::Turbulence_NPS( const Module* module )
    : AsyncComponent( Turbulence_NPSIF::NAME, module ),
      debug_( false ),
      firstPacket_( true ),
      loadControl_( Turbulence_NPSIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      sensorTimeout_( 15.0 ),
      pausePeriod_( 0.4 ),
      uart_( Turbulence_NPSIF::UART, Turbulence_NPSIF::BAUD, 0.10, logger_, 4095, true )
{
    gfScanActiveReader_ = newDataReader( CBITIF::GF_ACTIVE_STATE );

    microTemp1Writer_ = newBlobWriter( Turbulence_NPSIF::MICRO_TEMP1 );
    microCondWriter_ = newBlobWriter( Turbulence_NPSIF::MICRO_COND );
    microTemp2Writer_ = newBlobWriter( Turbulence_NPSIF::MICRO_TEMP2 );
    numTCSamplesWriter_ = newDataWriter( Turbulence_NPSIF::NUM_TEMP_COND_SAMPLES );
    acmPathWriter_ = newBlobWriter( Turbulence_NPSIF::ACM_PATH );
    rollWriter_ = newDataWriter( Turbulence_NPSIF::PLATFORM_ROLL_ANGLE );
    pitchWriter_ = newDataWriter( Turbulence_NPSIF::PLATFORM_PITCH_ANGLE );
    yawWriter_ = newDataWriter( Turbulence_NPSIF::PLATFORM_ORIENTATION );
    deltaXVelocityWriter_ = newDataWriter( Turbulence_NPSIF::DELTA_X_VELOCITY );
    deltaYVelocityWriter_ = newDataWriter( Turbulence_NPSIF::DELTA_Y_VELOCITY );
    deltaZVelocityWriter_ = newDataWriter( Turbulence_NPSIF::DELTA_Z_VELOCITY );
    fwdSigStrengthWriter_ = newBlobWriter( Turbulence_NPSIF::FWD_SIG_STRENGTH );
    packetNumWriter_ = newDataWriter( Turbulence_NPSIF::PACKET_NUM );

    this->setFailureMissionCritical( false );

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

    if( simulateHardware() )
    {
        setState( BLOCK_PERIODIC );
        setPeriod( 1.0 / 16.0 );
    }
}

Turbulence_NPS::~Turbulence_NPS()
{}

void Turbulence_NPS::receive()
{
    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 );
                setRunState( STOP );
            }
        }

        const unsigned char preamble[] = { 0xFE, 0xED };
        // Read from the uart, discarding bytes until the preamble
        if( uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), preamble, 2, true ).hasError() )
        {
            if( uart_.getError() != UartStream::TIMEOUT )
            {
                logger_.syslog( "Uart error: ", uart_.errorString(), Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
                setRunState( STOP );
            }
        }
        else
        {
            if( !firstPacket_ && uart_.bytesRead() > 3 )
            {
                logger_.syslog( "Bytes before packet: ", ( int )uart_.bytesRead(), Syslog::FAULT );
            }
            firstPacket_ = false;

            startTime_ = Timestamp::Now();
            uart_.read( deviceResponse_ + 2, 2 );
            short packetSize = ( deviceResponse_[2] & 0xff ) * 256 + ( deviceResponse_[3] & 0xff );
            if( packetSize != Turbulence_NPSIF::NUM_BYTES )
            {
                logger_.syslog( "Invalid packet size", packetSize, Syslog::FAULT );
                packetSize = Turbulence_NPSIF::NUM_BYTES;
            }
            uart_.read( deviceResponse_ + 4, packetSize - 4 );

            if( uart_.hasError() )
            {
                if( uart_.getError() != UartStream::TIMEOUT )
                {
                    logger_.syslog( "Uart error: ", uart_.errorString(), Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    setRunState( STOP );
                }
            }
            else // No error. We have a complete line.
            {
                writeData();
                //printf("*** wrote %s\n", value.asString().asHex().cStr());
                this->resetFailCount();
            }
        }

        // Finally, check for lapse in data
        if( startTime_.elapsed() > sensorTimeout_ )
        {
            logger_.syslog( Str( "No communication! Re-initializing" ), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            setRunState( STOP );
        }
    }
    else
    {
        pausePeriod_.sleepFor();
    }
}

void Turbulence_NPS::run()
{
}

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

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


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

    if( simulateHardware() )
    {
        return STARTING;
    }

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

    // 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 Turbulence_NPS::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    return RUNNABLE;
}


/// Pause for a short period (indicated by pauseTime)
Component::RunState Turbulence_NPS::pause()
{
    logger_.syslog( "Pause powering down", Syslog::INFO );

    rollWriter_->setInvalid( true );
    pitchWriter_->setInvalid( true );
    yawWriter_->setInvalid( true );

    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 Turbulence_NPS::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isDataRequested() )
    {
        return resume();
    }
    pausePeriod_.sleepFor();
    return PAUSED;
}


Component::RunState Turbulence_NPS::resume()
{
    logger_.syslog( "Resume powering up", 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( !simulateHardware() && !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        firstPacket_ = true;
    }
    return RESUMING;
}


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

    startTime_ = Timestamp::Now();
    return RUNNABLE;
}


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

    receive();

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


Component::RunState Turbulence_NPS::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );

    rollWriter_->setInvalid( true );
    pitchWriter_->setInvalid( true );
    yawWriter_->setInvalid( true );

    if( !simulateHardware() )
    {
        uninitialize(); // First power down then query for faults next cycle
    }
    pausePeriod_.sleepFor();
    return STOPPING;
}


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

    }
    // skipping pasusePeriod here as we can run straight through to stopped
    return STOPPED;
}


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

    pausePeriod_.sleepFor();

    return STOPPED;
}


bool Turbulence_NPS::isDataRequested()
{
    return microTemp1Writer_->isDataRequested()
           || microCondWriter_->isDataRequested()
           || microTemp2Writer_->isDataRequested();
}

void Turbulence_NPS::writeData()
{
    size_t offset = 4;
    for( int i = 0; i < 17; ++i )
    {
        memcpy( shortBuffer_ + i, deviceResponse_ + offset + i * 8, 2 );
    }
    microTemp1Writer_->writeBlob( ( void* )shortBuffer_, 17, startTime_ );

    offset += 2;
    for( int i = 0; i < 34; ++i )
    {
        memcpy( shortBuffer_ + i, deviceResponse_ + offset + i * 4, 2 );
    }
    microCondWriter_->writeBlob( ( void* )shortBuffer_, 34, startTime_ );

    offset += 2;
    for( int i = 0; i < 17; ++i )
    {
        memcpy( shortBuffer_ + i, deviceResponse_ + offset + i * 8, 2 );
    }
    microTemp2Writer_->writeBlob( ( void* )shortBuffer_, 17, startTime_ );

    // Skip ahead of the big interleaved arrays...
    offset = 140;

    int numTCSamples = ( ( deviceResponse_[offset] & 0xFF ) << 8 )
                       + ( deviceResponse_[offset + 1] & 0xFF );
    numTCSamplesWriter_->write( Units::COUNT, numTCSamples, startTime_ );
    offset += 2;

    acmPathWriter_->writeBlob( deviceResponse_ + offset, 4, startTime_ );
    offset += 8;

    float roll = 0.0001f * ( signed short )( ( ( deviceResponse_[offset] & 0xFF ) << 8 )
                 + ( deviceResponse_[offset + 1] & 0xFF ) );
    rollWriter_->write( Units::RADIAN, roll );
    offset += 2;

    float pitch = 0.0001f * ( signed short )( ( ( deviceResponse_[offset] & 0xFF ) << 8 )
                  + ( deviceResponse_[offset + 1] & 0xFF ) );
    pitchWriter_->write( Units::RADIAN, pitch );
    offset += 2;

    float yaw = 0.0001f * ( signed short )( ( ( deviceResponse_[offset] & 0xFF ) << 8 )
                                            + ( deviceResponse_[offset + 1] & 0xFF ) );
    yawWriter_->write( Units::RADIAN, yaw );
    offset += 2;

    signed short deltaXVelocity = ( signed short )( ( deviceResponse_[offset] & 0xFF ) << 8 )
                                  + ( deviceResponse_[offset + 1] & 0xFF );
    deltaXVelocityWriter_->write( Units::COUNT, deltaXVelocity, startTime_ );
    offset += 2;

    signed short deltaYVelocity = ( signed short )( ( deviceResponse_[offset] & 0xFF ) << 8 )
                                  + ( deviceResponse_[offset + 1] & 0xFF );
    deltaYVelocityWriter_->write( Units::COUNT, deltaYVelocity, startTime_ );
    offset += 2;

    signed short deltaZVelocity = ( signed short )( ( deviceResponse_[offset] & 0xFF ) << 8 )
                                  + ( deviceResponse_[offset + 1] & 0xFF );
    deltaZVelocityWriter_->write( Units::COUNT, deltaZVelocity, startTime_ );
    offset += 2;

    fwdSigStrengthWriter_->writeBlob( deviceResponse_ + offset, 4, startTime_ );
    offset += 8;

    const unsigned short packetNum = ( ( deviceResponse_[offset] & 0xff ) << 8 )
                                     + ( deviceResponse_[offset + 1] & 0xff );
    packetNumWriter_->write( Units::COUNT, packetNum, startTime_ );

    bool gfScanActive = gfScanActiveReader_->asInt( Units::BOOL );
    rollWriter_->setInvalid( gfScanActive );
    pitchWriter_->setInvalid( gfScanActive );
    yawWriter_->setInvalid( gfScanActive );

}

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