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

#include "SlowYo.h"
#include "SlowYoIF.h"

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

SlowYo::SlowYo( const Str& prefix, const Module* module )
    : Behavior( prefix + SlowYoIF::NAME, module, true, true ),
      /// Desired dive angle of the vehicle
      diveAngleSetting_( nan( "" ) ),
      /// Desired speed of the vehicle
      speedSetting_( nan( "" ) ),
      /// Minimum Depth of vehicle
      minDepthSetting_( nan( "" ) ),
      /// Maximum Depth of vehicle
      maxDepthSetting_( nan( "" ) ),
      /// Smoothing factor blending trapezoidal & sinusoidal dive rate profile
      smoothingFactorSetting_( nan( "" ) ),
      /// 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 when we reach the end of a SlowYo period
      satisfied_( false ),
      // True once we run once
      ranOnce_( false ),
      // Depth of start of yo
      startDepth_( nanf( "" ) ),
      reachedStartDepth_( false ),
      /// Command to set verticalMode
      verticalMode_( VerticalControlIF::NONE ),
      /// Command to set pitch
      pitchCmd_( nan( "" ) ),
      /// Command to set depth
      depthCmd_( nan( "" ) ),
      /// Command to set depth rate
      depthRateCmd_( nan( "" ) ),
      slowyogen_(NULL)
{

    logger_.syslog( "Construct SlowYo." );

    // Config settings
    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 ) );
    speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD ); //, this, Units::METER_PER_SECOND( 0.0 ) );

    // Behavior settings
    diveAngleSettingReader_ = newSettingReader( SlowYoIF::DIVE_ANGLE_SETTING );
    speedSettingReader_ = newSettingReader( SlowYoIF::SPEED_SETTING );
    minDepthSettingReader_ = newSettingReader( SlowYoIF::MIN_DEPTH_SETTING );
    maxDepthSettingReader_ = newSettingReader( SlowYoIF::MAX_DEPTH_SETTING );
    smoothingFactorSettingReader_ = newSettingReader( SlowYoIF::SMOOTHING_FACTOR_SETTING );

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

    // Vertical control commands
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    pitchCmdWriter_ = newDataWriter( VerticalControlIF::PITCH_CMD );
    depthCmdWriter_ = newDataWriter( VerticalControlIF::DEPTH_CMD );
    depthRateCmdWriter_ = newDataWriter( VerticalControlIF::DEPTH_RATE_CMD );
    speedCmdWriter_ = newDataWriter( SpeedControlIF::SPEED_CMD ); //, this, Units::METER_PER_SECOND( 0.0 ) );
}

SlowYo::~SlowYo()
{}

/// Initialize function
void SlowYo::initialize( void )
{
    logger_.syslog( "Initialize SlowYoComponent." );

    reachedStartDepth_ = false;
    pitchCmd_ = nan("");
    depthCmd_ = nan("");
    depthRateCmd_ = nan("");

    readSettings();

    if (!isnan(diveAngleSetting_)
        && !isnan(speedSetting_)
        && !isnan(minDepthSetting_)
        && !isnan(maxDepthSetting_)
        && !isnan(smoothingFactorSetting_))
    {
      if (slowyogen_ != NULL)
      {
        delete slowyogen_;
        slowyogen_ = NULL;
      }
      slowyogen_ = new SlowYoGenerator(minDepthSetting_,
                                       maxDepthSetting_,
                                       diveAngleSetting_,
                                       speedSetting_,
                                       smoothingFactorSetting_);
      /*
      std::cout << "slowyo.t:" << std::endl;
      print_vector(slowyogen_->slowyo.t);
      std::cout << "slowyo.depth_rate_profile:" << std::endl;
      print_vector(slowyogen_->slowyo.depth_rate_profile);
      std::cout << "slowyo.depth_profile:" << std::endl;
      print_vector(slowyogen_->slowyo.depth_profile);
      std::cout << "slowyo.pitch_profile:" << std::endl;
      print_vector(slowyogen_->slowyo.pitch_profile);
      */

      std::cout << "slowyo.T=[" << slowyogen_->slowyo.T << "]" << std::endl;

      pitchProfileIterator_ = slowyogen_->slowyo.pitch_profile.begin();
      depthProfileIterator_ = slowyogen_->slowyo.depth_profile.begin();
      depthRateProfileIterator_ = slowyogen_->slowyo.depth_rate_profile.begin();
    }

    if (slowyogen_ != NULL)
    {
      initialized_ = true;
      depthReader_->requestData( true );
      depthRateReader_->requestData( true );
    }

    satisfied_ = reachedEndOfProfile();
    ranOnce_ = false;
}

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

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

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

