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

#include "AltitudeServo.h"
#include "AltitudeServoIF.h"

#include "bitModule/CBITIF.h"
#include "controlModule/VerticalControlIF.h"
#include "data/ConfigReader.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataReader.h"
#include "utils/AuvMath.h"
#include "units/Units.h"

AltitudeServo::AltitudeServo( const Str& prefix, const Module* module )
    : Behavior( prefix + AltitudeServoIF::NAME, module, true, true ),
      targetAltitudeSetting_( nanf( "" ) ),
      altitudeDeviationSetting_( 0.1 ), // meter
      invalidAltitudeTimeoutSetting_( 600 ), // seconds
      invalidAltitudeReported_( false ),
      initDepthSetting_( nanf( "" ) ),
      maxDepthSetting_( nanf( "" ) ),
      depthCmdFilter_( 0.0 ), // decay setting (disable the filter by default)
      depthCmd_( nanf( "" ) ),
      depthDeadband_( nanf( "" ) ),
      doInitDepth_( true )
{
    logger_.syslog( "Construct." );

    // Setting readers
    targetAltitudeSettingReader_ = newSettingReader( AltitudeServoIF::TARGET_ALTITUDE_SETTING );
    altitudeDeviationSettingReader_ = newSettingReader( AltitudeServoIF::ALTITUDE_DEVIATION_SETTING );
    invalidAltitudeTimeoutSettingReader_ = newSettingReader( AltitudeServoIF::INVALID_ALTITUDE_TIMEOUT_SETTING );
    initDepthSettingReader_ = newSettingReader( AltitudeServoIF::INIT_DEPTH_SETTING );
    maxDepthSettingReader_ = newSettingReader( AltitudeServoIF::MAX_DEPTH_LIMIT_SETTING );
    iirFilterDecaySettingReader_ = newSettingReader( AltitudeServoIF::IIR_FILTER_DECAY_SETTING );

    // Configuration Readers
    stopDepthCfgReader_ = newConfigReader( CBITIF::STOP_DEPTH_CFG );
    depthDeadbandConfigReader_ = newConfigReader( VerticalControlIF::DEPTH_DEADBAND_CFG );

    // Slate output variables
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    depthCmdWriter_ = newDataWriter( VerticalControlIF::DEPTH_CMD );

    // Slate input measurements
    altitudeReader_ = newUniversalReader( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR );
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
}

AltitudeServo::~AltitudeServo()
{}

/// Initialize function
void AltitudeServo::initialize( void )
{
    logger_.syslog( "Initialize.", Syslog::INFO );

    targetAltitudeSetting_ = nanf( "" );
    altitudeDeviationSetting_ =  0.1;
    maxDepthSetting_ =  nanf( "" );
    initDepthSetting_ =  nanf( "" );
    depthCmdFilter_.setDecay( 0.0 );
    depthCmdFilter_.init( 0.0 );
    depthCmd_ = nanf( "" );
    doInitDepth_ = true;

    // Set depth command limit to the stop depth.
    // This will get overwritten if the mission specifies maxDepthSetting.
    stopDepthCfgReader_->read( Units::METER, maxDepthSetting_ );

    // Read in depth deadband
    depthDeadbandConfigReader_->read( Units::METER, depthDeadband_ );

    altitudeReader_->requestData( true );
    depthReader_->requestData( true );
    validAltitudeTimer_ = Timestamp::Now(); // Initialize the clock
}

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

/// Just do the satisfied: return true if envelope "satisfied"
bool AltitudeServo::isSatisfied()
{
    bool retVal( false );
    float depth( nanf( "" ) );
    float altitude( nanf( "" ) );

    if( readParams( altitude, depth ) )
    {
        retVal = calcSatisfied( altitude );
    }
    return retVal;
}

