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

#include "Radio_Surface.h"
#include "Radio_SurfaceIF.h"

#include "controlModule/VerticalControlIF.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"


const Timespan Radio_Surface::PERIOD( 0.4 );

Radio_Surface::Radio_Surface( const Module* module )
    : AsyncComponent( Radio_SurfaceIF::NAME, module, PERIOD ),
      surfaceThreshold_( 1.0 ), // This should be overwritten by a config value
      loadControl_( Radio_SurfaceIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startTime_( Timestamp::NOT_SET_TIME ),
      powerTime_( Timestamp::NOT_SET_TIME ),
      timeout_( 5.0 ),
      powerTimeout_( 60.0 ),
      resetFailTimeout_( 250 ),
      debug_( false ),
      radioPwr_( Radio_SurfaceIF::RADIO_SURFACE_POWER_OFF )
{

    // Slate inputs
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    verticalModeReader_ = newDataReader( VerticalControlIF::VERTICAL_MODE );

    // Slate outputs
    radioPwrWriter_ = newDataWriter( Radio_SurfaceIF::RADIO_SURFACE_POWER );

    // Configuration
    surfaceThresholdCfgReader_ = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );

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

    this->setAllowableFailures( 5 );
    this->setRetryTimeout( 150 );
}

Radio_Surface::~Radio_Surface()
{
}

void Radio_Surface::run()
{
    //logger_.syslog("Run", Syslog::INFO);
}

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

    // Set the time
    startTime_ = Timestamp::Now();
    if( !simulateHardware() )
    {
        logger_.syslog( "Powering up", Syslog::INFO );
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
    }

    return STARTING;
}

/// Might follow a STOP...START sequence
Component::RunState Radio_Surface::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );
    if( startTime_.elapsed() > timeout_ )
    {
        radioPwr_ = Radio_SurfaceIF::RADIO_SURFACE_POWER_ON;
        writeData();
        return RUNNABLE;
    }

    return STARTING;
}

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

/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState Radio_Surface::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    return STOPPED; // State not used at this time
}

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

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

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

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

    // Check to see if we can turn power off
    if( shouldBeOn() )
    {
        powerTime_ = Timestamp::Now();
    }
    else
    {
        // Keep on for a while longer to avoid unnecessary power recycles on the device.
        if( powerTime_.elapsed() > powerTimeout_ )
        {
            // Power off
            return STOP;
        }
    }

    // Are we working? Well, I guess if there haven't been any failure for awhile we can assume so.
    if( startTime_.elapsed() > resetFailTimeout_ )
    {
        this->resetFailCount();
        startTime_ = Timestamp::Now();
    }

    return RUNNABLE;
}

Component::RunState Radio_Surface::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );
    if( !simulateHardware() )
    {
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.powerDown() )  // First power down then query for faults next cycle
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
    }
    radioPwr_ = Radio_SurfaceIF::RADIO_SURFACE_POWER_OFF;
    writeData();

    return STOPPING;
}

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

    startTime_ = Timestamp::Now(); // Note the time of stopping
    return STOPPED;
}

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

    // Check to see if we should power on following the PO timeout
    if( startTime_.elapsed() > timeout_ )
    {
        if( shouldBeOn() )
        {
            return START;
        }
    }

    return STOPPED;
}

bool Radio_Surface::shouldBeOn( void ) // TODO: This could be a superclass method for radio components.
{
    int verticalMode( 0 );
    float depth( 0.0 );

    if( radioPwrWriter_->isDataRequested() )
    {
        return true;
    }

    // No active depth sensor - turn on.
    if( depthReader_->getOwner()->isFailed() || !depthReader_->isActive() )
    {
        return true;
    }

    // If depth is good and above the surface threshold then turn on. Also, if depth is nan, turn on anyway just in case.
    surfaceThresholdCfgReader_->read( Units::METER, surfaceThreshold_ );
    if( depthReader_->isActive() && depthReader_->read( Units::METER, depth ) && ( ( depth < surfaceThreshold_ ) || isnan( depth ) ) )
    {
        return true;
    }

    // Vehicle is trying to get to the surface (v-mode 1), on the surface (8), or uncommanded (0), turn on.
    if( verticalModeReader_->isActive() && verticalModeReader_->read( Units::ENUM, verticalMode )
            && ( ( verticalMode == 1 ) || ( verticalMode == 8 ) || ( verticalMode == 0 ) ) )
    {
        return true;
    }

    return false;
}

void Radio_Surface::writeData()
{
    radioPwrWriter_->write( Units::ENUM, radioPwr_ );
}

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

bool Radio_Surface::isDataRequested()
{
    return radioPwrWriter_->isDataRequested();
//           || communicationsWriter_->isDataRequested();
}

void Radio_Surface::uninitialize()
{
    // Looks like we're done. Wrap it up.
    //  stop(); // *** DEBUG Keep the radio on after "quit" command
}