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

#include "HorizontalControl.h"
#include "HorizontalControlIF.h"

#include <limits.h>
#include <stdlib.h>

#include "data/ConfigReader.h"
#include "data/UniversalURI.h"
#include "data/Location.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "data/StrValue.h"

HorizontalControl::HorizontalControl( const Module* module )
    : SyncControlComponent( HorizontalControlIF::NAME, module ),
      headingIntegral_( 0.0 ),
      xteIntegral_( 0.0 ),
      headingTrajectory_( true )
{

    logger_.syslog( "Construct HorizontalControl." );

    // Slate input settings
    horizontalModeReader_ = newDataReader( HorizontalControlIF::HORIZONTAL_MODE );
    latitudeCmdReader_ = newDataReader( HorizontalControlIF::LATITUDE_CMD );
    longitudeCmdReader_ = newDataReader( HorizontalControlIF::LONGITUDE_CMD );
    headingCmdReader_ = newDataReader( HorizontalControlIF::HEADING_CMD );
    headingRateCmdReader_ = newDataReader( HorizontalControlIF::HEADING_RATE_CMD );
    rudderAngleCmdReader_ = newDataReader( HorizontalControlIF::RUDDER_ANGLE_CMD );
    bearingCmdReader_ = newDataReader( HorizontalControlIF::BEARING_CMD );

    kdHeadingOverrideReader_ = newDataReader( HorizontalControlIF::KD_HEADING_OVERRIDE );
    kiHeadingOverrideReader_ = newDataReader( HorizontalControlIF::KI_HEADING_OVERRIDE );
    kpHeadingOverrideReader_ = newDataReader( HorizontalControlIF::KP_HEADING_OVERRIDE );

    // Configuration readers
    kdHeadingCfgReader_ = newConfigReader( HorizontalControlIF::KD_HEADING_CFG );      // Derivative gain
    kiHeadingCfgReader_ = newConfigReader( HorizontalControlIF::KI_HEADING_CFG );      // Integral gain
    kpHeadingCfgReader_ = newConfigReader( HorizontalControlIF::KP_HEADING_CFG );      // Proportional gain
    kwpHeadingCfgReader_ = newConfigReader( HorizontalControlIF::KWP_HEADING_CFG );    // Cross-track error gain
    kiwpHeadingCfgReader_ = newConfigReader( HorizontalControlIF::KIWP_HEADING_CFG );  // Cross-track integral error gain
    maxHdgAccelCfgReader_ = newConfigReader( HorizontalControlIF::MAX_HDG_ACCEL_CFG ); // Max turn accel
    maxHdgIntCfgReader_ = newConfigReader( HorizontalControlIF::MAX_HDG_INT_CFG );     // Max cmded rudder from hdg int
    maxHdgRateCfgReader_ = newConfigReader( HorizontalControlIF::MAX_HDG_RATE_CFG );   // Max turn rate
    maxKxteCfgReader_ = newConfigReader( HorizontalControlIF::MAX_KXTE_CFG );          // Max heading correction due to kwpHeading
    rudDeadbandCfgReader_ = newConfigReader( HorizontalControlIF::RUD_DEADBAND_CFG );  // Degree of rounding in output values
    rudLimitCfgReader_ = newConfigReader( HorizontalControlIF::RUD_LIMIT_CFG );        // Max rudder angle

    // Slate input measurements
    headingReader_ = newUniversalReader( UniversalURI::PLATFORM_ORIENTATION );
    headingRateReader_ = newUniversalReader( UniversalURI::PLATFORM_YAW_RATE );
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    rudderAngleReader_ = newUniversalReader( UniversalURI::PLATFORM_RUDDER_ANGLE );

    // Internal slate outputs
    headingCmdInternalWriter_ = newDataWriter( HorizontalControlIF::HEADING_CMD_INTERNAL );
    smoothHeadingCmdInternalWriter_ = newDataWriter( HorizontalControlIF::SMOOTH_HEADING_CMD_INTERNAL );
    headingIntegralInternalWriter_ = newDataWriter( HorizontalControlIF::HEADING_INTEGRAL_INTERNAL );
    xteIntegralInternalWriter_ = newDataWriter( HorizontalControlIF::XTE_INTEGRAL_INTERNAL );
    xteInternalWriter_ = newDataWriter( HorizontalControlIF::XTE_INTERNAL );
    kxteInternalWriter_ = newDataWriter( HorizontalControlIF::KXTE_INTERNAL );
    bearingInternalWriter_ = newDataWriter( HorizontalControlIF::BEARING_INTERNAL );

    // Slate outputs to servos
    rudderAngleActionWriter_    = newDataWriter( HorizontalControlIF::RUDDER_ANGLE_ACTION );

    /// Used to read last action, even if from another source
    rudderAngleActionReader_    = newDataReader( HorizontalControlIF::RUDDER_ANGLE_ACTION );

}

