/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DepthEnvelope.cc                                              */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "MissionTimeAttribute.h"
#include "DepthEnvelope.h"
#include "FloatAttribute.h"
#include "AcousticModemIF.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "Syslog.h"
#include "TimeP.h"
#include "LowAltitudeFollowing.h"

//const double PI=3.1415926535898;

DepthEnvelope::DepthEnvelope()
: Behavior(DepthEnvelopeBehaviorName, NonSequential)
{
    Attribute::Input *input;
    
    attributes.add(new FloatAttribute("minDepth", "Minimum depth",
                                      &_minDepth ));
    
    attributes.add(new FloatAttribute("maxDepth", "Maximum depth",
                                      &_maxDepth ));
    
    attributes.add(new FloatAttribute("abortLockoutDepth",
                                      "Altitude aborts are ignored when shallower than this depth",
                                      &_hardDeck, NoHardDeck));
    
    attributes.add(new FloatAttribute("minAltitude", "Minimum altitude",
                                      &_minAltitude, NoMinimumAltitude ));
    
    attributes.add(new FloatAttribute("abortDepth",
                                      "Abort when below this depth",
                                      &_abortDepth, NoAbortDepth ));
    
    attributes.add(new FloatAttribute("abortAltitude",
                                      "Abort when below this altitude",
                                      &_abortAltitude, NoAbortAltitude ));
    
    attributes.add(new FloatAttribute("deltaDepthRestart",
                                      "Restart when depth has decreased this much",
                                      &_deltaDepth, NoDeltaDepth ));
    
    attributes.add(new AngleAttribute("maxLowerPitch",
                                      "Lower pitch limit when near bottom.",
                                      &_maxLowerPitch, NoMaxLowerPitch ));
    
    attributes.add(new FloatAttribute("pitchLimitAltitude",
                                      "Altitude below which lower pitch limits",
                                      &_pitchLimitAlt, NoPitchLimitAlt ));
    
    attributes.add(new FloatAttribute("minObstacleRange",
                                      "Abort when an obstacle is too close",
                                      &_minFlsHorzRange, 50. ));
    
    attributes.add(new BooleanAttribute("altSuspendDepthControl",
                                        "Use depth control while altitude is invalid",
                                        &_altSuspendDepthControl, False ));
    
    attributes.add(new AngleAttribute("altSuspendPitch",
                                      "Use pitch control while altitude is invalid",
                                      &_altSuspendPitch, NoAltSuspendPitch ));
    
    attributes.add(new FloatAttribute("altTimeOut",
                                      "Suspend prop if altitude invalid too long",
                                      &_altTimeOut, NoAltTimeOut ));
    
    // Depth envelope should be default run forever
    input = new Attribute::Input("endTime", NoTimeLimitMnem);
    attributes.parse(input);
    delete input;
    _altitudeStop = False;
    _depthStop = False;
    
    // Elevator angle during deltaDepth shutoff period.
    _elevator = -3.*Math::RadsPerDeg;
    _lastDepth = 5.;
    _latchData = True;
    _indirectAltCntrl = False;
}


DepthEnvelope::~DepthEnvelope()
{
    
}


Boolean DepthEnvelope::validInput()
{
    Boolean valid = True;
    
    if (_maxDepth < _minDepth) {
        printError("maxDepth is less than minDepth");
        valid = False;
    }
    
    if( _maxLowerPitch < 0. && (_maxLowerPitch != NoMaxLowerPitch*Math::RadsPerDeg))
    {
        Syslog::write("DepthEnvelope - Error. maxLowerPitch must be a postive "
                      "number.\n"
                      "Flipping the sign now.");
        _maxLowerPitch *= -1;
        Syslog::write("maxLowerPitch = %.2f", _maxLowerPitch);
    }
    
    if( _maxLowerPitch == NoMaxLowerPitch*Math::RadsPerDeg &&
       _pitchLimitAlt != NoPitchLimitAlt )
    {
        Syslog::write("DepthEnvelope - Error. Since pitchLimitAlt is specified, \n"
                      "you must also specify maxLowerPitch.");
        valid = False;
    }
    
    if( _maxLowerPitch != NoMaxLowerPitch*Math::RadsPerDeg &&
       _pitchLimitAlt == NoPitchLimitAlt )
    {
        Syslog::write("DepthEnvelope - Error. Since maxLowerPitch is specified, \n"
                      "you must also specify pitchLimitAlt.");
        valid = False;
    }
    
    if( _altSuspendDepthControl && _altSuspendPitch != NoAltSuspendPitch*Math::RadsPerDeg )
    {
        Syslog::write("DepthEnvelope - Error. You cannot specify both \n"
                      "altSuspendDepthControl and altSuspendPitch.");
        valid = False;
    }
    
    if( _altTimeOut == NoAltTimeOut &&
       (_altSuspendDepthControl || _altSuspendPitch != NoAltSuspendPitch*Math::RadsPerDeg ))
    {
        Syslog::write("DepthEnvelope - Error. You must specify altTimeOut.");
        valid = False;
    }
    
    return valid;
}


