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

#include "PitchServo.h"
#include "PitchServoIF.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"

PitchServo::PitchServo( const Str& prefix, const Module* module )
    : Behavior( prefix + PitchServoIF::NAME, module, true, true ),
      holdValueSetting_( 0.0 ),
      measuredValueIntegral_( 0.0 ),
      lastMeasuredValue_( 0.0 ),
      trajectory_( false ),
      startBelowSetting_( false )
{
    logger_.syslog( "Construct." );

    // Mission setting variables
    holdValueSettingReader_ = newSettingReader( PitchServoIF::HOLD_VALUE_SETTING );
    inputSettingReader_ = newSettingReader( PitchServoIF::INPUT_SETTING );
    proportionalGainSettingReader_ = newSettingReader( PitchServoIF::PROPORTIONAL_GAIN_SETTING );
    integralGainSettingReader_ = newSettingReader( PitchServoIF::INTEGRAL_GAIN_SETTING );
    derivativeGainSettingReader_ = newSettingReader( PitchServoIF::DERIVATIVE_GAIN_SETTING );
    changePerMeterSettingReader_ = newSettingReader( PitchServoIF::CHANGE_PER_METER_SETTING );

    // Config setting readers
    kdDepthCfgReader_ = newConfigReader( VerticalControlIF::KD_DEPTH_CFG );
    kiDepthCfgReader_ = newConfigReader( VerticalControlIF::KI_DEPTH_CFG );
    kpDepthCfgReader_ = newConfigReader( VerticalControlIF::KP_DEPTH_CFG );
    maxDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_DIVE_RATE_CFG );
    maxDiveAccelCfgReader_ = newConfigReader( VerticalControlIF::MAX_DIVE_ACCEL_CFG );
    maxDepthIntCfgReader_ = newConfigReader( VerticalControlIF::MAX_DEPTH_INT_CFG );

    // Data readers
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    // Slate output variables
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    pitchCmdWriter_ = newDataWriter( VerticalControlIF::PITCH_CMD );
}

PitchServo::~PitchServo()
{}

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

    holdValueSetting_ = 0.0;
    measuredValueIntegral_ = 0.0;
    lastMeasuredValue_ = 0.0;

    float measuredValue = NAN;
    readSettings( measuredValue );
    startBelowSetting_ = measuredValue < holdValueSetting_;

}

// Read configuration
void PitchServo::readConfig()
{
    kdDepthCfgReader_->read( Units::RADIAN_PER_SECOND_PER_METER, derivativeGainSetting_ );
    kiDepthCfgReader_->read( Units::RADIAN_PER_SECOND_PER_METER, integralGainSetting_ );
    kpDepthCfgReader_->read( Units::RADIAN_PER_METER, proportionalGainSetting_ );
    maxDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxDiveRate_ );
    maxDiveAccelCfgReader_->read( Units::METER_PER_SECOND_SQUARED, maxDiveAccel_ );
    maxDepthIntCfgReader_->read( Units::RADIAN, maxDepthInt_ );
}

// Read script settings
bool PitchServo::readSettings( float& measuredValue )
{
    bool ok = true;
    if( !holdValueSettingReader_->read( Units::NONE, holdValueSetting_ ) )
    {
        logger_.syslog( "Could not read hold value", Syslog::CRITICAL );
        ok = false;
    }
    if( !inputSettingReader_->read( Units::NONE, measuredValue ) )
    {
        measuredValue = NAN;
    }
    proportionalGainSettingReader_->read( Units::NONE, proportionalGainSetting_ );
    integralGainSettingReader_->read( Units::NONE, integralGainSetting_ );
    derivativeGainSettingReader_->read( Units::NONE, derivativeGainSetting_ );

    float changePerMeterSetting = 1.0;
    changePerMeterSettingReader_->read( Units::NONE, changePerMeterSetting );
    maxChangeRate_ = maxDiveRate_ * changePerMeterSetting;

    return ok;
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool PitchServo::readParams( float& measuredValue )
{
    // Read configuration
    readConfig();

    // Read settings
    bool ok = readSettings( measuredValue );

    if( isnan( measuredValue ) )
    {
        ok &= depthReader_->read( Units::METER, measuredValue );
    }

    return ok;
}

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

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

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

            float holdValueRate = ( measuredValue - lastMeasuredValue_ ) / dt;
            if( holdValueRate != holdValueRate )
            {
                holdValueRate = 0.0;
            }
            projectedValue = trajectory_.project( measuredValue, dt, holdValueSetting_, maxChangeRate_, maxDiveAccel_ );
            valueError = measuredValue - projectedValue;

            if( isnan( measuredValueIntegral_ ) )
            {
                measuredValueIntegral_ = 0.0f;
            }
            measuredValueIntegral_ = AuvMath::Limit( measuredValueIntegral_ + integralGainSetting_ * valueError * dt, -maxDepthInt_, maxDepthInt_ );

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

            float pitchCmd( AuvMath::Limit( proportionalGainSetting_ * valueError + AuvMath::NanZero( measuredValueIntegral_ ) + derivativeGainSetting_ * holdValueRate, -1.0f, 1.0f ) );
            //printf("kp=%f, kd=%f, value:%f, setting:%f, err:%f maxChange:%f, maxAccel:%f, projected:%f, rate:%f, integral:%f, pitch:%f\n",
            //		proportionalGainSetting_,derivativeGainSetting_,measuredValue,holdValueSetting_,valueError, maxChangeRate_, maxDiveAccel_ ,
            //		projectedValue, holdValueRate, measuredValueIntegral_, R2D(pitchCmd)); //*** DEBUG
            pitchCmdWriter_->write( Units::RADIAN, pitchCmd );
        }
        lastMeasuredValue_ = measuredValue;
    }
}

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

    float measuredValue;

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

    return calcSatisfied( measuredValue );
}

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

    bool retVal( false );
    float measuredValue;

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

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

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