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

#include "SpeedControl.h"
#include "SpeedControlIF.h"

#include <limits.h>
#include <stdlib.h>

#include "data/ConfigReader.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalURI.h"
#include "utils/AuvMath.h"
#include "units/Units.h"

SpeedControl::SpeedControl( const Module* module )
    : SyncControlComponent( SpeedControlIF::NAME, module )
{

    logger_.syslog( "Construct SpeedControl." );

    // Slate input settings
    speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD ); //, this, Units::METER_PER_SECOND( 0.0 ) );

    // Configuration settings
    propPitchCfgReader_ = newConfigReader( SpeedControlIF::PROP_PITCH_CFG );

    // Slate input measurements
    speedReader_ = newUniversalReader( UniversalURI::PLATFORM_SPEED_WRT_SEA_WATER ); //, this, Units::METER_PER_SECOND( 0.0 ) );

    // Slate outputs
    propOmegaActionWriter_ = newDataWriter( SpeedControlIF::PROP_OMEGA_ACTION ); //, this, Units::RADIAN_PER_SECOND( 0.0 ) );
}

SpeedControl::~SpeedControl()
{}

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

    ok_ = readConfig();
    if( !ok_ )
    {
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine. Returning.\n" ), Syslog::CRITICAL );
    }
}

/// The actual "payload" of the component
void SpeedControl::run()
{
    if( !ok_ )
    {
        initialize();
        if( !ok_ )
        {
            logger_.syslog( Str( "Error: Error running SpeedControl. Returning.\n" ), Syslog::ERROR );
            return;
        }
    }
    else
    {
        readConfig();
    }

    if( speedCmdReader_->isActive() )
    {
        controlSpeed();
    }
    else
    {
        setSpeed( 0.0 );
    }

}

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

////Private Methods

/// Load SpeedControl parameters
bool SpeedControl::readConfig( void )
{
    // Check if all the parameters are read correctly
    bool ok = true;
    ok &= propPitchCfgReader_->read( Units::METER_PER_RADIAN, propPitch_ );

    propOmegaMargin_ = 0.1;

    return ok;
}

void SpeedControl::controlSpeed()
{
    float speedCmd( nanf( "" ) );
    if( !speedCmdReader_->wasTouchedSinceLastRun( this )
            || !speedCmdReader_->read( Units::METER_PER_SECOND, speedCmd )
            || isnan( speedCmd ) )
    {
        speedCmd = 0;
    }
    setSpeed( speedCmd );
}

void SpeedControl::setSpeed( float speedCmd )
{
    float propOmegaAction( 0.0 );
    if( !isnan( speedCmd ) )
    {
        propOmegaAction = calcPropOmga( speedCmd );
    }
    propOmegaActionWriter_->write( Units::RADIAN_PER_SECOND, propOmegaAction );
}

// returns prop speed in rad/sec. Expects vehicle speed in m/s.
float SpeedControl::calcPropOmga( float speedMetersPerSec )
{
    return ( speedMetersPerSec / propPitch_ );
}