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

#include "AltitudeEnvelope.h"
#include "AltitudeEnvelopeIF.h"

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

AltitudeEnvelope::AltitudeEnvelope( const Str& prefix,  const Module* module )
    : Behavior( prefix + AltitudeEnvelopeIF::NAME, module, true, true ),
      minAltitude_( nanf( "" ) ),
      maxAltitude_( nanf( "" ) ),
      maxDepthIgnore_( 0 ),
      /// Desired down depth rate of the vehicle
      downDepthRateSetting_( nan( "" ) ),
      /// Desired up depth rate of the vehicle
      upDepthRateSetting_( nan( "" ) ),
      /// Desired down pitch of the vehicle
      downPitchSetting_( nan( "" ) ),
      /// Desired up pitch of the vehicle
      upPitchSetting_( nan( "" ) ),
      /// Maximum dive rate of the vehicle
      maxDiveRate_( nan( "" ) ),
      /// Maximum dive rate of the vehicle under only buoyancy control
      maxBuoyDiveRate_( nan( "" ) ),

      inactiveAltitudeCount_( 0 )
{

    logger_.syslog( "Construct AltitudeEnvelope." );

    // Config settings
    pitchLimitCfgReader_ = newConfigReader( VerticalControlIF::PITCH_LIMIT_CFG );
    maxDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_DIVE_RATE_CFG );
    maxBuoyDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_BUOY_DIVE_RATE_CFG );
    readConfig();

    // Current control commands
    verticalModeReader_ = newDataReader( VerticalControlIF::VERTICAL_MODE ); //, this, Units::ENUM( VerticalControlIF::NONE ) );
    speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD ); //, this, Units::METER_PER_SECOND( 0.0 ) );

    // Behavior settings
    minAltitudeSettingReader_ = newSettingReader( AltitudeEnvelopeIF::MIN_ALTITUDE_SETTING );
    maxAltitudeSettingReader_ = newSettingReader( AltitudeEnvelopeIF::MAX_ALTITUDE_SETTING );
    maxDepthIgnoreSettingReader_ = newSettingReader( AltitudeEnvelopeIF::MAX_DEPTH_IGNORE_SETTING );
    pitchSettingReader_ = newSettingReader( AltitudeEnvelopeIF::PITCH_SETTING );
    downPitchSettingReader_ = newSettingReader( AltitudeEnvelopeIF::DOWN_PITCH_SETTING );
    upPitchSettingReader_ = newSettingReader( AltitudeEnvelopeIF::UP_PITCH_SETTING );
    depthRateSettingReader_ = newSettingReader( AltitudeEnvelopeIF::DEPTH_RATE_SETTING );
    downDepthRateSettingReader_ = newSettingReader( AltitudeEnvelopeIF::DOWN_DEPTH_RATE_SETTING );
    upDepthRateSettingReader_ = newSettingReader( AltitudeEnvelopeIF::UP_DEPTH_RATE_SETTING );

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

    // Slate output variables
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    elevatorAngleCmdWriter_ = newDataWriter( VerticalControlIF::ELEVATOR_ANGLE_CMD );
    massPositionCmdWriter_ = newDataWriter( VerticalControlIF::MASS_POSITION_CMD );
    depthRateCmdWriter_ = newDataWriter( VerticalControlIF::DEPTH_RATE_CMD );
    pitchCmdWriter_ = newDataWriter( VerticalControlIF::PITCH_CMD );
}

AltitudeEnvelope::~AltitudeEnvelope()
{}

/// Initialize function
void AltitudeEnvelope::initialize( void )
{
    logger_.syslog( "Initialize AltitudeEnvelopeComponent." );
    minAltitude_ = nanf( "" );
    maxAltitude_ = nanf( "" );
    maxDepthIgnore_ = 0;
    downDepthRateSetting_ = nan( "" );
    upDepthRateSetting_ = nan( "" );
    downPitchSetting_ = nan( "" );
    upPitchSetting_ = nan( "" );
    maxDiveRate_ = nan( "" );
    maxBuoyDiveRate_ = nan( "" );
    readSettings( true );
    altitudeReader_->requestData( true );
    depthReader_->requestData( true );
}

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

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

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