HorizontalControl::~HorizontalControl()
{}

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

    ok_ = readConfig();
    if( !ok_ )
    {
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine. Returning.\n" ), Syslog::CRITICAL );
    }
    double latitudeCmd;
    double longitudeCmd;
    latitudeCmdReader_ ->read( Units::RADIAN, latitudeCmd );
    longitudeCmdReader_->read( Units::RADIAN, longitudeCmd );

    double latitude;
    double longitude;
    latitudeReader_->read( Units::RADIAN, latitude );
    longitudeReader_->read( Units::RADIAN, longitude );

}

/// The actual "payload" of the component
void HorizontalControl::run()
{
    if( !ok_ )
    {
        initialize();
        if( !ok_ )
        {
            logger_.syslog( Str( "Error: Error running HorizontalControl. Returning.\n" ), Syslog::ERROR );
            return;
        }
    }
    else
    {
        readConfig();
    }

    if( horizontalModeReader_->isActive() )
    {
        controlHeading();
    }
    else
    {
        setRudderAngle( 0.0 );
    }

}

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

////Private Methods

/// Load HorizontalControl parameters
bool HorizontalControl::readConfig( void )
{
    // Check if all the parameters are read correctly
    bool ok = true;
    ok &= kdHeadingCfgReader_->read( Units::SECOND, kdHeading_ );
    ok &= kiHeadingCfgReader_->read( Units::RECIPROCAL_SECOND, kiHeading_ );
    ok &= kpHeadingCfgReader_->read( Units::NONE, kpHeading_ );
    ok &= kwpHeadingCfgReader_->read( Units::RADIAN_PER_METER, kwpHeading_ );
    ok &= kiwpHeadingCfgReader_->read( Units::RADIAN_PER_SECOND_PER_METER, kiwpHeading_ );
    ok &= maxHdgAccelCfgReader_->read( Units::RADIAN_PER_SECOND_SQUARED, maxHdgAccel_ );
    ok &= maxHdgIntCfgReader_->read( Units::RADIAN, maxHeadingInt_ );
    ok &= maxHdgRateCfgReader_->read( Units::RADIAN_PER_SECOND, maxHeadingRateCfg_ );
    ok &= maxKxteCfgReader_->read( Units::RADIAN, maxKxte_ );
    ok &= rudDeadbandCfgReader_->read( Units::RADIAN, rudDeadband_ );
    ok &= rudLimitCfgReader_->read( Units::RADIAN, rudderLimit_ );

    return ok;
}