void SlowYo::readConfig()
{
    maxDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxDiveRate_ );
    maxBuoyDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxBuoyDiveRate_ );
}

void SlowYo::readSettings()
{
    diveAngleSetting_ = nan( "" );
    speedSetting_ = nan( "" );
    minDepthSetting_ = nan( "" );
    maxDepthSetting_ = nan( "" );
    smoothingFactorSetting_ = nan( "" );

    // Read settings
    if( diveAngleSettingReader_->isActive() )
    {
        diveAngleSettingReader_->read( Units::DEGREE, diveAngleSetting_ );
        diveAngleSetting_ = fabs( diveAngleSetting_ );
    }
    if( speedSettingReader_->isActive() )
    {
        speedSettingReader_->read( Units::METER_PER_SECOND, speedSetting_ );
        speedSetting_ = fabs( speedSetting_ );
    }
    if( minDepthSettingReader_->isActive() )
    {
        minDepthSettingReader_->read( Units::METER, minDepthSetting_ );
        minDepthSetting_ = fabs( minDepthSetting_ );
    }
    if( maxDepthSettingReader_->isActive() )
    {
        maxDepthSettingReader_->read( Units::METER, maxDepthSetting_ );
        maxDepthSetting_ = fabs( maxDepthSetting_ );
    }
    if( smoothingFactorSettingReader_->isActive() )
    {
        smoothingFactorSettingReader_->read( Units::PERCENT, smoothingFactorSetting_ );
        smoothingFactorSetting_ = 0.01 * fabs( smoothingFactorSetting_ );
    }
}

