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

#include "SetSpeed.h"
#include "SetSpeedIF.h"

#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalURI.h"
#include "controlModule/LoopControlIF.h"
#include "controlModule/SpeedControlIF.h"
#include "units/Units.h"

SetSpeed::SetSpeed( const Str& prefix, const Module* module )
    : Behavior( prefix + SetSpeedIF::NAME, module, true, true ),
      speedSetting_( nan( "" ) ),
      periodSetting_( nan( "" ) )
{
    logger_.syslog( "Construct." );

    // Slate input setting variables
    speedSettingReader_ = newSettingReader( SetSpeedIF::SPEED_SETTING );
    periodSettingReader_ = newSettingReader( SetSpeedIF::PERIOD_SETTING );

    // Slate input measurements
    speedReader_ = newUniversalReader( UniversalURI::PLATFORM_SPEED_WRT_SEA_WATER );

    // Slate output variables
    speedCmdWriter_ = newDataWriter( SpeedControlIF::SPEED_CMD );
    periodCmdWriter_ = newDataWriter( LoopControlIF::PERIOD_CMD );

}

SetSpeed::~SetSpeed()
{}

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

    speedSetting_ = nan( "" );
    periodSetting_ = nan( "" );

    // Read in relevant settings
    readSettings();
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool SetSpeed::readParams( float& speed )
{
    if( isnan( speedSetting_ ) || NULL == speedReader_ || !speedReader_->isActive() )
    {
        logger_.syslog( "Speed measurement is not Active.", Syslog::ERROR );
        return false;
    }

    // Read in vehicle water speed
    return speedReader_->read( Units::METER_PER_SECOND, speed );
}

void SetSpeed::readSettings( void )
{
    // Read in speed and period settings
    speedSettingReader_->read( Units::METER_PER_SECOND, speedSetting_ );
    periodSettingReader_->read( Units::SECOND, periodSetting_ );
}

/// Perform the satisfied: return true if envelope "satisfied"
bool SetSpeed::calcSatisfied( const float speed )
{
    if( isnan( speedSetting_ ) || isnan( speed ) )
        return false;

    float deltaSpeed = fabs( speedSetting_ - speed );
    return ( deltaSpeed < 0.05 );
}

/// Just do the run: ignore the results of the satisfied
void SetSpeed::run()
{
    // Check for setting updates
    readSettings();

    if( !isnan( speedSetting_ ) )
    {
        speedCmdWriter_->write( Units::METER_PER_SECOND, speedSetting_ );
    }
    if( !isnan( periodSetting_ ) )
    {
        periodCmdWriter_->write( Units::SECOND, periodSetting_ );
    }
}

/// Just do the satisfied: return true if envelope "satisfied"
bool SetSpeed::isSatisfied()
{
    float holdValue( nanf( "" ) );

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

    return calcSatisfied( holdValue );
}

/// Do the run, and return true if envelope "satisfied"
bool SetSpeed::runIfUnsatisfied()
{
    //logger_.syslog( "Running." );
    float speed;

    if( readParams( speed ) )
    {
        if( !calcSatisfied( speed ) )
        {
            run();
            return false;
        }
    }
    return true;
}

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

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