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

#include "BioacousticsDataBridge.h"
#include "BioacousticsDataBridgeIF.h"
#include "data/UniversalDataWriter.h"
#include "data/SlateBridge.h"
#include "sensorModule/BackseatComponentIF.h"

#include "units/Units.h"

const StrValue BioacousticsDataBridge::NO_VALUE( "" );

BioacousticsDataBridge::BioacousticsDataBridge( const Module* module )
    : SyncSensorComponent( BioacousticsDataBridgeIF::NAME, module ),
      backseatAltitudeDataReader_( NULL ),
      altitudeTimeout_( ALTITUDE_TIMEOUT_SEC ),
      lcmHeartbeatTimeout_( LCM_HEARTBEAT_TIMEOUT_SEC ),
      debug_( false )
{
    this->setAllowableFailures( 5 );
    this->setRetryTimeout( 150 );

    lcmHeartbeatReader_ = newDataReader( BackseatComponentIF::BACKSEAT_HANDLE_MSG );
    altitudeWriter_ = newUniversalWriter( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR, Units::METER, ALTITUDE_ACCURACY );

    setRunState( START );
}

BioacousticsDataBridge::~BioacousticsDataBridge()
{}

/// Do what needs to be done to run
Component::RunState BioacousticsDataBridge::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );
    //Don't bother trying to read altitude unless the backseat is publishing data
    if( !lcmHeartbeatReader_->isActive() || lcmHeartbeatReader_->getTimestamp().elapsed() > lcmHeartbeatTimeout_ )
    {
        return START;
    }

    // Reader created on previous run, all set!
    if( backseatAltitudeDataReader_ != NULL )
    {
        return RUNNABLE;
    }

    // Wait until we see altitude specifically published
    unsigned short code = Slate::GetElementURICode( "_.height_above_sea_floor" );
    if( code == ElementURI::NO_CODE )
    {
        return START;
    }

    ElementURI* elementURI = Slate::GetElementURI( code );
    if( elementURI == NULL )
    {
        logger_.syslog( "No URI found for code", Syslog::FAULT );
        this->setFailure( FailureMode::DATA );
        return STOP;
    }
    backseatAltitudeDataReader_ = Slate::NewReader( *elementURI, this, &BioacousticsDataBridge::NO_VALUE, false );
    return RUNNABLE;
}

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

/// Pause for a short period (indicated by pauseTime)
Component::RunState BioacousticsDataBridge::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 BioacousticsDataBridge::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    return STOP;
}


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


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


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

    if( backseatAltitudeDataReader_ == NULL )
    {
        logger_.syslog( "Altitude data reader is null", Syslog::FAULT );
        this->setFailure( FailureMode::DATA );
        return STOP;
    }

    if( !lcmHeartbeatReader_->isActive() || lcmHeartbeatReader_->getTimestamp().elapsed() > lcmHeartbeatTimeout_ )
    {
        logger_.syslog( "Backseat LCM timeout, waiting for message to resume" );
        return START;
    }

    if( backseatAltitudeDataReader_->read( Units::METER, altitude_ ) && !isnan( altitude_ ) )
    {
        Timestamp readTs = backseatAltitudeDataReader_->getTimestamp();
        if( lastAltitudeTs_ < readTs )
        {
            lastAltitudeTs_ = readTs;
            altitudeWriter_->write( Units::METER, altitude_ );
            altitudeWriter_->setInvalid( false );
        }
        else if( lastAltitudeTs_.elapsed() > altitudeTimeout_ )
        {
            altitudeWriter_->setInvalid( true );
        }

        this->resetFailCount();
    }
    else
    {
        logger_.syslog( "No valid data from altitude reader, stopping", Syslog::INFO );
        altitudeWriter_->setInvalid( true );
        return STOP;
    }

    return RUNNABLE;
}

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

    return STOPPING;
}


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

    return STOPPED;
}


Component::RunState BioacousticsDataBridge::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );
    if( backseatAltitudeDataReader_ != NULL
            && backseatAltitudeDataReader_->read( Units::METER, altitude_ )
            && !isnan( altitude_ ) )
    {
        // Note: this first valid altitude will be overwritten without publishing
        logger_.syslog( "Received valid altitude data, resuming", Syslog::INFO );
        return RUNNABLE;
    }
    else
    {
        return STOPPED;
    }
}


/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI BioacousticsDataBridge::getConfigURI( ConfigOption configOption ) const
{
    return ConfigURI::NO_CONFIG_URI;
}