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

#include "DepthKeller33X.h"
#include "DepthKeller33XIF.h"

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"


#define KELLER33X_ACCURACY_PRESSURE (0.1) // in DECIBAR, for PAA33X 100BAR sensor w/ 0.01%FS calibration
#define KELLER33X_ACCURACY_DEPTH (0.02)

/*
 * NOTES:
 * This driver reads pressure measurements from a Keller 33X pressure sensor integrated with a PIC adapter board.
 * The adapter board polls the Keller 33X and streams pressure data in bar units at 15 Hz.
 *
 * Pressure is outputted in Paro format: *0001%f\r\n
 *
*/

const unsigned int DepthKeller33X::MAX_DEVICE_MSG_QUEUE_SIZE( 20 );

const unsigned int DepthKeller33X::MIN_DEVICE_RESPONSE( 13 );

DepthKeller33X::DepthKeller33X( const Module* module )
    : SyncSensorComponent( DepthKeller33XIF::NAME, module ),
      loadControl_( DepthKeller33XIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      uart_( DepthKeller33XIF::UART, DepthKeller33XIF::BAUD, 0.01, logger_ ),
      poTimeout_( 1.5 ), // Measured startup time for Daniel's board (sec).
      timeout_( 3.0 ), // Valid data from the unit should be expected within this time (sec).
      startTime_( Timestamp::NOT_SET_TIME ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      pressureOffset_( 0 ),
      maxPressBound_( 499 ),
      minPressBound_( -9 ),
      pressure_( nanf( "" ) ),
      latitude_( nanf( "" ) ),
      depth_( nanf( "" ) ),
      debug_( false )
{
    // data readers
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE ); // used in converting pressure to depth

    // data writers
    depthWriter_ = newUniversalWriter( UniversalURI::DEPTH, Units::METER, KELLER33X_ACCURACY_DEPTH );
    pressureWriter_ = newUniversalWriter( UniversalURI::SEA_WATER_PRESSURE, Units::DECIBAR, KELLER33X_ACCURACY_PRESSURE );

    // config readers
    offsetCfgReader_ = newConfigReader( DepthKeller33XIF::OFFSET_CFG );
    maxPressBoundCfgReader_ = newConfigReader( DepthKeller33XIF::MAX_PRESS_BOUND_CFG );
    minPressBoundCfgReader_ = newConfigReader( DepthKeller33XIF::MIN_PRESS_BOUND_CFG );

    setAllowableFailures( 5 );
    setRetryTimeout( 30 );

    setRunState( START );
}

DepthKeller33X::~DepthKeller33X()
{}

bool DepthKeller33X::readConfig()
{
    bool ok( true );
    ok &= offsetCfgReader_->read( Units::DECIBAR, pressureOffset_ );
    ok &= maxPressBoundCfgReader_->read( Units::DECIBAR, maxPressBound_ );
    ok &= minPressBoundCfgReader_->read( Units::DECIBAR, minPressBound_ );
    return ok;
}

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

    if( !simulateHardware() )
    {
        deviceResponse_[0] = '\0';

        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Load controller failed to power up.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return START;
        }

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

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

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

    // Allow time for the system to come up
    if( startTime_.elapsed() > poTimeout_ )
    {
        readConfig();

        if( !simulateHardware() )
        {
            // Look for data on the serial port
            uart_.flushCRLF().readLine( deviceResponse_, sizeof( deviceResponse_ ) );
            if( uart_.hasError() )
            {
                logger_.syslog( Str( "Failed to initialize." ), Syslog::FAULT );
                this->setFailure( FailureMode::COMMUNICATIONS );
                return STOP;
            }
            uart_.flush();
        }

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

    return STARTING;
}

/// Pause for a short period
Component::RunState DepthKeller33X::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    return STOP;
}

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

Component::RunState DepthKeller33X::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    return START;
}

Component::RunState DepthKeller33X::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    return START;
}

Component::RunState DepthKeller33X::runnable()
{
    pressure_ = nanf( "" );
    latitude_ = nanf( "" );
    depth_ = nanf( "" );
    readConfig();

    if( !simulateHardware() )
        logVoltageAndCurrent();

    readLatitude();

    // Read and process the latest pressure measurement
    if( readPressure() && processPressure() )
    {
        if( processDepth() )
        {
            // Publish depth data
            depthWriter_->write( Units::METER, depth_, dataTimestamp_ );
        }

        // Publish pressure data
        pressureWriter_->write( Units::DECIBAR, pressure_, dataTimestamp_ );
        startTime_ = Timestamp::Now();
        this->resetFailCount();
    }

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

    return RUNNABLE;
}

