/** \file
 *
 *  Contains the BackseatComponent class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 *
 *  This class simply provides power and a data writer to
 *  allowing a mission to request it
 */

#include "BackseatComponent.h"
#include "BackseatComponentIF.h"
#include "Power24vConverterIF.h"

#include "data/ConfigReader.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "component/ComponentRegistry.h"
#include "logger/Syslog.h"

#include <vector>
#include "data/LcmInstance.h"
#include "LcmMessageReader.h"
#include "LcmMessageWriter.h"

using namespace lrauv_lcm_tools;


const Timespan BackseatComponent::PERIOD( 0.2 );

const Str BackseatComponent::heartbeatLcmChannel_( "tethys_heartbeat" );
const Str BackseatComponent::dataRequestLcmChannel_( "tethys_data_request" );
const Str BackseatComponent::syslogLcmChannel_( "tethys_syslog" );
const Str BackseatComponent::commandLcmChannel_( "bsd_command" );


BackseatComponent::BackseatComponent( const Module* module )
    : AsyncComponent( BackseatComponentIF::NAME, module, PERIOD ),
      slateRequest_( this, dataRequestLcmChannel_, LcmInstance::GetInstance() ),
      syslogBridge_( this, syslogLcmChannel_, LcmInstance::GetInstance() ),
      heartbeatListener_( heartbeatLcmChannel_, LcmInstance::GetInstance() ),
      cmdWriter_( commandLcmChannel_ ),
      loadControl_( BackseatComponentIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      loadControl2_( BackseatComponentIF::LOAD_CONTROL2, !simulateHardware(), logger_, this ),
      hasLoadControl2_( false ),
      shutdownCmd_( Str:: EMPTY_STR ),
      vitalsTimeout_( 300 ), // Time in seconds till the BackseatComponent WDT cycles power on the device
      poTimeout_( 18 ),
      listenerTimeoutSec_( 2.5 ),
      startTime_( Timestamp::NOT_SET_TIME ),
      shutdownRequested_( false ),
      keepOn_( false ),
      verbosity_( 0 ),
      debug_( false )
{
    // Slate inputs
    msgHandleReader_ = newDataReader( BackseatComponentIF::BACKSEAT_HANDLE_MSG );
    power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER );

    // Slate outputs
    powerBackseatWriter_ = newDataWriter( BackseatComponentIF::POWER_BACKSEAT_COMP );
    msgHandleWriter_ = newDataWriter( BackseatComponentIF::BACKSEAT_HANDLE_MSG );

    // Configuration inputs
    lcmListenerTimeoutCfgReader_ = newConfigReader( BackseatComponentIF::LCM_LISTENER_TIMEOUT_CFG );
    shutdownCmdCfgReader_ = newConfigReader( BackseatComponentIF::BACKSEAT_SHUTDOWN_CMD_CFG );
    verbosityCfgReader_ = newConfigReader( BackseatComponentIF::VERBOSITY_CFG );
    alwaysOnCfgReader_ = newConfigReader( BackseatComponentIF::ALWAYS_ON_CFG );
    needs24vCfgReader_ = newConfigReader( BackseatComponentIF::NEEDS_24V_CFG );

    // Check secondary power supply configuration
    StrValue loadCtrl2;
    if( Slate::ReadOnce( BackseatComponentIF::LOAD_CONTROL2, loadCtrl2, logger_ ) )
    {
        if( !loadCtrl2.asString().endsWith( "null" ) )
        {
            logger_.syslog( "Found secondary power supply at: " + loadCtrl2.asString(), Syslog::INFO );
            hasLoadControl2_ = true;
        }
    }

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

    if( ! this->configSetFailureMissionCritical() )
    {
        this->setFailureMissionCritical( false );
    }
    this->setAllowableFailures( 5 );
    this->setRetryTimeout( 150 );
}

BackseatComponent::~BackseatComponent()
{
}

bool BackseatComponent::readConfig( void )
{
    bool ok = true;
    ok &= lcmListenerTimeoutCfgReader_->read( listenerTimeoutSec_ );
    ok &= shutdownCmdCfgReader_->read( shutdownCmd_ );
    ok &= verbosityCfgReader_->read( Units::COUNT, verbosity_ );
    ok &= alwaysOnCfgReader_->read( keepOn_ );
    ok &= needs24vCfgReader_->read( needs24v_ );
    return ok;
}