/// Do the run, and return true if envelope "satisfied"
bool AltitudeServo::runIfUnsatisfied()
{
    float depth( nanf( "" ) );
    float altitude( nanf( "" ) );

    if( readParams( altitude, depth ) )
    {
        if( ( doInitDepth_ || isnan( depthCmd_ ) ) && !isnan( initDepthSetting_ ) )
        {
            depthCmd_ = initDepthSetting_;

            if( depth >= initDepthSetting_ || fabs( depth - initDepthSetting_ ) <= depthDeadband_
                    || altitude <= targetAltitudeSetting_ || fabs( altitude - targetAltitudeSetting_ ) <= 5.0f )
            {
                logger_.syslog( "Reached init depth of " + Str( depth ) + " m.", Syslog::INFO );
                doInitDepth_ = false;
            }
        }
        else if( !isnan( altitude ) )
        {
            // Compute the next depth command from altitude
            depthCmd_ = depth + altitude - targetAltitudeSetting_;
            // Apply low-pass filter
            depthCmd_ = depthCmdFilter_.filter( depthCmd_ );
        }
    }

    // DEBUG
    // logger_.syslog( "targetAlt:" + Str( targetAltitudeSetting_, 2 ) + ", altitude:" + Str( altitude ) + ", depth:" + Str( depth ) + ", depthCmd:" + Str( depthCmd_ ) + ", maxDepth:" + Str( maxDepthSetting_ ) , Syslog::INFO );

    // Issue the depth commmand
    depthCmd_ = AuvMath::Limit( depthCmd_, 0.0, maxDepthSetting_ );
    verticalModeWriter_->write( Units::ENUM, VerticalControlIF::DEPTH );
    depthCmdWriter_->write( Units::METER, depthCmd_ );

    return calcSatisfied( altitude );
}

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

    if( altitudeDeviationSettingReader_->isActive() )
    {
        float altitudeDeviation( nanf( "" ) );
        if( altitudeDeviationSettingReader_->read( Units::METER, altitudeDeviation )
                && !isnan( altitudeDeviation ) )
        {
            altitudeDeviationSetting_ = altitudeDeviation;
        }
    }

    if( invalidAltitudeTimeoutSettingReader_->isActive() )
    {
        float altitudeTimeout( nanf( "" ) );
        if( invalidAltitudeTimeoutSettingReader_->read( Units::SECOND, altitudeTimeout )
                && !isnan( altitudeTimeout ) )
        {
            invalidAltitudeTimeoutSetting_ = altitudeTimeout;
        }
    }

    if( initDepthSettingReader_->isActive() && doInitDepth_ )
    {
        float initDepth( nanf( "" ) );
        if( initDepthSettingReader_->read( Units::METER, initDepth ) && !isnan( initDepth ) )
        {
            initDepthSetting_ = initDepth;
        }
        else
        {
            doInitDepth_ = false;
        }
    }

    float maxDepth( nanf( "" ) );
    if( maxDepthSettingReader_->isActive()
            && maxDepthSettingReader_->read( Units::METER, maxDepth ) && !isnan( maxDepth ) )
    {
        maxDepthSetting_ = maxDepth;
    }

    float iirDecay( 0.0 );
    if( iirFilterDecaySettingReader_->isActive()
            && iirFilterDecaySettingReader_->read( Units::NONE, iirDecay ) && !isnan( iirDecay ) )
    {
        if( iirDecay != depthCmdFilter_.getDecay() )
        {
            depthCmdFilter_.setDecay( iirDecay );
        }
    }

    if( !targetAltitudeSettingReader_->read( Units::METER, targetAltitudeSetting_ ) || isnan( targetAltitudeSetting_ ) )
    {
        logger_.syslog( "Failed to read valid target altitude setting.", Syslog::CRITICAL );
        ok = false;
    }

    return ok;
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool AltitudeServo::readParams( float& altitude, float& depth )
{
    bool ok( true );
    ok &= readSettings();
    ok &= depthReader_->read( Units::METER, depth ) && !isnan( depth );

    if( altitudeReader_->wasTouchedSinceLastRun( this )
            && altitudeReader_->read( Units::METER, altitude ) && !isnan( altitude ) )
    {
        validAltitudeTimer_ = Timestamp::Now();
        invalidAltitudeReported_ = false;
    }

    return ok;
}


/// Return true if altitude is "satisfied"
bool AltitudeServo::calcSatisfied( const float measuredAltitude )
{
    bool retVal( false );

    retVal = fabs( measuredAltitude - targetAltitudeSetting_ ) <= altitudeDeviationSetting_;

    if( !doInitDepth_ && validAltitudeTimer_.elapsed().asFloat() > invalidAltitudeTimeoutSetting_ )
    {
        if( !invalidAltitudeReported_ )
        {
            logger_.syslog( "Failed to read a valid altitude measurement within specified timeout.", Syslog::CRITICAL );
            invalidAltitudeReported_ = true;
        }

        retVal = true;
    }

    return retVal;
}

/// Uninit function
void AltitudeServo::uninitialize( void )
{
    logger_.syslog( "Uninitialize." );
    altitudeReader_->requestData( false );
    depthReader_->requestData( false );
}

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