Component::RunState DepthKeller33X::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );
    // Power down and disable the uart.
    uninitialize();

    return STOPPING;
}

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

    }

    return STOPPED;
}

Component::RunState DepthKeller33X::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );

    // Power cycle complete. Back to work.
    return START;
}

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

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

void DepthKeller33X::readLatitude()
{
    bool gotLat = latitudeReader_->isActive() && latitudeReader_->read( Units::RADIAN, latitude_ );

    if( gotLat && isnan( latitude_ ) )
    {
        logger_.syslog( "Failed to read valid latitude. Using worksite latitude.", Syslog::ERROR );
        gotLat = false;
    }
    if( !gotLat )
    {
        Slate::ReadOnce( "Config/workSite", "initLat", Units::RADIAN, latitude_, logger_ );
    }
}

bool DepthKeller33X::readPressure()
{
    // Take care of simulated case.
    if( simulateHardware() )
    {
        if( SimSlate::Read( SimSlate::DEPTH_METER, depth_ ) )
        {
            if( !isnan( latitude_ ) )
            {
                // Calc pressure in pascals from depth in meters and latitude in radians
                pressure_ = AuvMath::OceanPressure( depth_, latitude_ );
                pressure_ /= 1e5; // pascals to bar
                dataTimestamp_ = Timestamp::Now();
            }
        }

        return !isnan( pressure_ );
    }

    /* Running the hardware */

    bool processPacket( false );
    unsigned int readCount( 0 );

    // Cycle through the data available on the serial port and get to the most recent packet
    while( uart_.dataAvailable() && ( readCount < MAX_DEVICE_MSG_QUEUE_SIZE ) )
    {
        deviceResponse_[0] = '\0';
        // Look for packet termintation
        if( uart_.readUntil( deviceResponse_, MAX_DEVICE_RESPONSE, "\r\n", 2 ).flushCRLF().hasError() )
        {
            if( uart_.getError() != UartStream::TIMEOUT )
            {
                logger_.syslog( "UART error: ", uart_.errorString(), Syslog::ERROR );
                if( uart_.bytesRead() > 0 && debug_ )
                {
                    logger_.syslog( "Received: " + Str( deviceResponse_ ) + " (" + Str( ( int )uart_.bytesRead() ) + " bytes).", Syslog::INFO );
                }
            }
            processPacket = false;
            break;
        }
        else
        {
            // Got device response
            processPacket = readCount < MAX_DEVICE_MSG_QUEUE_SIZE;
        }

        ++readCount;
    }

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

        if( uart_.bytesRead() >= MIN_DEVICE_RESPONSE )
        {
            // Remove trailing LF
            deviceResponse_[uart_.bytesRead() - 1] = '\0';
            if( debug_ ) logger_.syslog( "Read " + Str( ( int )uart_.bytesRead() ) + " bytes (skip=" + Str( ( int )readCount ) + "): " + Str( deviceResponse_ ), Syslog::INFO );
            return true;
        }
    }

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

bool DepthKeller33X::processPressure()
{
    // Parse out pressure (incoming pressure in bar units)
    if( !simulateHardware() && 1 != sscanf( deviceResponse_, "*0001%lf", &pressure_ ) )
    {
        logger_.syslog( "Failed to parse pressure: " + Str( deviceResponse_ ), Syslog::ERROR );
        pressureWriter_->setInvalid( true );
        return false;
    }

    // Convert bar to decibar and apply offset
    pressure_ *= 10;
    pressure_ += pressureOffset_;

    if( ( pressure_ > minPressBound_ ) && ( pressure_ < maxPressBound_ ) )
    {
        pressureWriter_->setInvalid( false );
        return true;
    }

    logger_.syslog( "Pressure reading out of range: " + Str( pressure_ ) + " decibar.", Syslog::ERROR );
    pressureWriter_->setInvalid( true );
    return false;
}

bool DepthKeller33X::processDepth()
{
    if( !isnan( latitude_ ) )
    {
        // Get depth in meters from pressure in pascals and latitude in radians
        double pressurePascal = pressure_ * 10000.0;  // decibar to pascal
        depth_ = AuvMath::OceanDepth( pressurePascal, latitude_ );

        if( !isnan( depth_ ) )
        {
            depthWriter_->setInvalid( false );
            return true;
        }
    }

    depthWriter_->setInvalid( true );
    return false;
}

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

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