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

#include "YoYo.h"
#include "YoYoIF.h"

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

YoYo::YoYo( const Str& prefix, const Module* module )
    : Behavior( prefix + YoYoIF::NAME, module, true, true ),
      /// Desired down pitch of the vehicle
      downPitchSetting_( nan( "" ) ),
      /// Desired up pitch of the vehicle
      upPitchSetting_( nan( "" ) ),
      /// Desired down depth rate of the vehicle
      downDepthRateSetting_( nan( "" ) ),
      /// Desired up depth rate of the vehicle
      upDepthRateSetting_( nan( "" ) ),
      /// Minimum depth change required to enable sequence behavior to be satisfied
      satisfiedThresholdSetting_( 3.0 ),
      /// Maximum dive rate of the vehicle
      maxDiveRate_( nan( "" ) ),
      /// Maximum dive rate of the vehicle under only buoyancy control
      maxBuoyDiveRate_( nan( "" ) ),
      /// Indicates initialization success
      initialized_( false ),
      /// True if going down.
      goingDown_( true ),
      // True when we inflect
      satisfied_( false ),
      // True once we run once
      ranOnce_( false ),
      // Depth of start of yo
      startDepth_( nanf( "" ) ),
      /// Command to set verticalMode
      verticalMode_( VerticalControlIF::NONE ),
      /// Command to set pitch
      pitchCmd_( nan( "" ) ),
      /// Command to set depth rate
      depthRateCmd_( nan( "" ) )
{

    logger_.syslog( "Construct YoYo." );

    // 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 ) );
    depthRateCmdReader_ = newDataReader( VerticalControlIF::DEPTH_RATE_CMD ); //, this, Units::METER_PER_SECOND( 0.0 ) );
    pitchCmdReader_ = newDataReader( VerticalControlIF::PITCH_CMD ); //, this, Units::RADIAN( 0.0 ) );
    speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD ); //, this, Units::METER_PER_SECOND( 0.0 ) );

    // Behavior settings
    pitchSettingReader_ = newSettingReader( YoYoIF::PITCH_SETTING );
    downPitchSettingReader_ = newSettingReader( YoYoIF::DOWN_PITCH_SETTING );
    upPitchSettingReader_ = newSettingReader( YoYoIF::UP_PITCH_SETTING );
    depthRateSettingReader_ = newSettingReader( YoYoIF::DEPTH_RATE_SETTING );
    downDepthRateSettingReader_ = newSettingReader( YoYoIF::DOWN_DEPTH_RATE_SETTING );
    upDepthRateSettingReader_ = newSettingReader( YoYoIF::UP_DEPTH_RATE_SETTING );
    satisfiedThresholdSettingReader_ = newSettingReader( YoYoIF::SATISFIED_THRESHOLD_SETTING );

    // Measurements
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    depthRateReader_ = newUniversalReader( UniversalURI::DEPTH_RATE );
    pitchReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE );

    // Vertical control commands
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    depthRateCmdWriter_ = newDataWriter( VerticalControlIF::DEPTH_RATE_CMD );
    pitchCmdWriter_ = newDataWriter( VerticalControlIF::PITCH_CMD );
}

YoYo::~YoYo()
{}

/// Initialize function
void YoYo::initialize( void )
{
    logger_.syslog( "Initialize YoYoComponent." );

    downPitchSetting_ = nan( "" );
    upPitchSetting_ = nan( "" );
    downDepthRateSetting_ = nan( "" );
    upDepthRateSetting_ = nan( "" );
    satisfiedThresholdSetting_ = 3.0;

    readSettings();

    initialized_ = true;

    satisfied_ = false;

    goingDown_ = true;

    ranOnce_ = false;

    depthReader_->requestData( true );
    depthRateReader_->requestData( true );
    pitchReader_->requestData( true );

}

bool YoYo::isSatisfied()
{
    return calculate() && calcSatisfied();
}

void YoYo::run()
{
    if( calculate() )
    {
        writeCmds();
    }
}

bool YoYo::runIfUnsatisfied()
{
    if( calculate() )
    {
        if( satisfied_ )
        {
            logger_.syslog( "Reached end of yo", Syslog::INFO );
        }
        writeCmds();
    }
    return satisfied_;
}

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

void YoYo::readSettings()
{
    downPitchSetting_ = nan( "" );
    upPitchSetting_ = nan( "" );
    downDepthRateSetting_ = nan( "" );
    upDepthRateSetting_ = nan( "" );
    satisfiedThresholdSetting_ = 3.0;

    // Read settings
    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::METER_PER_SECOND, downDepthRateSetting_ );
        downDepthRateSetting_ = fabs( downDepthRateSetting_ );
        upDepthRateSetting_ = -downDepthRateSetting_;
    }
    if( downDepthRateSettingReader_->isActive() )
    {
        downDepthRateSettingReader_->read( Units::METER_PER_SECOND, downDepthRateSetting_ );
        downDepthRateSetting_ = fabs( downDepthRateSetting_ );
    }
    if( upDepthRateSettingReader_->isActive() )
    {
        upDepthRateSettingReader_->read( Units::METER_PER_SECOND, upDepthRateSetting_ );
        upDepthRateSetting_ = -fabs( upDepthRateSetting_ );
    }
    if( satisfiedThresholdSettingReader_->isActive() )
    {
        satisfiedThresholdSettingReader_->read( Units::METER, satisfiedThresholdSetting_ );
        satisfiedThresholdSetting_ = -fabs( satisfiedThresholdSetting_ );
    }
}