/// Do the run, and return true if envelope "satisfied"
bool AltitudeEnvelope::runIfUnsatisfied()
{
    float altitude, depth;
    bool goUp( false );
    bool goDown( false );

    if( readParams( altitude, depth ) )
    {
        if( !calcSatisfied( altitude, depth ) )
        {
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::PITCH );
            if( altitude < minAltitude_ )
            {
                float depth( nanf( "" ) );

                if( depthReader_->read( Units::METER, depth ) && depth > 0 && depth > maxDepthIgnore_ )
                {
                    // Go up!
                    goUp = true;
                }
                else
                {
                    // Don't try to porpoise.
                }
            }
            else if( altitude > maxAltitude_ )
            {
                // Go down!
                goDown = true;
            }

            if( !goUp && !goDown )
            {
                return true;
            }

            bool depthRateMode( false );
            bool pitchMode( false );

            if( goUp )
            {
                elevatorAngleCmdWriter_->write( Units::RADIAN, nanf( "" ) );
                massPositionCmdWriter_->write( Units::METER, nanf( "" ) );

                if( depthRateSettingReader_->isActive() || upDepthRateSettingReader_->isActive() )
                {
                    depthRateMode = true;
                }
                else if( pitchSettingReader_->isActive() || upPitchSettingReader_->isActive() )
                {
                    pitchMode = true;
                }
            }
            else
            {
                if( depthRateSettingReader_->isActive() || downDepthRateSettingReader_->isActive() )
                {
                    depthRateMode = true;
                }
                else if( pitchSettingReader_->isActive() || downPitchSettingReader_->isActive() )
                {
                    pitchMode = true;
                }
            }

            if( speedCmd_ == 0.0f )
            {
                depthRateMode = true;
            }

            if( !depthRateMode && !pitchMode )
            {
                int verticalMode;
                if( verticalModeReader_->read( Units::ENUM, verticalMode ) )
                {
                    switch( verticalMode )
                    {
                    case VerticalControlIF::DEPTH:
                    case VerticalControlIF::DEPTH_RATE:
                    case VerticalControlIF::FLOAT_ON_SURFACE:
                    case VerticalControlIF::MASS_AND_ELEVATOR:
                    case VerticalControlIF::NONE:
                        depthRateMode = true;
                        break;
                    case VerticalControlIF::PITCH:
                    case VerticalControlIF::PITCH_RATE:
                    case VerticalControlIF::PITCH_ZERO:
                        pitchMode = true;
                        break;
                    case VerticalControlIF::SURFACE:
                        depthRateMode = true;
                        break;
                    }
                }
            }

            if( depthRateMode )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::DEPTH_RATE );
                if( goDown )
                {
                    depthRateCmdWriter_->write( Units::METER_PER_SECOND, adjustDepthRate( downDepthRateSetting_, true ) );
                    if( pitchSettingReader_->isActive() || downPitchSettingReader_->isActive() )
                    {
                        pitchCmdWriter_->write( Units::RADIAN, downPitchSetting_ );
                    }
                }
                else if( goUp )
                {
                    depthRateCmdWriter_->write( Units::METER_PER_SECOND, adjustDepthRate( upDepthRateSetting_, false ) );
                    if( pitchSettingReader_->isActive() || upPitchSettingReader_->isActive() )
                    {
                        pitchCmdWriter_->write( Units::RADIAN, upPitchSetting_ );
                    }
                }
            }
            else if( pitchMode )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::PITCH );
                if( goDown )
                {
                    pitchCmdWriter_->write( Units::RADIAN, downPitchSetting_ );
                }
                else if( goUp )
                {
                    pitchCmdWriter_->write( Units::RADIAN, upPitchSetting_ );
                }
            }
            return false;
        }
        return true;
    }
    return false;
}

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

void AltitudeEnvelope::readConfig()
{
    pitchLimitCfgReader_->read( Units::RADIAN, upPitchSetting_ );
    downPitchSetting_ = -upPitchSetting_;
    maxDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxDiveRate_ );
    maxBuoyDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxBuoyDiveRate_ );
}