bool SlowYo::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 );
    int verticalMode;
    if( verticalModeReader_->isActive()
            && verticalModeReader_->wasTouchedSinceLastRun( this )
            && verticalModeReader_->read( Units::ENUM, verticalMode ) )
    {
        switch( verticalMode )
        {
        case VerticalControlIF::DEPTH:
            depthRateMode = true;
            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:
            // slowyo not set up for pitch
            break;
        case VerticalControlIF::PITCH_RATE:
            // slowyo not set up for pitch rate
            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 )
    {
        if( !measureDepthRate )
        {
            depthRateCmdReader_->read( Units::METER_PER_SECOND, depthRateCmd_ );
        }
        else
        {
            depthRateReader_->read( Units::METER_PER_SECOND, depthRateCmd_ );
        }
    }
    else
    {
        verticalMode_ = VerticalControlIF::NONE;
    }

    if( depthRateMode )
    {
        if (!reachedStartDepth_ && !reachedStartDepth())
        {
            verticalMode_ = VerticalControlIF::DEPTH;
            depthCmd_ = minDepthSetting_ + 0.5;
            // speedCmd_ = speedSetting_;
            pitchCmd_ = nan("");  // 20.0 / 57.29578;
            speedCmd_ = nan("");
            depthRateCmd_ = nan("");
            // std::cout << "move to slowyo start depth: depth [" << startDepth_
            //           << "] < minDepth [" << depthCmd_ << "]" << std::endl;
        }
        else if (!reachedEndOfProfile())
        {
            std::size_t select = 0U;
            if (select == 0U)
            {
                verticalMode_ = VerticalControlIF::DEPTH;
                depthCmd_ = *(depthProfileIterator_++);
                // pitchCmd_ = *(pitchProfileIterator_++);
                pitchCmd_ = nan("");
                // speedCmd_ = speedSetting_;
                speedCmd_ = nan("");
                // depthRateCmd_ = *(depthRateProfileIterator_++);
                depthRateCmd_ = nan("");
                // std::cout << "slowyo: depthCmd_ = " << depthCmd_ << std::endl;
                // std::cout << "slowyo: pitchCmd_ = " << pitchCmd_ << std::endl;
                // std::cout << "slowyo: depthRateCmd_ = " << depthRateCmd_ << std::endl;
            }
            else if (select == 1U)
            {
                if (!reachedStartDepth_)
                {
                    std::cout << "slowyo writing nan depth cmd to clear" << std::endl;
                    depthCmdWriter_->write( Units::METER, nan("") );
                    // speedCmdWriter_->write( Units::METER, nan("") );
                }
                verticalMode_ = VerticalControlIF::DEPTH_RATE;
                depthCmd_ = nan("");
                speedCmd_ = nan("");
                depthRateCmd_ = *(depthRateProfileIterator_++);
                std::cout << "slowyo: depthRateCmd_ = " << depthRateCmd_ << std::endl;
            }
            else if (select == 2U)
            {
                verticalMode_ = VerticalControlIF::PITCH;
                depthCmd_ = nan("");  // *(depthProfileIterator_++);
                pitchCmd_ = *(pitchProfileIterator_++);
                std::cout << "slowyo: pitchCmd_ = " << pitchCmd_ << std::endl;
                // speedCmd_ = speedSetting_;
                speedCmd_ = nan("");
                depthRateCmd_ = nan("");  // *(depthRateProfileIterator_++);
            }
            reachedStartDepth_ = true;
        }
    }
    else
    {
        depthCmd_ = nanf( "" );
        depthRateCmd_ = nanf( "" );
    }

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

    return true;
}

bool SlowYo::reachedStartDepth()
{
  bool got_depth = depthReader_->read( Units::METER, startDepth_ );
  return got_depth
    && startDepth_ >= minDepthSetting_ - 0.25
    && startDepth_ <= minDepthSetting_ + 0.25;
}

bool SlowYo::reachedEndOfProfile()
{
    return slowyogen_ != NULL
           && (depthRateProfileIterator_ == slowyogen_->slowyo.depth_rate_profile.end()
               || depthProfileIterator_ == slowyogen_->slowyo.depth_profile.end()
               || pitchProfileIterator_ == slowyogen_->slowyo.pitch_profile.end());
}

// depthReader_->read( Units::METER, depth )

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

        if( !isnan( depthCmd_ ) )
        {
            // std::cout << "slowyo writing depth cmd" << std::endl;
            depthCmdWriter_->write( Units::METER, depthCmd_ );
        }

        if( !isnan( depthRateCmd_ ) )
        {
            std::cout << "slowyo writing depth rate" << std::endl;
            depthRateCmdWriter_->write( Units::METER_PER_SECOND, depthRateCmd_ );
        }

        if( !isnan( pitchCmd_ ) )
        {
            std::cout << "slowyo writing pitch cmd" << std::endl;
            pitchCmdWriter_->write( Units::RADIAN, pitchCmd_ );
        }

        /*
        if( !isnan( speedCmd_ ) )
        {
            std::cout << "slowyo writing speed cmd" << std::endl;
            speedCmdWriter_->write( Units::METER_PER_SECOND, speedCmd_ );
        }
        */


    }
}

/// Uninit function
void SlowYo::uninitialize( void )
{
    logger_.syslog( "Uninitialize SlowYoComponent." );
    depthReader_->requestData( false );
    depthRateReader_->requestData( false );
    initialized_ = false;
    delete slowyogen_;
    slowyogen_ = NULL;
}

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