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

#include "DepthEnvelope.h"
#include "DepthEnvelopeIF.h"

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

DepthEnvelope::DepthEnvelope( const Str& prefix, const Module* module )
    : Behavior( prefix + DepthEnvelopeIF::NAME, module, true, true ),
      minDepthSetting_( nanf( "" ) ),
      maxDepthSetting_( nanf( "" ) ),
      /// 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( "" ) )
{
    logger_.syslog( "Construct DepthEnvelope." );

    // Config settings
    pitchLimitCfgReader_ = newConfigReader( VerticalControlIF::PITCH_LIMIT_CFG );
    maxDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_DIVE_RATE_CFG );
    maxBuoyDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_BUOY_DIVE_RATE_CFG );
    elevTurnTimeCfgReader_ = newConfigReader( VerticalControlIF::ELEVATOR_TURN_TIME_CFG );
    massTurnTimeCfgReader_ = newConfigReader( VerticalControlIF::MASS_TURN_TIME_CFG );

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

    // Behavior settings
    minDepthSettingReader_ = newSettingReader( DepthEnvelopeIF::MIN_DEPTH_SETTING ); //, ( *baseUnit_ )( minDepthSetting_ ) );
    maxDepthSettingReader_ = newSettingReader( DepthEnvelopeIF::MAX_DEPTH_SETTING ); //, ( *baseUnit_ )( maxDepthSetting_ ) );
    depthSurrogateSettingReader_ = newSettingReader( DepthEnvelopeIF::DEPTH_SURROGATE_SETTING ); //, new StrValue( "depth" ) );
    pitchSettingReader_ = newSettingReader( DepthEnvelopeIF::PITCH_SETTING ); //, Units::RADIAN( upPitchSetting_ ) );
    downPitchSettingReader_ = newSettingReader( DepthEnvelopeIF::DOWN_PITCH_SETTING ); //, Units::RADIAN( downPitchSetting_ ) );
    upPitchSettingReader_ = newSettingReader( DepthEnvelopeIF::UP_PITCH_SETTING ); //, Units::RADIAN( upPitchSetting_ ) );
    depthRateSettingReader_ = newSettingReader( DepthEnvelopeIF::DEPTH_RATE_SETTING ); //, Units::RADIAN( downDepthRateSetting_ ) );
    downDepthRateSettingReader_ = newSettingReader( DepthEnvelopeIF::DOWN_DEPTH_RATE_SETTING ); //, Units::RADIAN( downDepthRateSetting_ ) );
    upDepthRateSettingReader_ = newSettingReader( DepthEnvelopeIF::UP_DEPTH_RATE_SETTING ); //, Units::RADIAN( upDepthRateSetting_ ) );

    // Slate input measurements - defined at initialization
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    depthRateReader_ = newUniversalReader( UniversalURI::DEPTH_RATE ); //, this, Units::METER_PER_SECOND( nanf( "" ) ) );

    // 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 );
}

DepthEnvelope::~DepthEnvelope()
{}

/// Initialize function
void DepthEnvelope::initialize( void )
{
    logger_.syslog( "Initialize DepthEnvelopeComponent." );

    minDepthSetting_ = nanf( "" );
    maxDepthSetting_ = nanf( "" );
    downDepthRateSetting_ = nan( "" );
    upDepthRateSetting_ = nan( "" );
    downPitchSetting_ = nan( "" );
    upPitchSetting_ = nan( "" );

    readSettings();

    if( !depthSurrogateSettingReader_->isActive() )
    {
        depthReader_->requestData( true );
    }
}

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

/// Just do the satisfied: return true if envelope "satisfied"
bool DepthEnvelope::isSatisfied()
{

    float param;

    if( !readParams( param ) )
    {
        return false;
    }

    return calcSatisfied( param );
}

