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

#include "PitchEnvelope.h"
#include "PitchEnvelopeIF.h"

#include "data/ConfigReader.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"

// Outputs go to Dynamic Control
#include "controlModule/VerticalControlIF.h"
#include "controlModule/SpeedControlIF.h"

PitchEnvelope::PitchEnvelope( const Str& prefix, const Module* module )
    : Behavior( prefix + PitchEnvelopeIF::NAME, module, true, true ),
      surfaceTimeoutReported_( false ),
      pitchTimeoutReported_( false ),
      pitchLimit_( -0.523599 ),  // -30 degrees
      massPositionLimitAft_( -0.03 ), // -3 cm
      /// Settings
      surfacingTimeoutSetting_( 1000 ),
      pitchTimeoutCfgSetting_( 30 ),
      speedSetting_( SpeedControlIF::SPEED_FAST )
{
    logger_.syslog( "Construct PitchEnvelope." );

    // Settings
    surfaceTimeoutSettingReader_ = newSettingReader( PitchEnvelopeIF::SURFACE_TIMEOUT_NAME );
    speedSettingReader_ = newSettingReader( PitchEnvelopeIF::SPEED_NAME );

    // Slate input measurements
    depthReader_    = newUniversalReader( UniversalURI::DEPTH );
    altitudeReader_ = newUniversalReader( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR );
    pitchReader_    = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE );
    depthCmdReader_ = newDataReader( VerticalControlIF::DEPTH_CMD );
    speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD );

    // Slate output variables
    depthRateCmdWriter_     = newDataWriter( VerticalControlIF::DEPTH_RATE_CMD );
    pitchCmdWriter_         = newDataWriter( VerticalControlIF::PITCH_CMD );
    speedCmdWriter_         = newDataWriter( SpeedControlIF::SPEED_CMD );
    verticalModeWriter_     = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    elevatorAngleCmdWriter_ = newDataWriter( VerticalControlIF::ELEVATOR_ANGLE_CMD );
    massPositionCmdWriter_  = newDataWriter( VerticalControlIF::MASS_POSITION_CMD );

    // Configuration
    pitchLimitCfgReader_           = newConfigReader( VerticalControlIF::PITCH_LIMIT_CFG );
    massPositionLimitAftCfgReader_ = newConfigReader( VerticalControlIF::MASS_POSITION_LIMIT_AFT_CFG );
    pitchTimeoutCfgReader_         = newConfigReader( VerticalControlIF::PITCH_TIMEOUT_GO_TO_SURFACE );
}

PitchEnvelope::~PitchEnvelope()
{}

// Initialize function
void PitchEnvelope::initialize( void )
{
    logger_.syslog( "Initialize PitchEnvelope." );
    surfacingTimeoutSetting_ = Timespan( 1000 );
    pitchTimeoutCfgSetting_ = Timespan( 30 );
    speedSetting_ = SpeedControlIF::SPEED_FAST;

    if( !loadParams() ) logger_.syslog( "Error loading parameters in initialization routine.", Syslog::CRITICAL );
    depthReader_->requestData( true );
    altitudeReader_->requestData( true );
    pitchReader_->requestData( true );

    // Begin running the clock to keep track of timeouts for pitch and surfacing
    resetTimeouts();
}

