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

#include "WaitDepth.h"
#include "WaitDepthIF.h"

#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"

WaitDepth::WaitDepth( const Str& prefix, const Module* module )
    : Behavior( prefix + WaitDepthIF::NAME, module, true, true ),
      startTime_( Timestamp::NOT_SET_TIME ),
      timeoutDuration_( Timespan::INVALID_TIMESPAN ),
      targetDepth_( nanf( "" ) ),
      depthTol_( nanf( "" ) )
{

    logger_.syslog( "Construct WaitDepth." );

    // Slate input setting variables
    targetDepthSettingReader_ = newSettingReader( WaitDepthIF::TARGET_DEPTH_SETTING );
    depthDeadbandSettingReader_ = newSettingReader( WaitDepthIF::DEPTH_DEADBAND_SETTING );
    timeoutDurationSettingReader_ = newSettingReader( WaitDepthIF::TIMEOUT_SETTING );
    // TODO: make targetDepth/depthDeadbandSettingReader_ optional

    // Slate input measurements
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    // Slate output variables
}

WaitDepth::~WaitDepth()
{}

/// Initialize function
void WaitDepth::initialize( void )
{
    logger_.syslog( "Initialize WaitDepth Component." );

    readSettings();

}

void WaitDepth::run()
{
    runIfUnsatisfied();
}

/// Run until target depth is reached or timer runs
bool WaitDepth::runIfUnsatisfied()
{

    float depth;

    if( startTime_.elapsed() >= timeoutDuration_ )
    {
        logger_.syslog( "Done waiting. Failed to reach target depth within specified timeout.", Syslog::INFO );  // TODO: concider changing to critical
        return true;
    }

    if( readParams( depth ) )
    {
        if( calcSatisfied( depth ) )
        {
            logger_.syslog( "Target depth reached. Done Waiting.", Syslog::INFO );
            return true;
        }
    }
    return false;
}

bool WaitDepth::isSatisfied()
{
    float depth;

    if( !readParams( depth ) )
    {
        return false;
    }

    return calcSatisfied( depth );
}

/// Uninit function
void WaitDepth::uninitialize( void )
{
    logger_.syslog( "Uninitialize Wait Component." );
}

void WaitDepth::readSettings()
{
    // Update target depth
    if( targetDepthSettingReader_->isActive() )
    {
        float newTargetDepth_;
        targetDepthSettingReader_->read( Units::METER, newTargetDepth_ );
        if( !( newTargetDepth_ == targetDepth_ ) )
        {
            targetDepth_ = newTargetDepth_;
            startTime_ = Timestamp::Now();  // Start the timer
        }
    }

    // Update target depth deadband tolerance
    if( depthDeadbandSettingReader_->isActive() )
    {
        depthDeadbandSettingReader_->read( Units::METER, depthTol_ );
    }

    // Update max duration
    if( timeoutDurationSettingReader_->isActive() )
    {
        float newTimeoutSetting( nanf( "" ) );
        timeoutDurationSettingReader_->read( Units::SECOND, newTimeoutSetting );
        if( timeoutDuration_ == Timespan::INVALID_TIMESPAN ||
                durationSeconds_ != newTimeoutSetting )
        {
            durationSeconds_ = newTimeoutSetting;
            timeoutDuration_ = Timespan( durationSeconds_ );
        }
    }
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool WaitDepth::readParams( float& param )
{
    bool ok = false;

    readSettings();

    if( depthReader_->isActive() && depthReader_->wasTouchedSinceLastRun( this ) )
    {
        ok = depthReader_->read( Units::METER, param );
    }

    // Let's read in all the relevant measurements
    return ok;
}

bool WaitDepth::calcSatisfied( const float param )
{
    return ( param >= ( targetDepth_ - depthTol_ ) && param < ( targetDepth_ + depthTol_ ) );
}

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