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

#include "GoToSurface.h"
#include "GoToSurfaceIF.h"

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

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

GoToSurface::GoToSurface( const Str& prefix, const Module* module )
    : Behavior( prefix + GoToSurfaceIF::NAME, module, true, true, GoToSurfaceIF::ENABLE_BROADCAST ),
      surfaceTimeoutReported_( false ),
      pitchTimeoutReported_( false ),
      /// Settings
      depthRateSetting_( nanf( "" ) ),
      pitchSetting_( nanf( "" ) ),
      speedSetting_( 1.0 ),
      pitchTimeoutSetting_( 30 ),
      surfacingTimeoutSetting_( 1000 )
{

    logger_.syslog( "Construct GoToSurface." );

    // Settings
    depthRateSettingReader_ = newSettingReader( GoToSurfaceIF::DEPTH_RATE_NAME );
    pitchSettingReader_ = newSettingReader( GoToSurfaceIF::PITCH_NAME );
    speedSettingReader_ = newSettingReader( GoToSurfaceIF::SPEED_NAME );
    surfaceTimeoutSettingReader_ = newSettingReader( GoToSurfaceIF::SURFACE_TIMEOUT_NAME );

    // Slate input measurements
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    altitudeReader_ = newUniversalReader( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR );
    pitchReader_    = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE );

    // 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
    surfaceThresholdCfgReader_    = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );
    pitchLimitCfgReader_          = newConfigReader( VerticalControlIF::PITCH_LIMIT_CFG );
    buoyancyPumpDepthCfgReader_   = newConfigReader( VerticalControlIF::BUOYANCY_PUMP_DEPTH_CFG );
    massPositionLimitAftCfgReader_  = newConfigReader( VerticalControlIF::MASS_POSITION_LIMIT_AFT_CFG );
    massBackOnGoToSurfaceCfgReader_ = newConfigReader( VerticalControlIF::MASS_BACK_ON_GO_TO_SURFACE );
    pitchTimeoutCfgReader_ = newConfigReader( VerticalControlIF::PITCH_TIMEOUT_GO_TO_SURFACE );

    // set the broadcast channel name for all writers
    // replaces the auto-generated channel name (component name) assigned to the behavior by the mission manager
    setChannelName( GoToSurfaceIF::NAME );
}

GoToSurface::~GoToSurface()
{}


// Initialize function
void GoToSurface::initialize( void )
{
    logger_.syslog( "Initialize GoToSurfaceComponent." );
    depthRateSetting_ = nanf( "" );
    pitchSetting_ = nanf( "" );
    speedSetting_ = 1.0;
    pitchTimeoutSetting_ = 30;
    surfacingTimeoutSetting_ = 1000;

    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();
}


// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool GoToSurface::readParams( float& depth )
{
    // Let's read in all the relevant measurements
    return depthReader_->read( Units::METER, depth );
}


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

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

    if( pitchSettingReader_->read( Units::RADIAN, pitchSetting_ ) )
    {
        logger_.syslog( "Received pitch setting " + Str( R2D( pitchSetting_ ) ) + " degrees.", Syslog::INFO );
        pitchSetting_ = fabs( pitchSetting_ );
    }
    else
    {
        logger_.syslog( "No pitch setting specified. Using default value of " + Str( R2D( pitchSetting_ ) ) + " degrees.", 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 );
    }

    if( isnan( speedSetting_ ) ) // TODO: This isn't supposed to happen, unless someone writes a NaN to GoToSurfaceIF::SPEED_NAME, but we're seeing it anyway.
    {
        logger_.syslog( "Speed setting is NaN! Changing to 1.0 m/s.", Syslog::ERROR );
        speedSetting_ = 1.0;
    }

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

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

    ok &= surfaceThresholdCfgReader_->read( Units::METER, surfaceThreshold_ );
    ok &= pitchLimitCfgReader_->read( Units::RADIAN, pitchLimit_ );
    ok &= buoyancyPumpDepthCfgReader_->read( Units::METER, buoyancyPumpDepth_ );
    ok &= massPositionLimitAftCfgReader_->read( Units::METER, massPositionLimitAft_ );
    ok &= massBackOnGoToSurfaceCfgReader_->read( Units::BOOL, shiftMassOnGoToSurface_ );

    return ok;

}


