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

#include "BackseatDriver.h"
#include "BackseatDriverIF.h"
#include "sensorModule/BackseatComponentIF.h"

#include "data/ConfigReader.h"
#include "units/Units.h"


// Outputs go to Dynamic Control
#include "controlModule/VerticalControlIF.h"

BackseatDriver::BackseatDriver( const Str& prefix, const Module* module )
    : Behavior( prefix + BackseatDriverIF::NAME, module, true, true ),
      lcmBridge_( this, "tethys_slate" ),
      lcmHandleTimeout_( 0.0025 ), // 2.5 milliseconds
      timeoutCfg_( 300 ),
      powerBackseat_( false ),
      initialized_( false ),
      debug_( false )
{
    logger_.syslog( "Construct BackseatDriver." );

    // Slate input setting variables
    channelSettingReader_ = newSettingReader( BackseatDriverIF::LCM_CHANNEL_SETTING );
    sourceIdSettingReader_ = newSettingReader( BackseatDriverIF::SOURCE_ID_SETTING );
    powerBackseatSettingReader_ = newSettingReader( BackseatDriverIF::BACKSEAT_POWER_SETTING );

    // Slate inputs
    //lcmUniversalBroadcastReader_ = newDataReader( DataURI( "LcmUniversalReporter", "enableBroadcast", Units::BOOL ) );
    powerBackseatCompReader_ = newDataReader( BackseatComponentIF::POWER_BACKSEAT_COMP );
    lcmHeartbeatReader_ = newDataReader( BackseatComponentIF::BACKSEAT_HANDLE_MSG );


    // Slate configuration outputs
    lcmListenerTimeoutCfgReader_ = newConfigReader( BackseatComponentIF::LCM_LISTENER_TIMEOUT_CFG );

    // Slate outputs
    lcmHeartbeatWriter_ = newDataWriter( BackseatComponentIF::BACKSEAT_HANDLE_MSG );
}

BackseatDriver::~BackseatDriver()
{}

/// Initialize function
void BackseatDriver::initialize( void )
{
    // Do nothing here. We'll init forreal if backseat power is requested.
}

void BackseatDriver::initComms( void )
{
    logger_.syslog( "Initializing backseat", Syslog::INFO );

    // Power up the backseat comp
    powerBackseatCompReader_->requestData( true );

    // Enable Universal messaging
    //lcmUniversalBroadcastReader_->requestData( true );

    // Allow logging from external source to the backseat component
    lcmBridge_.authorizeComponent( BackseatComponentIF::NAME );
    //lcmBridge_.setQueueCapacity( 30 );

    // Subscribe
    initialized_ = NULL != lcmBridge_.subscribe();
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
void BackseatDriver::readParams()
{
    // Read in setting params
    if( channelSettingReader_->isActive() )
    {
        lcmBridge_.setChannel( channelSettingReader_->asString( Units::NONE ) );
    }
    if( sourceIdSettingReader_->isActive() )
    {
        int sourceID;
        if( sourceIdSettingReader_->read( Units::COUNT, sourceID ) )
        {
            lcmBridge_.setSourceID( ( short )sourceID );
        }
    }
    if( ! powerBackseatSettingReader_->read( Units::BOOL, powerBackseat_ ) )
    {
        powerBackseat_ = false;
    }

    double timeout( nanf( "" ) );
    if( lcmListenerTimeoutCfgReader_->read( Units::SECOND, timeout ) && !isnan( timeout ) )
    {
        timeoutCfg_ = timeout;
    }
}

/// Perform the satisfied: return true if envelope "satisfied"
bool BackseatDriver::calcSatisfied()
{
    return false;
}

/// Just do the run: ignore the results of the satisfied
void BackseatDriver::run()
{
    if( runIfUnsatisfied() )
    {
        // Do nothing, we don't care about the return value, but this should satisfy Coverity
    }
}

/// Just do the satisfied: return true if envelope "satisfied"
bool BackseatDriver::isSatisfied()
{
    return runIfUnsatisfied();
}

/// Do the run, and return true if envelope "satisfied"
bool BackseatDriver::runIfUnsatisfied()
{
    readParams();
    if( !powerBackseat_ )
    {
        // Power was previously requested but is no longer, shut down
        if( initialized_ )
        {
            // Log here, not in uninit, so we don't spam missions that never init the backseat
            logger_.syslog( "Uninitializing backseat", Syslog::INFO );
            uninitialize();
        }
    }
    else if( !initialized_ )
    {
        initComms();
    }
    // Power is requested and connection is initialized, check heartbeat
    else
    {
        startTime_ = Timestamp::Now();

        int state = 1;
        while( state > 0 && startTime_.elapsed() < lcmHandleTimeout_ )
        {

            state = lcmBridge_.handleTimeout( lcmHandleTimeout_ );

            if( state == 0 )
            {
                // Listener timed out.
                if( debug_ ) logger_.syslog( "LCM listener timed out.", Syslog::INFO );
            }
            else if( state > 0 )
            {
                // Listener handeled a msg, reset the clock...
                lcmHeartbeatWriter_->write( Units::BOOL, 1 );
            }
            else
            {
                // LCM handler encountered an error.
                logger_.syslog( "Listener failed to handle LCM message.", Syslog::ERROR );
                return calcSatisfied();
            }
        }
    }

    return calcSatisfied();
}

/// Uninit function
void BackseatDriver::uninitialize( void )
{
    // Power down the backseat comp
    powerBackseatCompReader_->requestData( false );

    // Disable Universal messaging
    //lcmUniversalBroadcastReader_->requestData( false );

    // Unsubscribe
    lcmBridge_.unsubscribe();
    initialized_ = false;
}


/// Mission Component factory interface
Behavior* BackseatDriver::CreateBehavior( const Str& prefix, const Module* module )
{
    return new BackseatDriver( prefix, module );
}