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

#include "DepthServo.h"
#include "DepthServoIF.h"

#include "bitModule/CBITIF.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 "data/UniversalDataReader.h"
#include "units/Units.h"

DepthServo::DepthServo( const Str& prefix, const Module* module )
    : Behavior( prefix + DepthServoIF::NAME, module, true, true ),
      holdValueSetting_( 0.0 ),
      proportionalGainSetting_( 0.0 ),
      integralGainSetting_( 0.0 ),
      derivativeGainSetting_( 0.0 ),
      changePerMeterSetting_( 1.0 ),
      holdValueIntegral_( 0.0 ),
      lastMeasuredValue_( 0.0 ),
      trajectory_( false ),
      maxDiveRate_( 0.0 ),
      maxBuoyDiveRate_( 0.0 ),
      maxDiveAccel_( 0.0 ),
      maxBuoyDiveAccel_( 0.0 ),
      abortDepth_( 0.0 ),
      speedCmd_( 0.0 ),
      startBelowSetting_( false )
{
    logger_.syslog( "Construct." );

    // Setting readers
    holdValueSettingReader_ = newSettingReader( DepthServoIF::HOLD_VALUE_SETTING );
    inputSettingReader_ = newSettingReader( DepthServoIF::INPUT_SETTING );
    proportionalGainSettingReader_ = newSettingReader( DepthServoIF::PROPORTIONAL_GAIN_SETTING );
    integralGainSettingReader_ = newSettingReader( DepthServoIF::INTEGRAL_GAIN_SETTING );
    derivativeGainSettingReader_ = newSettingReader( DepthServoIF::DERIVATIVE_GAIN_SETTING );
    changePerMeterSettingReader_ = newSettingReader( DepthServoIF::CHANGE_PER_METER_SETTING );

    /// Configuration Readers
    abortDepthCfgReader_ = newConfigReader( CBITIF::ABORT_DEPTH_CFG );                // Depth at which we drop the weight. Should be greater than all depth envelopes

    speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD ); //, this, Units::METER_PER_SECOND( 0.0 ) );

    maxDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_DIVE_RATE_CFG );
    maxBuoyDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_BUOY_DIVE_RATE_CFG );
    maxDiveAccelCfgReader_ = newConfigReader( VerticalControlIF::MAX_DIVE_ACCEL_CFG );
    maxBuoyDiveAccelCfgReader_ = newConfigReader( VerticalControlIF::MAX_BUOY_DIVE_ACCEL_CFG );

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

    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
}

DepthServo::~DepthServo()
{}

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

    holdValueSetting_ = 0.0;
    proportionalGainSetting_ = 0.0;
    integralGainSetting_ = 0.0;
    derivativeGainSetting_ = 0.0;
    changePerMeterSetting_ = 1.0;
    holdValueIntegral_ = 0.0;
    lastMeasuredValue_ = 0.0;

    float measured;

    if( readConfig() && readSettings( measured ) )
    {
        startBelowSetting_ = measured < holdValueSetting_;
        //logger_.syslog( "kp=" + Str( proportionalGainSetting_, 2 ) + ", ki=" + Str( integralGainSetting_, 2 ) + ", kd=" + Str( derivativeGainSetting_, 2 ), Syslog::INFO );
    }
}

bool DepthServo::readConfig()
{
    bool ok = true;
    ok &= maxDiveRateCfgReader_ ->read( Units::NONE, maxDiveRate_ );
    ok &= maxBuoyDiveRateCfgReader_ ->read( Units::NONE, maxBuoyDiveRate_ );
    ok &= maxDiveAccelCfgReader_ ->read( Units::NONE, maxDiveAccel_ );
    ok &= maxBuoyDiveAccelCfgReader_ ->read( Units::NONE, maxBuoyDiveAccel_ );
    return ok;
}