// Load parameters
bool PitchEnvelope::loadParams( void )
{
    // Check if all the parameters are read correctly
    bool ok = true;

    float surfacingTimeout( nanf( "" ) );
    if( surfaceTimeoutSettingReader_->read( Units::SECOND, surfacingTimeout ) )
    {
        logger_.syslog( "Received surface timeout setting " + Str( surfacingTimeout ) + " seconds.", Syslog::INFO );
        surfacingTimeoutSetting_ = Timespan( surfacingTimeout );
    }
    else
    {
        logger_.syslog( "No surface timeout specified. Using default value of " + Str( surfacingTimeoutSetting_.asFloat() ) + " seconds.", Syslog::DEBUG );
    }

    float pitchTimeout( nanf( "" ) );
    if( pitchTimeoutCfgReader_->read( Units::SECOND, pitchTimeout ) )
    {
        logger_.syslog( "Received pitch timeout configuration " + Str( pitchTimeout ) + " seconds.", Syslog::INFO );
        pitchTimeoutCfgSetting_ = Timespan( pitchTimeout );
    }
    else
    {
        logger_.syslog( "No pitch timeout specified. Using default value of " + Str( pitchTimeoutCfgSetting_.asFloat() ) + " seconds.", Syslog::DEBUG );
    }

    if( speedSettingReader_->read( Units::METER_PER_SECOND, speedSetting_ ) )
    {
        logger_.syslog( "Received speed setting " + Str( speedSetting_ ) + " m/s.", Syslog::INFO );
        speedSetting_ = fabs( speedSetting_ );
    }
    else
    {
        logger_.syslog( "No speed setting specified. Using default value of " + Str( speedSetting_ ) + " m/s.", Syslog::DEBUG );
    }

    ok &= massPositionLimitAftCfgReader_->read( Units::METER, massPositionLimitAft_ );
    ok &= pitchLimitCfgReader_->read( Units::RADIAN, pitchLimit_ );

    return ok;
}

// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool PitchEnvelope::readParams()
{
    bool ok( true );

    depth_ = nanf( "" );
    pitch_ = nanf( "" );
    depthCmd_ = nanf( "" );
    speedCmd_ = nanf( "" );

    ok &= speedCmdReader_->wasTouchedSinceLastRun( this );

    if( ok )
    {
        // Read in all the relevant measurements
        ok &= speedCmdReader_->read( Units::METER_PER_SECOND, speedCmd_ ) && !isnan( speedCmd_ );
        ok &= pitchReader_->read( Units::RADIAN, pitch_ ) && !isnan( pitch_ );

        // Leave depthCmd set to nan if not actively controlling depth
        if( depthCmdReader_->isActive() && depthCmdReader_->wasTouchedSinceLastRun( this ) )
        {
            depthCmdReader_->read( Units::METER, depthCmd_ );
        }
    }

    ok &= depthReader_->read( Units::METER, depth_ ) && !isnan( depth_ );
    return ok;
}

// Perform the satisfied: return true if envelope "satisfied"
bool PitchEnvelope::calcSatisfied()
{
    // TODO: consider other vertical modes
    if( speedCmd_ > 0 && ( depth_ > depthCmd_ || pitch_ < -pitchLimit_ ) )
    {
        // Activate the envelope
        return false;
    }

    // Reset all timeouts and return true.
    resetTimeouts();
    return true;
}


// Just do the run: ignore the results of the satisfied
void PitchEnvelope::run()
{
    runIfUnsatisfied();
}

// Just do the satisfied: return true if envelope "satisfied"
bool PitchEnvelope::isSatisfied()
{
    if( !readParams() )
    {
        return false;
    }
    return calcSatisfied();
}