void HorizontalControl::controlHeading()
{
    HorizontalControlIF::HorizontalMode horizontalMode( HorizontalControlIF::NONE );
    int horizontalModeInt( ( int ) horizontalMode );
    if( horizontalModeReader_->wasTouchedSinceLastRun( this )
            && horizontalModeReader_->read( Units::ENUM, horizontalModeInt )
            && horizontalModeInt >= HorizontalControlIF::NONE
            && horizontalModeInt <= HorizontalControlIF::RUDDER_ANGLE )
    {
        horizontalMode = ( HorizontalControlIF::HorizontalMode ) horizontalModeInt;
    }

    // Set the heading rate if specified
    bool headingRateSpecified( false );
    float headingRateCmd( nanf( "" ) );
    if( headingRateCmdReader_->wasTouchedSinceLastRun( this )
            && headingRateCmdReader_->read( Units::RADIAN_PER_SECOND, headingRateCmd )
            && !isnan( headingRateCmd ) )
    {
        headingRate_ = AuvMath::Limit( fabsf( headingRateCmd ), 0.0f, maxHeadingRateCfg_ );
        headingRateSpecified = true;
    }
    else
    {
        headingRate_ = maxHeadingRateCfg_;
    }

    float headingCmd( nanf( "" ) );
    float rudderAngleCmd( nanf( "" ) );
    switch( horizontalMode )
    {
    case HorizontalControlIF::NONE:
        setRudderAngle( 0.0 );
        break;

    case HorizontalControlIF::WAYPOINT:
        if( !bearingCmdReader_->isActive() || !bearingCmdReader_->wasTouchedSinceLastRun( this ) )
        {
            return;
        }
        float bearing;
        double latitudeCmd;
        double longitudeCmd;
        bearingCmdReader_  ->read( Units::RADIAN, bearing );
        if( latitudeCmdReader_->isActive() && latitudeCmdReader_->wasTouchedSinceLastRun( this )
                && longitudeCmdReader_->isActive() && longitudeCmdReader_->wasTouchedSinceLastRun( this )
                && latitudeReader_->isActive() && latitudeReader_->wasTouchedSinceLastRun( this )
                && longitudeReader_->isActive() && longitudeReader_->wasTouchedSinceLastRun( this )
                && latitudeCmdReader_->read( Units::RADIAN, latitudeCmd )
                && !isnan( latitudeCmd )
                && longitudeCmdReader_->read( Units::RADIAN, longitudeCmd )
                && !isnan( longitudeCmd ) )
        {
            float dt( AuvMath::NanZero( dt_.asDouble() ) );
            double latitude;
            double longitude;
            latitudeReader_->read( Units::RADIAN, latitude );
            longitudeReader_->read( Units::RADIAN, longitude );
            float dn, de, kxte;
            //
            // Compute differential northings and eastings:
            dn = ( latitudeCmd - latitude )   * Location::EARTH_RADIUS;
            de = ( longitudeCmd - longitude ) * Location::EARTH_RADIUS
                 * cos( latitude );
            //
            // Find the cross-track error:
            xte_     = -sin( bearing ) * dn + cos( bearing ) * de;
//          kxte     = atan( xte_ * kwpHeading_ * M_PI_2 / maxKxte_ ) * M_2_PI * maxKxte_;
            kxte     = xte_ * kwpHeading_;

            xteIntegral_ += kiwpHeading_ * dt * xte_;
            //
            // First limit the integral term to prevent windup.
            //
            xteIntegral_ = AuvMath::Limit( xteIntegral_, maxKxte_, -maxKxte_ );

            xteIntegralInternalWriter_->write( Units::RADIAN, xteIntegral_ );

            float crabCmd = kxte + xteIntegral_;  //radians
            //
            // Re-limit to the same value.
            crabCmd = AuvMath::Limit( crabCmd, maxKxte_, -maxKxte_ );

            bearing += crabCmd;
            xteInternalWriter_->write( Units::METER, xte_ );
            kxteInternalWriter_->write( Units::RADIAN, kxte );
        }

        setHeading( AuvMath::ModPi( bearing ), true );
        bearingInternalWriter_->write( Units::RADIAN, bearing );

        break;

    case HorizontalControlIF::HEADING:
        if( headingCmdReader_->read( Units::RADIAN, headingCmd )
                && !isnan( headingCmd ) )
        {
            setHeading( headingCmd );
        }
        break;

    case HorizontalControlIF::HEADING_RATE:
        if( headingRateSpecified )
        {
            setHeadingRate( headingRate_ );
        }
        break;

    case HorizontalControlIF::RUDDER_ANGLE:
        if( rudderAngleCmdReader_->read( Units::RADIAN, rudderAngleCmd ) )
        {
            setRudderAngle( rudderAngleCmd );
        }
        break;

    default:
        logger_.syslog( Str( "Error: Invalid heading mode specified." ), Syslog::ERROR );
        break; // *** log error here??
    }
}