bool DepthServo::readSettings( float& measuredValue )
{
    bool ok = true;
    if( !holdValueSettingReader_->read( Units::NONE, holdValueSetting_ ) )
    {
        ok = false;
        logger_.syslog( "Could not read hold value.", Syslog::CRITICAL );
    }

    if( !inputSettingReader_->read( Units::NONE, measuredValue ) )
    {
        ok = false;
        logger_.syslog( "Could not read input value.", Syslog::CRITICAL );
    }
    // defaults to 0.0
    proportionalGainSettingReader_->read( Units::NONE, proportionalGainSetting_ );
    // defaults to 0.0
    integralGainSettingReader_->read( Units::NONE, integralGainSetting_ );
    // defaults to 0.0
    derivativeGainSettingReader_->read( Units::NONE, derivativeGainSetting_ );
    // defaults to 1.0
    changePerMeterSettingReader_->read( Units::NONE, changePerMeterSetting_ );
    return ok;
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool DepthServo::readParams( )
{
    if( !speedCmdReader_->wasTouchedSinceLastRun( this )
            || !speedCmdReader_->read( Units::METER_PER_SECOND, speedCmd_ ) )
    {
        speedCmd_ = 0.0f;
    }
    return true;
}

/// Perform the satisfied: return true if envelope "satisfied"
bool DepthServo::calcSatisfied( const float measuredValue )
{
    return ( startBelowSetting_ && measuredValue >= holdValueSetting_ )
           || ( !startBelowSetting_ && measuredValue <= holdValueSetting_ );
}

/// Just do the run: ignore the results of the satisfied
void DepthServo::run()
{
    float measuredValue;

    if( readConfig() && readSettings( measuredValue ) && readParams() )
    {
        float dt( dt_.asDouble() );
        if( dt == dt )
        {
            float projectedValue;
            float valueError;

            float measuredValueRate = ( measuredValue - lastMeasuredValue_ ) / dt;
            if( measuredValueRate != measuredValueRate )
            {
                measuredValueRate = 0.0;
            }
            float maxChangeRate( ( speedCmd_ == 0.0f ? maxBuoyDiveRate_ : maxDiveRate_ )
                                 * changePerMeterSetting_ );
            float maxDiveAccel( ( speedCmd_ == 0.0f ? maxBuoyDiveAccel_ : maxDiveAccel_ )
                                * changePerMeterSetting_ );
            projectedValue = trajectory_.project( measuredValue, dt, holdValueSetting_, maxChangeRate, maxDiveAccel );
            valueError = projectedValue - measuredValue;

            //debug
            //printf("kp=%f, ki=%f, kd=%f, value:%f, setting:%f, err:%f projected:%f, rate:%f\n",proportionalGainSetting_,integralGainSetting_,derivativeGainSetting_,measuredValue,holdValueSetting_,valueError, projectedValue, measuredValueRate); //*** DEBUG
            //logger_.syslog( "setting:" + Str( holdValueSetting_, 2 ) + ", value:" + Str( measuredValue ) + ", projected:" + Str( projectedValue ) + ", err:" + Str( valueError ) + ", rate:" + Str( measuredValueRate ), Syslog::INFO );

            if( isnan( holdValueIntegral_ ) )
            {
                holdValueIntegral_ = 0.0f;
            }
            holdValueIntegral_ = holdValueIntegral_ + integralGainSetting_ * valueError * dt;

            // We are going to specify a different depth
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::DEPTH );

            abortDepthCfgReader_->read( Units::METER, abortDepth_ );

            float depth( nanf( "" ) );
            if( ! depthReader_->read( Units::METER, depth ) )
            {
                depth = nanf( "" );
            }

            float depthCmd( depth - ( proportionalGainSetting_ * valueError + AuvMath::NanZero( holdValueIntegral_ ) + derivativeGainSetting_ * measuredValueRate ) );
            depthCmd = AuvMath::Limit( depthCmd, abortDepth_, 0.0f );

            //debug
            //printf( "For valueError=%g, depth=%g, depthCmd=%g\n", valueError, depth, depthCmd );
            //logger_.syslog( "depth=" + Str( depth ) + ", depthCmd=" + Str( depthCmd ) , Syslog::INFO );

            depthCmdWriter_->write( Units::METER, depthCmd );
        }
        lastMeasuredValue_ = measuredValue;
    }
}

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

    float measuredValue;

    if( !( readConfig() && readSettings( measuredValue ) && readParams() ) )
    {
        return false;
    }

    return calcSatisfied( measuredValue );
}

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

    bool retVal( false );
    float measuredValue;

    if( readConfig() && readSettings( measuredValue ) && readParams() )
    {
        if( !calcSatisfied( measuredValue ) )
        {
            run();
            retVal = false;
        }
        else
        {
            lastMeasuredValue_ = measuredValue;
        }
        retVal = true;
    }
    else
    {
        lastMeasuredValue_ = nan( "" );
    }
    return retVal;
}

/// Uninit function
void DepthServo::uninitialize( void )
{
    logger_.syslog( "Uninitialize." );

}

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