// Do the run, and return true if envelope "satisfied"
bool PitchEnvelope::runIfUnsatisfied()
{
    bool satisfied( false );

    if( readParams() )
    {
        satisfied = calcSatisfied();
        if( !satisfied )
        {
            // Check the pitch and altitude of the vehicle to make sure we can drive at the commanded speed.
            if( !checkPitchAndAltitude() )
            {
                // Go up!

                // Write pitch here so that controlDepth doesn't have a leftover pitch command from something else while it's trying to go up.
                pitchCmdWriter_->write( Units::RADIAN, pitchLimit_ );

                // Command the mass back
                massPositionCmdWriter_->write( Units::METER, massPositionLimitAft_ );

                // Also write the nan depthRate in case someone else had written a "real" value at some point
                depthRateCmdWriter_->write( Units::METER_PER_SECOND, nanf( "" ) );

                // Same here. Write nan for elevators to allow vertical control to take over
                elevatorAngleCmdWriter_->write( Units::RADIAN, nanf( "" ) );

                if( surfaceStartTime_.elapsed() < surfacingTimeoutSetting_ )
                {
                    // Request speed to adjust pitch and altitude
                    speedCmdWriter_->write( Units::METER_PER_SECOND, speedSetting_ );
                }
                else
                {
                    // Stop the thruster
                    speedCmdWriter_->write( Units::METER_PER_SECOND, 0 );

                    // Maximize buoyancy and control depth at 0 m.
                    // We'll rely on calcSatisfied to hand control back to subsquent components once we're free and clear
                    verticalModeWriter_->write( Units::ENUM, VerticalControlIF::FLOAT_ON_SURFACE );
                }
            }
        }
    }
    // Depth read has failed. Head up
    else if( isnan( depth_ ) )  // TODO: || isnan( pitch_ )?
    {
        if( surfaceStartTime_.elapsed() > surfacingTimeoutSetting_ )
        {
            if( !surfaceTimeoutReported_ )
            {
                logger_.syslog( "Failed to read depth within specified timeout. Returning to surface.", Syslog::CRITICAL );
                surfaceTimeoutReported_ = true;
            }
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::FLOAT_ON_SURFACE );
            satisfied = true;
        }
    }

    return satisfied;
}

bool PitchEnvelope::checkPitchAndAltitude( void )
{
    if( !isnan( pitch_ ) )
    {
        // We have a good read of pitch
        if( ( pitch_ < -pitchLimit_ || ( pitch_ < 0 && depth_ > depthCmd_ ) ) && pitchStartTime_.elapsed() > pitchTimeoutCfgSetting_ )
        {
            // We've been pitched down for too long.
            if( !pitchTimeoutReported_ )
            {
                logger_.syslog( "Pitch down timeout. Pitch: " + Str( R2D( pitch_ ), 2 ) + " deg, depth: " + Str( depth_, 2 ) + " m,  depthCmd: " + Str( depthCmd_, 2 ) + " m", Syslog::ERROR );
                pitchTimeoutReported_ = true;
            }
            return false;
        }
        else if( pitchTimeoutReported_ && ( ( pitch_ > -pitchLimit_ && depth_ <= depthCmd_ ) || ( pitch_ > 0 && depth_ > depthCmd_ ) ) )
        {
            // restart the pitch clock only
            //logger_.syslog( "Clearing pitch down timeout. Pitch: " + Str( R2D( pitch_ ), 2 ) + " deg, depth: " + Str( depth_, 2 ) + " m,  depthCmd: " + Str( depthCmd_, 2 ) + " m", Syslog::INFO );
            pitchStartTime_ = Timestamp::Now();
            pitchTimeoutReported_ = false;
        }

        // Now check on altitude
        float altitude( nanf( "" ) );
        if( altitudeReader_->isActive() && altitudeReader_->read( Units::METER, altitude ) && !isnan( altitude ) )
        {
            // We have a good read of altitude AND pitch
            if( altitude < 2 )
            {
                logger_.syslog( "Altitude too low: " + Str( altitude, 2 ), Syslog::ERROR );
                return false;
            }
            else
            {
                // Altitude is greater than min
                return true;
            }
        }
        else // No altitude? Let's just go for it for the duration of pitchTimeout
        {
            return true;
        }
    }
    // No pitch? Let's not continue
    return false;
}

// Reset the surface and pitch timeout and arm the reporting of them.
void PitchEnvelope::resetTimeouts( void )
{
    surfaceStartTime_ = Timestamp::Now();
    pitchStartTime_ = Timestamp::Now();

    surfaceTimeoutReported_ = false;
    pitchTimeoutReported_ = false;
}

// Uninit function
void PitchEnvelope::uninitialize( void )
{
    logger_.syslog( "Uninitialize PitchEnvelope." );
    depthReader_->requestData( false );
    altitudeReader_->requestData( false );
    pitchReader_->requestData( false );
}

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