/// Subscribe handler to LCM channel
void BackseatComponent::subscribe()
{
    logger_.syslog( "Subscribing to LCM channels.", Syslog::INFO );
    heartbeatListener_.subscribe();
    slateRequest_.subscribe();
    syslogBridge_.subscribe();
}

/// Unsubscribe the LCM message handler
void BackseatComponent::unsubscribe()
{
    logger_.syslog( "Unsubscribing from LCM channels.", Syslog::INFO );
    heartbeatListener_.unsubscribe();
    slateRequest_.unsubscribe();
    syslogBridge_.unsubscribe();
}

/// Do what needs to be done to run
Component::RunState BackseatComponent::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

    if( !simulateHardware() )
    {
        logger_.syslog( "Powering up", Syslog::INFO );
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        if( hasLoadControl2_ )
        {
            logger_.syslog( "Powering up secondary power supply.", Syslog::INFO );
            Timespan::Milliseconds( 15 ).sleepFor();
            if( !loadControl2_.powerUp() )
            {
                logger_.syslog( "Failed to power up secondary load control board.", Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
        }
    }

    // Start the PO timer
    startTime_ = Timestamp::Now();
    if( !readConfig() )
    {
        logger_.syslog( "Failed to load configuration parameters in initialization routine.", Syslog::CRITICAL );
        this->setFailure( FailureMode::DATA );
        return START;
    }

    // Request 24V power
    if( needs24v_ )
        power24vConverterDataReader_->requestData( true );

    // Subscribe handler to LCM channel
    if( LcmInstance::IsValid() )
    {
        subscribe();
    }
    else
    {
        logger_.syslog( "Failed to obtain a valid LCM instance.", Syslog::FAULT );
        this->setFailure( FailureMode::SOFTWARE );
        return STOP;
    }

    return STARTING;
}

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

    // Allow time for the BSD to boot up
    if( startTime_.elapsed() > poTimeout_ )
    {
        shutdownRequested_ = false;
        msgHandleWriter_->write( Units::BOOL, 0 );
        startTime_ = Timestamp::Now();
        return RUNNABLE;
    }
    return STARTING;
}

/// Pause for a short period (indicated by pauseTime)
Component::RunState BackseatComponent::pause()
{
    // State not used at this time. Just stop.
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    return STOP;
}

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

