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

#include "Buoyancy.h"
#include "BuoyancyIF.h"
#include "OffshoreEnvelopeIF.h"

#include "controlModule/VerticalControlIF.h"
#include "data/ConfigReader.h"
#include "data/Slate.h"
#include "data/UniversalURI.h"
#include "data/UniversalDataReader.h"
#include "servoModule/BuoyancyServoIF.h"
#include "units/Units.h"

Buoyancy::Buoyancy( const Str& prefix, const Module* module )
    : Behavior( prefix + BuoyancyIF::NAME, module, true, true ),
      positionSetting_( nanf( "" ) ),
      buoyancyDeviation_( nanf( "" ) )
{

    logger_.syslog( "Construct Buoyancy." );

    // Servo Config
    buoyancyDeviationCfgReader_ = newConfigReader( BuoyancyServoIF::DEVIATION_VOLUME_CFG );

    // Slate input setting variables
    positionReader_ = newUniversalReader( UniversalURI::PLATFORM_BUOYANCY_POSITION );

    // Slate input measurements
    positionSettingReader_ = newSettingReader( BuoyancyIF::POSITION_SETTING );

    // Slate output variables
    positionCmdWriter_ = newDataWriter( VerticalControlIF::BUOYANCY_CMD );
}

Buoyancy::~Buoyancy()
{}

/// Initialize function
void Buoyancy::initialize( void )
{
    logger_.syslog( "Initialize Buoyancy Component." );
    positionSetting_ = nanf( "" );
}

void Buoyancy::run()
{
    positionSettingReader_->read( Units::CUBIC_CENTIMETER, positionSetting_ );
    positionCmdWriter_->write( Units::CUBIC_CENTIMETER, positionSetting_ );
}

/// The actual "payload" of the component
bool Buoyancy::runIfUnsatisfied()
{
    run();

    if( !calcSatisfied( ) )
    {
        return false;
    }
    else
    {
        return true;
    }

    return false;
}

bool Buoyancy::isSatisfied()
{
    return calcSatisfied();
}

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

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

bool Buoyancy::calcSatisfied( )
{
    bool retVal = false;
    float position;
    if( positionReader_->read( Units::CUBIC_CENTIMETER, position ) )
    {
        positionSettingReader_->read( Units::CUBIC_CENTIMETER, positionSetting_ );
        if( !isnan( position ) && !isnan( positionSetting_ ) )
        {
            buoyancyDeviationCfgReader_->read( Units::CUBIC_CENTIMETER, buoyancyDeviation_ );
            return fabs( position - positionSetting_ ) < buoyancyDeviation_;
        }
        else
        {
            logger_.syslog( Str( "fabs( position(" ) + position + ") - positionSetting_(" + positionSetting_ + ") ) = " + fabs( position - positionSetting_ ), Syslog::CRITICAL );
        }

        retVal = true;
    }
    else // The position has never been written. This is a critical failure.
    {
        logger_.syslog( "Buoyancy position not being written", Syslog::CRITICAL );
    }

    return retVal;
}