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

#include "PrepareToDive.h"
#include "PrepareToDiveIF.h"

#include "controlModule/VerticalControlIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "controlModule/SpeedControlIF.h"
#include "data/ConfigReader.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "servoModule/MassServoIF.h"
#include "units/Units.h"

// Outputs go to Dynamic Control
// Get my interface names back
#include "PrepareToDiveIF.h"

PrepareToDive::PrepareToDive( const Str& prefix, const Module* module )
    : Behavior( prefix + PrepareToDiveIF::NAME, module, true, true )
{

    logger_.syslog( "Construct PrepareToDive." );

    // Configuration
    massDefaultConfigReader_ = newConfigReader( VerticalControlIF::MASS_DEFAULT_CFG );
    massDeviationCfgReader_ = newConfigReader( MassServoIF::DEVIATION_DISTANCE );

    // Slate input measurements
    massPositionReader_ = newUniversalReader( UniversalURI::PLATFORM_MASS_POSITION );

    // Slate output variables
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    massPositionCmdWriter_ = newDataWriter( VerticalControlIF::MASS_POSITION_CMD );
    speedCmdWriter_ = newDataWriter( SpeedControlIF::SPEED_CMD );
}

PrepareToDive::~PrepareToDive()
{}


// Initialize function
void PrepareToDive::initialize( void )
{
    logger_.syslog( "Initialize PrepareToDiveComponent." );
    ok_ = readConfig();
    if( !ok_ )
    {
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine. Returning.\n" ), Syslog::ERROR );
    }
}


// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool PrepareToDive::readParams( float& massPosition )
{

    // Let's read in all the relevant measurements
    return massPositionReader_->read( Units::METER, massPosition ) & readConfig();
}


// Load parameters
bool PrepareToDive::readConfig( void )
{
    // Check if all the parameters are read correctly
    bool ok = true;
    ok &= massDefaultConfigReader_->read( Units::METER, massDefault_ );
    ok &= massDeviationCfgReader_->read( Units::METER, massDeviation_ );

    return ok;
}


// Perform the satisfied: return true if envelope "satisfied"
bool PrepareToDive::calcSatisfied( const float massPosition )
{
    return ( massPosition <= massDefault_ + massDeviation_ );
}


// Just do the run: ignore the results of the satisfied
void PrepareToDive::run()
{
    runIfUnsatisfied();
}


// Just do the satisfied: return true if envelope "satisfied"
bool PrepareToDive::isSatisfied()
{
    float depth;

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


// Do the run, and return true if envelope "satisfied"
bool PrepareToDive::runIfUnsatisfied()
{
    bool retVal = false;
    float massPosition;

    if( readParams( massPosition ) )
    {
        if( !calcSatisfied( massPosition ) )
        {
            // Shift the mass!
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::MASS_AND_ELEVATOR );
            massPositionCmdWriter_->write( Units::METER, massDefault_ - massDeviation_ );
            // Make sure we are not moving
            speedCmdWriter_->write( Units::METER_PER_SECOND, 0.0 );
        }
        else
        {
            retVal = true;
        }
    }
    else
    {
        logger_.syslog( "Could not read mass position in PrepareToDive", Syslog::ERROR );
        this->setFailure( FailureMode::SOFTWARE );
        retVal = true;
    }
    return retVal;
}


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


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