/// Do the run, and return true if envelope "satisfied"
bool DepthEnvelope::runIfUnsatisfied()
{
    //logger_.syslog( "Running DepthEnvelope." );

    float param;
    bool goUp( false );
    bool goDown( false );

    if( readParams( param ) )
    {
        param = projectDepth( param );
        if( !calcSatisfied( param ) )
        {
            // Go down!
            if( ( minDepthSetting_ <= maxDepthSetting_ && param < minDepthSetting_ )
                    || ( minDepthSetting_ > maxDepthSetting_ && param > minDepthSetting_ )
                    || ( isnan( maxDepthSetting_ )  && param < minDepthSetting_ ) )
            {
                // Don't go down if we are using depth and above the surface
                // and the minimum depth is the surface
                if( !depthSurrogateSettingReader_->isActive() && param <= 0 && minDepthSetting_ == 0 )
                {
                    // Do nothing
                }
                else
                {
                    goDown = true;
                }
            }
            // Go up!
            else if( ( minDepthSetting_ <= maxDepthSetting_ && param > maxDepthSetting_ )
                     || ( minDepthSetting_ > maxDepthSetting_ && param < maxDepthSetting_ )
                     || ( isnan( minDepthSetting_ )  && param > maxDepthSetting_ ) )
            {
                goUp = 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 DepthEnvelope::uninitialize( void )
{
    logger_.syslog( "Uninitialize." );
    if( !depthSurrogateSettingReader_->isActive() )
    {
        depthReader_->requestData( false );
    }
}

void DepthEnvelope::readConfig()
{
    pitchLimitCfgReader_->read( Units::RADIAN, upPitchSetting_ );
    downPitchSetting_ = -upPitchSetting_;
    maxDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxDiveRate_ );
    maxBuoyDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxBuoyDiveRate_ );
    elevTurnTimeCfgReader_->read( Units::SECOND, elevatorTurnSeconds_ );
    massTurnTimeCfgReader_->read( Units::SECOND, massTurnSeconds_ );
}

void DepthEnvelope::readSettings()
{
    // Read settings (handle depthSurrogateSettingReader_ separately)

    if( minDepthSettingReader_->isActive()
            || maxDepthSettingReader_->isActive() )
    {
        minDepthSettingReader_->read( Units::NONE, minDepthSetting_ );
        maxDepthSettingReader_->read( Units::NONE, maxDepthSetting_ );
    }

    if( pitchSettingReader_->isActive() )
    {
        pitchSettingReader_->read( Units::RADIAN, upPitchSetting_ );
        upPitchSetting_ = fabs( upPitchSetting_ );
        downPitchSetting_ = -upPitchSetting_;
    }
    if( downPitchSettingReader_->isActive() )
    {
        downPitchSettingReader_->read( Units::RADIAN, downPitchSetting_ );
        downPitchSetting_ = -fabs( downPitchSetting_ );
    }
    if( upPitchSettingReader_->isActive() )
    {
        upPitchSettingReader_->read( Units::RADIAN, upPitchSetting_ );
        upPitchSetting_ = fabs( upPitchSetting_ );
    }
    if( depthRateSettingReader_->isActive() )
    {
        depthRateSettingReader_->read( Units::RADIAN, downDepthRateSetting_ );
        downDepthRateSetting_ = fabs( downDepthRateSetting_ );
        upDepthRateSetting_ = -downDepthRateSetting_;
    }
    if( downDepthRateSettingReader_->isActive() )
    {
        downDepthRateSettingReader_->read( Units::RADIAN, downDepthRateSetting_ );
        downDepthRateSetting_ = fabs( downDepthRateSetting_ );
    }
    if( upDepthRateSettingReader_->isActive() )
    {
        upDepthRateSettingReader_->read( Units::RADIAN, upDepthRateSetting_ );
        upDepthRateSetting_ = -fabs( upDepthRateSetting_ );
    }
}

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

    readConfig();
    readSettings();

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

    if( depthSurrogateSettingReader_->isActive() )
    {
        ok &= depthSurrogateSettingReader_->read( depthSurrogateSettingReader_->getDefaultUnit()->getBaseUnit(), param );
    }
    else
    {
        ok &= depthReader_->read( Units::METER, param );
    }

    // Let's read in all the relevant measurements
    return ok;
}

/// Perform the satisfied: return true if envelope "satisfied"s
bool DepthEnvelope::calcSatisfied( const float param )
{
    if( isnan( minDepthSetting_ ) && !isnan( maxDepthSetting_ ) )
    {
        return ( param <= maxDepthSetting_ );
    }
    else if( !isnan( minDepthSetting_ ) && isnan( maxDepthSetting_ ) )
    {
        return ( param >= minDepthSetting_ );
    }
    else if( isnan( minDepthSetting_ ) && isnan( maxDepthSetting_ ) )
    {
        return false;
    }
    else if( minDepthSetting_ < maxDepthSetting_ )
    {
        return ( param >= minDepthSetting_ && param <= maxDepthSetting_ );
    }
    else if( minDepthSetting_ > maxDepthSetting_ )
    {
        return ( param <= minDepthSetting_ && param >= maxDepthSetting_ );
    }
    else
    {
        return false;
    }
}

float DepthEnvelope::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* DepthEnvelope::CreateBehavior( const Str& prefix, const Module* module )
{
    return new DepthEnvelope( prefix, module );
}

// If usingDepth_ is true, project the depth out to when it can be switched
float DepthEnvelope::projectDepth( float depth )
{
    if( depthSurrogateSettingReader_->isActive() )
    {
        return depth;
    }

    float depthRate( nanf( "" ) );
    if( !depthRateReader_->read( Units::METER_PER_SECOND, depthRate ) )
    {
        return depth;
    }

    float elevatorAngleCmd( nanf( "" ) );
    elevatorAngleCmdReader_->read( Units::RADIAN, elevatorAngleCmd );

    float massPositionCmd( nanf( "" ) );
    massPositionCmdReader_->read( Units::METER, massPositionCmd );

    float turnTime( 0.0f );
    if( isnan( elevatorAngleCmd ) )
    {
        turnTime = elevatorTurnSeconds_;
    }
    else if( isnan( massPositionCmd ) )
    {
        turnTime = massTurnSeconds_;
    }
    else
    {
        turnTime = 0.0f;
    }

    depth += depthRate * turnTime;
    return depth;
}