void HorizontalControl::setHeading( float desiredHeading, bool useInt )
{
    float heading( nanf( "" ) );
    float headingRate( nanf( "" ) );
    if( !isnan( desiredHeading )
            && headingReader_->read( Units::RADIAN, heading )
            && !isnan( heading )
            && headingRateReader_->read( Units::RADIAN_PER_SECOND, headingRate )
            && !isnan( headingRate ) )
    {
        // Derive the rudder command for the current control cycle
        float rudderAction = headingControl( heading, desiredHeading, headingRate, useInt );

        float lastRudderAction;
        if( rudderAngleActionReader_->read( Units::RADIAN, lastRudderAction )
                && !isnan( lastRudderAction )
                && fabs( rudderAction ) < rudderLimit_
                && fabs( rudderAction - lastRudderAction ) < rudDeadband_ )
        {
            if( AuvMath::Sign( rudderAction ) != AuvMath::Sign( lastRudderAction ) )
            {
                rudderAction = 0.0f;
            }
            else
            {
                rudderAction = lastRudderAction;
            }
        }

        rudderAngleActionWriter_->write( Units::RADIAN, rudderAction );
        headingCmdInternalWriter_->write( Units::RADIAN, desiredHeading );
    }
}

void HorizontalControl::setHeadingRate( float desiredHeadingRate )
{
    float heading( nanf( "" ) );
    if( headingReader_->read( Units::RADIAN, heading )
            && !isnan( heading ) )
    {
        float dt( AuvMath::NanZero( dt_.asDouble() ) );
        float desiredHeading( heading + dt * desiredHeadingRate );
        setHeading( desiredHeading );
    }
}

void HorizontalControl::setRudderAngle( float desiredRudderAngle )
{
    if( !isnan( desiredRudderAngle ) )
    {
        rudderAngleActionWriter_->write( Units::RADIAN, desiredRudderAngle );
    }
}

float HorizontalControl::headingControl( float heading, float headingCmd, float headingRate, bool useInt )
{
    float dt( AuvMath::NanZero( dt_.asDouble() ) );
    float smoothHeadingCmd( headingTrajectory_.project( heading, dt, headingCmd, headingRate_, maxHdgAccel_ ) );
    smoothHeadingCmdInternalWriter_->write( Units::RADIAN, smoothHeadingCmd );

    headingControlGainOverrides();
    float headingError( AuvMath::ModPi( heading - smoothHeadingCmd ) );
    float headingProportional( kpHeading_ * headingError );
    float headingDifferential( kdHeading_ * ( headingRate - headingTrajectory_.getXDot() ) );

    if( fabs( headingTrajectory_.getXDot() ) < .0175 )      //TODO: make this value a config! Fix later. .017 rad = 1 deg.
    {
        float rudderIn( nanf( "" ) );
        if( rudderAngleReader_->read( Units::RADIAN, rudderIn )
                && !isnan( rudderIn ) )
        {
            if( isnan( headingIntegral_ ) )
            {
                headingIntegral_ = 0;
            }
            if( ( fabs( rudderIn ) < rudderLimit_ ) || ( fabs( kiHeading_ ) <= 1e-6 ) )
                headingIntegral_ += kiHeading_ * dt * headingError;
            else
                headingIntegral_ = AuvMath::Sign( rudderIn ) * rudderLimit_
                                   - headingProportional - headingDifferential;
        }

        if( !useInt ) headingIntegral_ = 0.;

        // Limit the integral term
        headingIntegral_ = AuvMath::Limit( headingIntegral_, maxHeadingInt_, -maxHeadingInt_ );
        headingIntegralInternalWriter_->write( Units::RADIAN, headingIntegral_ );

    }

    float rudderOut( AuvMath::Limit(
                         headingProportional + AuvMath::NanZero( headingIntegral_ ) + headingDifferential,
                         rudderLimit_,
                         -rudderLimit_ ) );
    return rudderOut;
}

void HorizontalControl::headingControlGainOverrides()
{
    if( kpHeadingOverrideReader_->wasTouchedSinceLastRun( this ) )
    {
        float kp( nanf( "" ) );
        if( kpHeadingOverrideReader_->read( Units::RATIO, kp ) && !isnan( kp ) )
        {
            kpHeading_ = kp;
        }
    }

    if( kiHeadingOverrideReader_->wasTouchedSinceLastRun( this ) )
    {
        float ki( nanf( "" ) );
        if( kiHeadingOverrideReader_->read( Units::RECIPROCAL_SECOND, ki ) && !isnan( ki ) )
        {
            kiHeading_ = ki;
        }
    }

    if( kdHeadingOverrideReader_->wasTouchedSinceLastRun( this ) )
    {
        float kd( nanf( "" ) );
        if( kiHeadingOverrideReader_->read( Units::SECOND, kd ) && !isnan( kd ) )
        {
            kdHeading_ = kd;
        }
    }
}