bool YoYo::calculate()
{
    if( !initialized_ )
    {
        return false;
    }

    readConfig();
    readSettings();

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

    bool depthRateMode( false );
    bool measureDepthRate( false );
    bool pitchMode( false );
    int verticalMode;
    if( verticalModeReader_->isActive()
            && verticalModeReader_->wasTouchedSinceLastRun( this )
            && verticalModeReader_->read( Units::ENUM, verticalMode ) )
    {
        switch( verticalMode )
        {
        case VerticalControlIF::DEPTH:
            // Should not be yo-yoing in this mode
            break;
        case VerticalControlIF::DEPTH_RATE:
            depthRateMode = true;
            break;
        case VerticalControlIF::FLOAT_ON_SURFACE:
            // Should not be yo-yoing in this mode
            break;
        case VerticalControlIF::MASS_AND_ELEVATOR:
            depthRateMode = true;
            break;
        case VerticalControlIF::NONE:
            depthRateMode = true;
            measureDepthRate = true;
            break;
        case VerticalControlIF::PITCH:
            pitchMode = true;
            break;
        case VerticalControlIF::PITCH_RATE:
            pitchMode = true;
            break;
        case VerticalControlIF::PITCH_ZERO:
            // Should not be yo-yoing in this mode
            break;
        case VerticalControlIF::SURFACE:
            // Should not be yo-yoing in this mode
            break;
        }
    }
    else
    {
        depthRateMode = true;
        measureDepthRate = true;
    }

    if( depthRateMode )
    {
        verticalMode_ = VerticalControlIF::DEPTH_RATE;
        if( !measureDepthRate )
        {
            depthRateCmdReader_->read( Units::METER_PER_SECOND, depthRateCmd_ );
        }
        else
        {
            depthRateReader_->read( Units::METER_PER_SECOND, depthRateCmd_ );
        }
        pitchCmd_ = nanf( "" );
        setGoingDown( depthRateCmd_ > 0 );
    }
    else if( pitchMode )
    {
        verticalMode_ = VerticalControlIF::PITCH;
        pitchCmdReader_->read( Units::METER_PER_SECOND, pitchCmd_ );
        setGoingDown( pitchCmd_ < 0 );
    }
    else
    {
        verticalMode_ = VerticalControlIF::NONE;
    }

    if( goingDown_ )
    {
        if( depthRateMode )
        {
            depthRateCmd_ = adjustDepthRate( downDepthRateSetting_, true );
            if( pitchSettingReader_->isActive() || downPitchSettingReader_->isActive() )
            {
                pitchCmd_ = downPitchSetting_;
            }
        }
        else if( pitchMode )
        {
            pitchCmd_ = downPitchSetting_;
        }
        else
        {
            depthRateCmd_ = pitchCmd_ = nanf( "" );
        }
    }
    else
    {
        if( depthRateMode )
        {
            depthRateCmd_ = adjustDepthRate( upDepthRateSetting_, false );
            if( pitchSettingReader_->isActive() || upPitchSettingReader_->isActive() )
            {
                pitchCmd_ = upPitchSetting_;
            }
        }
        else if( pitchMode )
        {
            pitchCmd_ = upPitchSetting_;
        }
        else
        {
            depthRateCmd_ = pitchCmd_ = nanf( "" );
        }
    }

    if( !ranOnce_ )
    {
        ranOnce_ = depthReader_->read( Units::METER, startDepth_ );
    }

    return true;
}

float YoYo::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_;
}

void YoYo::setGoingDown( bool goingDown )
{
    float depth;
    depthReader_->read( Units::METER, depth );
    satisfied_ = goingDown_ != goingDown
                 && ranOnce_
                 && depthReader_->read( Units::METER, depth )
                 && ( goingDown_ ? depth > startDepth_ + satisfiedThresholdSetting_
                      : depth  < startDepth_ - satisfiedThresholdSetting_ );
    //if( goingDown_ != goingDown && ranOnce_ ) printf( "satisfied_=%d,goingDown=%d,goingDown_=%d,depth=%g,startDepth_=%g,satisfiedThresholdSetting_=%g\n",
    //        satisfied_,goingDown,goingDown_,depth,startDepth_,satisfiedThresholdSetting_);
    goingDown_ = goingDown;
}

bool YoYo::calcSatisfied()
{
    return satisfied_;
}
///
void YoYo::writeCmds()
{
    if( verticalMode_ != VerticalControlIF::NONE )
    {
        verticalModeWriter_->write( Units::ENUM, verticalMode_ );

        if( !isnan( pitchCmd_ ) )
        {
            pitchCmdWriter_->write( Units::RADIAN, pitchCmd_ );
        }

        if( !isnan( depthRateCmd_ ) )
        {
            depthRateCmdWriter_->write( Units::RADIAN, depthRateCmd_ );
        }
    }
}

/// Uninit function
void YoYo::uninitialize( void )
{
    logger_.syslog( "Uninitialize YoYoComponent." );
    depthReader_->requestData( false );
    depthRateReader_->requestData( false );
    pitchReader_->requestData( false );
    initialized_ = false;
}

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