void AltitudeEnvelope::readSettings( bool force )
{
    // Read settings
    if( force || minAltitudeSettingReader_->isActive() )
    {
        minAltitudeSettingReader_->read( Units::METER, minAltitude_ );
    }
    if( force || maxAltitudeSettingReader_->isActive() )
    {
        maxAltitudeSettingReader_->read( Units::METER, maxAltitude_ );
    }
    if( force || maxDepthIgnoreSettingReader_->isActive() )
    {
        maxDepthIgnoreSettingReader_->read( Units::METER, maxDepthIgnore_ );
    }
    if( force || pitchSettingReader_->isActive() )
    {
        pitchSettingReader_->read( Units::RADIAN, upPitchSetting_ );
        upPitchSetting_ = fabs( upPitchSetting_ );
        downPitchSetting_ = -upPitchSetting_;
    }
    if( force || downPitchSettingReader_->isActive() )
    {
        downPitchSettingReader_->read( Units::RADIAN, downPitchSetting_ );
        downPitchSetting_ = -fabs( downPitchSetting_ );
    }
    if( force || upPitchSettingReader_->isActive() )
    {
        upPitchSettingReader_->read( Units::RADIAN, upPitchSetting_ );
        upPitchSetting_ = fabs( upPitchSetting_ );
    }

    if( force || depthRateSettingReader_->isActive() )
    {
        depthRateSettingReader_->read( Units::RADIAN, downDepthRateSetting_ );
        downDepthRateSetting_ = fabs( downDepthRateSetting_ );
        upDepthRateSetting_ = -downDepthRateSetting_;
    }
    if( force || downDepthRateSettingReader_->isActive() )
    {
        downDepthRateSettingReader_->read( Units::RADIAN, downDepthRateSetting_ );
        downDepthRateSetting_ = fabs( downDepthRateSetting_ );
    }
    if( force || upDepthRateSettingReader_->isActive() )
    {
        upDepthRateSettingReader_->read( Units::RADIAN, upDepthRateSetting_ );
        upDepthRateSetting_ = -fabs( upDepthRateSetting_ );
    }
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool AltitudeEnvelope::readParams( float& altitude, float& depth )
{
    readConfig();
    readSettings( false );
    bool retVal = true; // hope for the best.

    if( !speedCmdReader_->wasTouchedSinceLastRun( this ) ||
            !speedCmdReader_->read( Units::METER_PER_SECOND, speedCmd_ ) )
    {
        speedCmd_ = 0.0f;
    }

    if( !altitudeReader_->isActive() )
    {
        ++inactiveAltitudeCount_;
        if( 5 ==  inactiveAltitudeCount_ )
        {
            logger_.syslog( "Altitude Measurement is not Active.", Syslog::ERROR );
        }
        return false;
    }

    // Return true if a successful read of valid data.
    retVal &= ( ( !altitudeReader_->isInvalid() ) && ( altitudeReader_->read( Units::METER, altitude ) ) && ( altitude == altitude ) );
    retVal &= ( ( !depthReader_->isInvalid() ) && ( depthReader_->read( Units::METER, depth ) ) && ( depth == depth ) );
    return retVal;
}

/// Perform the satisfied: return true if envelope "satisfied"
bool AltitudeEnvelope::calcSatisfied( const float& altitude, const float& depth )
{
    return ( ( isnan( minAltitude_ ) || ( altitude >= minAltitude_ || depth < maxDepthIgnore_ ) )
             && ( isnan( maxAltitude_ ) || altitude <= maxAltitude_ ) );
}

float AltitudeEnvelope::adjustDepthRate( float depthRate, bool goDown )
{
    if( !isnan( depthRate ) )
    {
        return depthRate;
    }
    if( speedCmd_ > 0 )
    {
        return ( goDown ? 1.0f : -1.0f ) * maxDiveRate_ * speedCmd_ / SpeedControlIF::SPEED_FAST;
    }
    return ( goDown ? 1.0f : -1.0f ) * maxBuoyDiveRate_;
}

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