void DepthEnvelope::execute()
{
    Boolean debug = True;
    Boolean abortDepthSpecified = (_abortDepth != NoAbortDepth);
    Boolean abortAltitudeSpecified = (_abortAltitude != NoAbortAltitude );
    Boolean minAltitudeSpecified = (_minAltitude != NoMinimumAltitude);
    Boolean hardDeckSpecified = (_hardDeck != NoHardDeck);
    Boolean deltaDepthSpecified = (_deltaDepth != NoDeltaDepth);
    Boolean maxLowerPitchSpecified = (_maxLowerPitch != NoMaxLowerPitch*Math::RadsPerDeg);
    Boolean pitchLimitAltSpecified = (_pitchLimitAlt != NoPitchLimitAlt);
    
    double maxDepth = _maxDepth, minDepth = _minDepth, hardDeck = _hardDeck;
    double depth = _navigation->depth();
    double altitude = _navigation->altitude();
    
    double flsVertRange, flsHorzRange;
    Boolean useFls;
    
    _navigation->getFlsData( &flsVertRange, &flsHorzRange, &useFls );
    
    
    //make sure minAltitude is reasonable, since the prop is stopped at
    //the abort altitude and restarted at the minimum altitude
    if (!minAltitudeSpecified) _minAltitude = _abortAltitude;
    
    //** First, confirm vehicle is not outside of abort depth
    if (abortDepthSpecified && (depth > _abortDepth)) {
        Syslog::write("DepthEnvelope -- Measured depth %lf over allowable "
                      "depth %lf at t= %.2f.  Stopping!",
                      depth, _abortDepth, _missionClock->seconds() );
        _depthStop = True;
    }
    
    //if we've floated shallower than the max depth, then clear the depth stop flag
    if (depth <= maxDepth) {
        if (_depthStop) Syslog::write("DepthEnvelope -- Measured depth %lf shallower than %lf. Restarting!", depth, maxDepth);
        _depthStop = False;
    }
    
    // check the altitude,stop or restart the prop as appropriate
    if (abortAltitudeSpecified && altitude != INVALID_ALTITUDE) {
        //Too close to the bottom. Latch the prop off.
        if (altitude < _abortAltitude && !useFls) {
            if( !_altitudeStop )
            {
                Syslog::write("DepthEnvelope -- Measured altitude = %lf. Allowable "
                              "altitude = %lf. t= %.2f. Stopping!",
                              altitude, _abortAltitude, _missionClock->seconds());
                _restartDepth = depth - _deltaDepth;
            }
            _altitudeStop = True;
        }
        //The FLS has detected something in our path ahead. Latch the prop off.
        if (altitude < _abortAltitude && useFls && flsHorzRange<_minFlsHorzRange ) {
            if( !_altitudeStop )
            {
                Syslog::write("DepthEnvelope -- Obstacle detected at altitude = %lf "
                              "and range %lf.\n Allowable "
                              "altitude = %lf. t= %.2f. Stopping!",
                              altitude, flsHorzRange, _abortAltitude,
                              _missionClock->seconds());
                _restartDepth = depth - _deltaDepth;
            }
            _altitudeStop = True;
        }
        //restart prop when we are higher than the minimum altitude
        if (!deltaDepthSpecified && _altitudeStop &&  (altitude>_minAltitude)) {
            Syslog::write("DepthEnvelope -- Measured altitude = %lf. Abort altitude "
                          "= %lf. t= %.2f, starting prop!",
                          altitude, _abortAltitude, _missionClock->seconds());
            _altitudeStop = False;
        }
        if (deltaDepthSpecified && (altitude>_minAltitude) && _altitudeStop &&
            (depth < _restartDepth)) {
            Syslog::write("DepthEnvelope -- Depth %lf shallower than restart "
                          "depth %lf \nand altitude %lf greater than abort altitude "
                          "%lf at t= %.2f. Starting prop!",
                          depth, _restartDepth, altitude, _abortAltitude,
                          _missionClock->seconds());
            _altitudeStop = False;
        }
    }
    
    if ((altitude==INVALID_ALTITUDE) &&
        (hardDeckSpecified && (depth < hardDeck)) &&
        _altitudeStop) {
        Syslog::write("DepthEnvelope -- Depth %lf shallow enough to restart "
                      "prop at t= %.2f!",
                      depth, _missionClock->seconds());
        _altitudeStop = False;
    }
    
    // (this is new from Ody) if the stack is empty, then pass it through
    // (that is, if all behaviors above are inactive, then be inactive also
    if( isOutputEmpty() ) {
        dprintf("DepthEnvelope::execute() - Received empty stack, passing it through\n");
        return;
    }
    
    if ( altitude==INVALID_ALTITUDE )
    {
        if (getVerticalMode() == DynamicControlIF::Altitude )
        {
            if( _latchData )
            {
                Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
                              "Invalid Altitude at t= %.2f!. "
                              "Latching depth %lf.", _missionClock->seconds(), depth );
                _lastDepth = depth;
                if( deltaDepthSpecified ) _restartDepth = depth - _deltaDepth;
                _lastTime = _missionClock->seconds();
                _latchData = False;
                if( _altSuspendDepthControl )
                {
                    Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
                                  "Maintaining constant depth.");
                }
                if( _altSuspendPitch != NoAltSuspendPitch*Math::RadsPerDeg )
                {
                    Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
                                  "Maintaining constant pitch.");
                }
            }
            if( _altSuspendDepthControl )
            {
                setVertical(DynamicControlIF::DepthImmediate, _lastDepth);
            }
            else if( _altSuspendPitch != NoAltSuspendPitch*Math::RadsPerDeg )
            {
                setVertical(DynamicControlIF::Pitch, _altSuspendPitch);
            }
            else
            {
                // The vehicle won't restart unless both the abort and minimum
                // altitudes are specified.
                if( !_altitudeStop )
                {
                    Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
                                  "Altitude invalid at t= %.2f!. "
                                  "Suspending propulsion.", _missionClock->seconds());
                }
                _altitudeStop = True;
            }
            if( _missionClock->seconds() - _lastTime > _altTimeOut )
            {
                if( !_altitudeStop )
                {
                    Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
                                  "Altitude still invalid at t= %.2f!. "
                                  "Suspending propulsion.", _missionClock->seconds());
                }
                _altitudeStop = True;
                //abortMission();
            }
        }
    }
    else if( !_latchData )
    {
        Syslog::write("DepthEnvelope -- Obtained valid altitude at %.2f.",
                      _missionClock->seconds());
        
        if( _missionClock->seconds() - _lastTime < _altTimeOut &&
           getVerticalMode() == DynamicControlIF::Altitude )
        {
            Syslog::write("DepthEnvelope -- Direct Altitude Control: %.2f seconds left until timeout.",
                          _altTimeOut- (_missionClock->seconds() - _lastTime));
        }
        _latchData = True;
    }
    
    if( minAltitudeSpecified )
    {
        double depthToMinAltitude = depth + altitude - _minAltitude;
        //
        // 
        _maxLowerPitchDepth = depth + altitude - _pitchLimitAlt;
        
        if (depthToMinAltitude < _maxDepth)
            // "Clip" maximum depth
            maxDepth = depthToMinAltitude;
    }
    
    if (_altitudeStop || _depthStop) { //if _altitudeStop flag is set, then turn off the prop
        setSpeed(DynamicControlIF::Speed, 0.0);
        if( deltaDepthSpecified ) 
            setVertical(DynamicControlIF::Elevator, _elevator);
    }
    
    // Commanded too deep
    if( (  (getVerticalMode()==DynamicControlIF::Depth) ||
         (getVerticalMode()==DynamicControlIF::DepthImmediate)) &&
       (getVertical() > maxDepth ) ) 
    {
        dprintf("DepthEnvelope::execute() - commanded depth = %.3f, too deep\n",
                getVertical() );
        setVertical(getVerticalMode(), maxDepth);
        
        if( pitchLimitAltSpecified &&
           maxLowerPitchSpecified && (depth > _maxLowerPitchDepth ))
        {
            setVertical(DynamicControlIF::Depth, maxDepth, _maxLowerPitch);
            //Syslog::write("DepthEnvelope - maxLowerPitch = %.2f",
            //	       _maxLowerPitch);
        }
        return;
    } 
    
    // Too shallow?
    // Commanded shallow?
    if( ((getVerticalMode() == DynamicControlIF::Depth) ||
         (getVerticalMode() == DynamicControlIF::DepthImmediate)) &&
       (getVertical() < minDepth ) ) {
        dprintf("DepthEnvelope::execute() - commanded depth = %.3f, too shallow\n",
                getVertical() );
        setVertical(getVerticalMode(), minDepth);
        return;
    }
}