Component::RunState BackseatComponent::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    return STOPPED; // State not used at this time
}

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

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

    if( !simulateHardware() )
    {
        // Log voltage and current and check for any faults
        logVoltageAndCurrent();
    }

    int state = -1;
    if( LcmInstance::IsValid() )
    {
        // Listen for incoming LCM messages. Block until message arrives or timeout is expired.
        state = LcmInstance::GetInstance()->handleTimeout( ( int )listenerTimeoutSec_.asMillis() );
        //state = slateBridge_.handleTimeout();

        if( state == 0 )
        {
            // Listener timed out.
            if( verbosity_ > 0 ) logger_.syslog( "LCM listener timed out.", Syslog::INFO );
        }
        else if( state > 0 )
        {
            // Listener handeled a msg, reset the clock...
            msgHandleWriter_->write( Units::BOOL, 1 );
            this->resetFailCount(); // ...and the fail count
        }
        else
        {
            // LCM handler encountered an error.
            logger_.syslog( "Listener failed to handle LCM message.", Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
        }
    }

    // Vitals check. Look at the msg handle reader — that will allow other compnents
    // (e.g., BackseatDriver behavior) to reset the timer.
    if( msgHandleReader_->getTimestamp().elapsed() > vitalsTimeout_ )
    {
        logger_.syslog( "Failed to receive device response within the specified timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    if( !isDataRequested() )
    {
        // Service is no longer required
        return STOP;
    }

    return RUNNABLE;
}

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

    // Send request to terminate the BSD application and power down
    if( !shutdownRequested_ )
    {
        startTime_ = Timestamp::Now();
        requestShutdown();
        logger_.syslog( "Shutdown requested. Waiting (" + Str( poTimeout_.asDouble(), 2 ) + " sec to power down.).", Syslog::INFO );
    }

    // Allow time for device to shutdown
    if( startTime_.elapsed() > poTimeout_ )
    {
        // 24V power is no longer needed
        if( needs24v_ )
            power24vConverterDataReader_->requestData( false );

        if( !simulateHardware() )
        {
            logger_.syslog( "Powering down and starting shutdown timer (" + Str( poTimeout_.asDouble(), 2 ) + " sec).", Syslog::INFO );
            // First power down then query for faults next cycle
            if( !loadControl_.powerDown() )
            {
                logger_.syslog( "Failed to power down", Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
            if( hasLoadControl2_ )
            {
                logger_.syslog( "Powering down secondary power supply.", Syslog::INFO );
                if( !loadControl2_.powerDown() )  // First power down then query for faults next cycle
                {
                    logger_.syslog( "Failed to power down secondary power supply.", Syslog::FAULT );
                    this->setFailure( FailureMode::HARDWARE );
                }
            }
        }

        // Unsubscribe the LCM message handler
        unsubscribe();

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

    return STOP;
}

Component::RunState BackseatComponent::stopping()
{
    if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO );

    // Allow time for device capacitance to drain and fully power down
    if( startTime_.elapsed() < poTimeout_ )
    {
        return STOPPING;
    }

    if( !simulateHardware() )
    {
        // Check for LCB errors
        loadControl_.readFaults();
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        if( hasLoadControl2_ )
        {
            loadControl2_.readFaults();
            if( loadControl2_.hasError() )
            {
                logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
        }
    }

    return STOPPED;
}

Component::RunState BackseatComponent::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );
    if( isDataRequested() )
    {
        return start();
    }

    return STOPPED;
}

/// Send shutdown request to the BSD on LCM channel
void BackseatComponent::requestShutdown()
{
    if( publishCommand( shutdownCmd_ ) )
    {
        logger_.syslog( "Sent LCM shutdown request.", Syslog::INFO );
        shutdownRequested_ = true;
    }
}

/// Publish command to the BSD on LCM channel
bool BackseatComponent::publishCommand( Str& cmd )
{
    // Update command and publish the message to LCM
    if( cmdWriter_.writeStr( "bsd_command", cmd ) && cmdWriter_.publish( Timestamp::Now().asMillis() ) )
    {
        return true;
    }

    // Faild. Report and return false
    logger_.syslog( "Failed to publish LCM command to the BSD.", Syslog::ERROR );
    this->setFailure( FailureMode::DATA );

    return false;
}

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

    if( hasLoadControl2_ )
    {
        loadControl2_.requestVoltageAndCurrent();
        if( loadControl2_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl2_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            stop();
        }
    }
}

/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI BackseatComponent::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_SIMULATE_HARDWARE:
        return BackseatComponentIF::SIMULATE_HARDWARE;
    case CONFIG_MISSION_CRITICAL:
        return BackseatComponentIF::MISSION_CRITICAL;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}

bool BackseatComponent::isDataRequested()
{
    alwaysOnCfgReader_->read( keepOn_ );
    return ( keepOn_ || powerBackseatWriter_->isDataRequested() );
}

BackseatWriter::BackseatWriter( const Str& channelName )
    : channel_( channelName )
{}

bool BackseatWriter::writeStr( const Str& varName, const Str& str )
{
    return writeStr( varName, str.cStr() );
}

bool BackseatWriter::writeStr( const Str& varName, const char *str )
{
    // String dimensions and type
    DataType type = msg_.getType( varName.cStr() );

    if( type == Unknown )
    {
        // Add array for this variable if not found
        Dim dim( 0, 0 );
        if( msg_.addArray( String, varName.cStr(), varName.cStr(), "string", dim ) )
        {
            type = String;
        }
    }

    if( type == String )
    {
        if( msg_.set( varName.cStr(), str ) )
        {
            return true;
        }
    }

    return false;
}

bool BackseatWriter::writeBool( const Str& varName, bool val )
{
    DataType type = msg_.getType( varName.cStr() );

    if( type == Unknown )
    {
        Dim dim( 0, 0 );
        // Add array for this variable if not found
        if( msg_.addArray( Byte, varName.cStr(), varName.cStr(), "bool", dim ) )
        {
            type = Byte;
        }
    }

    if( type == Byte )
    {
        if( msg_.assign( varName.cStr(), ( const unsigned char * )&val, 1 ) )
        {
            return true;
        }
    }

    return false;
}

bool BackseatWriter::publish( long long epoch_millisec )
{
    if( LcmInstance::IsValid() )
    {
        return msg_.publish( *LcmInstance::GetInstance(), channel_.cStr(), epoch_millisec );
    }

    // Invalid LCM instance.
    return false;
}