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

#include "Mass.h"
#include "MassIF.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/MassServoIF.h"
#include "units/Units.h"

Mass::Mass( const Str& prefix, const Module* module )
    : Behavior( prefix + MassIF::NAME, module, true, true ),
      massPositionSetting_( nanf( "" ) ),
      massDeviation_( nanf( "" ) ),
      inactivePositionCount_( 0 )
{

    logger_.syslog( "Construct." );

    // Slate input setting variables
    massPositionSettingReader_ = newSettingReader( MassIF::MASS_POSITION_SETTING );

    // Servo Config
    massDeviationCfgReader_ = newConfigReader( MassServoIF::DEVIATION_DISTANCE );

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

    // Slate output variables
    massPositionCmdWriter_ = newDataWriter( VerticalControlIF::MASS_POSITION_CMD );
}

Mass::~Mass()
{}

/// Initialize function
void Mass::initialize( void )
{
    logger_.syslog( "Initialize." );
    massPositionSetting_ = nanf( "" );
    inactivePositionCount_ = 0;

    massDeviationCfgReader_->read( Units::METER, massDeviation_ );
    readSettings();
}

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

/// Do the run, and return true if position is "satisfied"
bool Mass::runIfUnsatisfied()
{
    bool retVal( false );

    if( readSettings() )
    {
        massPositionCmdWriter_->write( Units::METER, massPositionSetting_ );
        retVal = calcSatisfied();
    }

    return retVal;
}

/// Return true if position is "satisfied"
bool Mass::isSatisfied()
{
    bool retVal( false );

    if( readSettings() )
    {
        retVal = calcSatisfied();
    }

    return retVal;
}

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

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

    if( !massPositionReader_->read( Units::METER, position ) && ( position != position ) )
    {
        if( ++inactivePositionCount_ == 5 )
        {
            logger_.syslog( "Faild to read a valid mass position measurement.", Syslog::ERROR );
            inactivePositionCount_ = 0;
        }
        ok = false;
    }

    return ok;
}

bool Mass::readSettings( )
{
    bool ok( true );

    if( !massPositionSettingReader_->read( Units::METER, massPositionSetting_ )
            && massPositionSetting_ != massPositionSetting_ )
    {
        ok = false;
        logger_.syslog( "Failed to read a valid mass position setting.", Syslog::CRITICAL );
    }

    return ok;
}

bool Mass::calcSatisfied( void )
{
    bool retVal( false );
    float position( nanf( "" ) );

    if( readParams( position ) )
    {
        retVal = fabs( position - massPositionSetting_ ) < massDeviation_;
    }

    return retVal;
}

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