// Perform the satisfied: return true if envelope "satisfied"
bool GoToSurface::calcSatisfied( const float depth )
{

    // If we're on the surface, reset all timeouts and return true.
    if( depth <= surfaceThreshold_ )
    {
        resetTimeouts();
        return true;
    }

    // We've exceeded the time that it should take to surface. Even though we may not be technically there
    // this will allow for the behavior to drop through so the vehicle can take other measures.
    if( surfaceStartTime_.elapsed() > surfacingTimeoutSetting_ )
    {
        if( !surfaceTimeoutReported_ )
        {
            logger_.syslog( "Surfacing timeout. Returning calc satisfied.", Syslog::CRITICAL );
            surfaceTimeoutReported_ = true;
        }
        return true;
    }
    return false;
}


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


// Just do the satisfied: return true if envelope "satisfied"
bool GoToSurface::isSatisfied()
{
    float depth( nanf( "" ) );

    if( !readParams( depth ) )
    {
        return false;
    }
    return calcSatisfied( depth );
}


// Do the run, and return true if envelope "satisfied"
bool GoToSurface::runIfUnsatisfied()
{
    bool retVal = false;
    bool massCommandedBack = false;
    float depth( nanf( "" ) );

    if( readParams( depth ) )
    {
        if( !calcSatisfied( depth ) )
        {
            // Go up!
            if( !isnan( depthRateSetting_ ) )
            {
                depthRateCmdWriter_->write( Units::METER_PER_SECOND, depthRateSetting_ );
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::DEPTH_RATE );
            }
            if( !isnan( pitchSetting_ ) )
            {
                pitchCmdWriter_->write( Units::RADIAN, pitchSetting_ );
                if( isnan( depthRateSetting_ ) )
                {
                    verticalModeWriter_->write( Units::ENUM, VerticalControlIF::PITCH );
                }
            }
            else if( isnan( depthRateSetting_ ) )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::SURFACE );
                // Write pitch here so that controlDepth doesn't have a leftover pitch command from something else while it's trying to get to the surface.
                pitchCmdWriter_->write( Units::RADIAN, pitchLimit_ );

                if( shiftMassOnGoToSurface_ )
                {
                    // Command the mass back
                    if( massPositionCmdWriter_->write( Units::METER, massPositionLimitAft_ ) ) massCommandedBack = true;
                }

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

            // Check the pitch and altitude of the vehicle to make sure we can drive at the commanded speed.
            if( checkPitchAndAltitude() )
            {
                // Vehicle pitch and altitude are okay (or mostly so). Let's try requested speed.
                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
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::FLOAT_ON_SURFACE );
            }

        }
        else
        {
            // Stop the thruster
            speedCmdWriter_->write( Units::METER_PER_SECOND, 0 );
            // Maximize buoyancy
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::FLOAT_ON_SURFACE );
            retVal = true;
        }

    }

    // Depth read has failed. Head up
    else
    {
        // We've exceeded the time that it should take to surface. Even though we may not be technically there
        // this will allow for the behavior to drop through so the vehicle can take other measures.
        // This is also checked in calcSatisfied, but needs to be done here to cover the case where depth reading has failed.
        if( surfaceStartTime_.elapsed() > surfacingTimeoutSetting_ )
        {

            if( !surfaceTimeoutReported_ )
            {
                surfaceTimeoutReported_ = true;
                logger_.syslog( "Surfacing timeout. Returning calc satisfied after failed depth reading.", Syslog::CRITICAL );
            }
            retVal = true;
        }
        verticalModeWriter_->write( Units::ENUM, VerticalControlIF::FLOAT_ON_SURFACE );
    }
    elevatorAngleCmdWriter_->write( Units::RADIAN, nanf( "" ) );
    if( !massCommandedBack ) massPositionCmdWriter_->write( Units::METER, nanf( "" ) );
    return retVal;
}


bool GoToSurface::checkPitchAndAltitude( void )
{
    float pitch( nanf( "" ) );
    float altitude( nanf( "" ) );

    if( pitchReader_->isActive() && pitchReader_->read( Units::DEGREE, pitch ) && !isnan( pitch ) )
    {
        // We have a good read of pitch
        if( pitch < 0 && pitchStartTime_.elapsed() > pitchTimeoutSetting_ )
        {
            // We've been pitched down for too long.
            if( !pitchTimeoutReported_ )
            {
                pitchTimeoutReported_ = true;
                logger_.syslog( "Pitch down timeout. Pitch: " + Str( pitch, 2 ), Syslog::ERROR );
            }
            return false;
        }
        else if( pitch > 0 )
        {
            // restart the pitch clock only
            pitchStartTime_ = Timestamp::Now();
            pitchTimeoutReported_ = false;
        }

        // Now check on altitude
        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 GoToSurface::resetTimeouts( void )
{
    surfaceStartTime_ = Timestamp::Now();
    pitchStartTime_ = Timestamp::Now();

    surfaceTimeoutReported_ = false;
    pitchTimeoutReported_ = false;
}


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


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