/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Behavior.cc                                                   */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <string.h>
#include <malloc.h>
#include "Task.h"
#include "Behavior.h"
#include "MissionTimeAttribute.h"
#include "IntegerAttribute.h"
#include "LayeredControl.h"
#include "Syslog.h"

const Boolean Behavior::Sequential = True;
const Boolean Behavior::NonSequential = False;
MissionClock *Behavior::_missionClock = 0;
NavigationIF *Behavior::_navigation = 0;
VehicleConfigurationIF *Behavior::_vehicleConfig = 0;
GpsIF *Behavior::_gps = 0;
InsIF *Behavior::_ins = 0;
CtdIF *Behavior::_ctd = 0;
DvlIF *Behavior::_dvl = 0;
DvlSideIF *Behavior::_dvlSide = 0;
MultibeamerIF *Behavior::_multibeamer = 0;
DeltaTIF *Behavior::_deltaT = 0;
UsblIF *Behavior::_usbl = 0;
DockIF *Behavior::_dock = 0;
Reson6046IF *Behavior::_reson = 0;
EdgetechFSDWIF *Behavior::_edgetech = 0;
AcousticModemIF *Behavior::_modem = 0;
CARL_IF *Behavior::_carl = 0;
CathxIF *Behavior::_cathx = 0;
static Boolean Behavior::_lowAltitudeFlightEnabled = False;


Behavior::Behavior(const char *name, Boolean sequential)
: attributes(name)
{
    _name = strdup(name);
    
    setState(Suspended);
    _sequential = sequential;
    _abortMission = False;
    
    attributes.add(new MissionTimeAttribute("startTime", "start time",
                                            &_startTime, 0.0));
    
    attributes.add(new MissionTimeAttribute("endTime", "end time",
                                            &_endTime, -1.0));
    
    attributes.add(new MissionTimeAttribute("duration", "duration",
                                            &_duration, -1.0) );
    
    attributes.add(new IntegerAttribute("id", "optional integer id",
                                        &_id, -1) );
}


Behavior::~Behavior()
{
    //  for (int i = 0; i < attributes.list.size(); i++) {
    //    Attribute *attribute;
    //    attributes.list.get(i, &attribute);
    //    delete attribute;
    //  }
    
    //attributes created above are deleted in Attributes class destructor
    free((void *)_name);
}


void Behavior::printError(const char *msg)
{
    Syslog::write("%s: %s\n", name(), msg);
}



void Behavior::resetOutput()
{
    _output.verticalMode = DynamicControlIF::VmInitial;
    _output.horizontalMode = DynamicControlIF::HmInitial;
    _output.speedMode = DynamicControlIF::SmInitial;
}


void Behavior::output(DynamicControlIF::Command *cmd)
{
    memcpy((void *)cmd, (void *)&_output,
           sizeof(DynamicControlIF::Command));
}

void Behavior::input(DynamicControlIF::Command *cmd)
{
    memcpy( (void *)&_output, (void *)cmd,
           sizeof(DynamicControlIF::Command));
}

int Behavior::verifyBaseAttributes()
{
    Boolean debug = False;
    dprintf("Behavior::verifyBaseAttributes()...\n");
    
    if (_startTime >= _endTime) {
        printError("Start time is after end time\n");
        return -1;
    }
    else if( (_endTime < 0) && (_duration < 0) ) {
        printError("Neither end time nor duration specified\n");
        return -1;
    }
    else if( (_endTime > 0) && (_duration > 0) ) {
        printError("Both end time and duration and end time specified\n");
        return -1;
    }
    else {
        return 0;
    }
}



int Behavior::verify()
{
    Boolean debug = True;
    dprintf("Behavior::verify()...\n");
    return verifyBaseAttributes();
}


void Behavior::setVertical(DynamicControlIF::VerticalMode mode,
                           double vertical, double maxLowerPitch) {
    _output.verticalMode = mode;
    _output.vertical = vertical;
    _output.maxLowerPitch = maxLowerPitch;
    _output.thetaFF = 0.;
    _output.beamIndx = 60;
    _output.rangeToWall = 0.;
}

void Behavior::setVertical(DynamicControlIF::VerticalMode mode,
                           double vertical, double maxLowerPitch,
                           double thetaFF) {
    _output.verticalMode = mode;
    _output.vertical = vertical;
    _output.maxLowerPitch = maxLowerPitch;
    _output.thetaFF = thetaFF;
    _output.beamIndx = 60;
    _output.rangeToWall = 0.;
}

void Behavior::setVertical(DynamicControlIF::VerticalMode mode, double vertical,
                           double maxLowerPitch,
                           double thetaFF,
                           long   beamIndx,
                           double rangeToWall)
{
    _output.verticalMode = mode;
    _output.vertical = vertical;
    _output.maxLowerPitch = maxLowerPitch;
    _output.thetaFF = thetaFF;
    _output.beamIndx = beamIndx;
    _output.rangeToWall = rangeToWall;
}


void Behavior::setHorizontal(DynamicControlIF::HorizontalMode mode,
                             double horizontal)
{
    _output.horizontalMode = mode;
    _output.horizontal = horizontal;
    _output.radius = 0.;
    _output.north = _output.east = 0.;
    _output.newBearing = 0.;
    _output.newNorthing = 0.; _output.newEasting = 0.;
    _output.xTrkOffset  = 0.;
}


void Behavior::setHorizontal(DynamicControlIF::HorizontalMode mode,
                             double horizontal, double north,
                             double east)
{
    _output.horizontalMode = mode;
    _output.horizontal = horizontal;
    _output.radius = 0.;
    _output.north = north;
    _output.east = east;
    _output.newBearing = 0.;
    _output.newNorthing = 0.; _output.newEasting = 0.;
    _output.xTrkOffset  = 0.;
}

void Behavior::setHorizontal(DynamicControlIF::HorizontalMode mode,
                             double horizontal, double north,
                             double east, double radius)
{
    _output.horizontalMode = mode;
    _output.horizontal = horizontal;
    _output.radius = radius;
    _output.north = north;
    _output.east = east;
    _output.newBearing = 0.;
    _output.newNorthing = 0.; _output.newEasting = 0.;
    _output.xTrkOffset  = 0.;
}

void Behavior::setHorizontal(DynamicControlIF::HorizontalMode mode,
                             double horizontal, double north,
                             double east, double newBearing,
                             double newNorthing, double newEasting,
                             Boolean first, double xTrkOffset)
{
    _output.horizontalMode = mode;
    _output.horizontal = horizontal;
    _output.radius = 0.;
    _output.north = north;
    _output.east = east;
    _output.newBearing  = newBearing;
    _output.newNorthing = newNorthing;
    _output.newEasting  = newEasting;
    _output.xTrkOffset  = xTrkOffset;
    _output.first       = first;
}

Boolean Behavior::lowAltitudeFlightEnabled()
{
    return _lowAltitudeFlightEnabled;
}

void Behavior::setLowAltitudeFlightFlag( Boolean val )
{
    _lowAltitudeFlightEnabled = val;
}

void Behavior::resolve( void )
{
    Boolean debug = False;
    double currentTime;
    
    // A wrapper around the behavior-specific "meat"
    
    // If the behavior is finished, forget it
    if( _state == Behavior::Finished )
        return;
    
    try {
        currentTime = _missionClock->seconds();
    }
    catch (SharedObject::Error e) {
        Syslog::write("Behavior %s - caught "
                      "SharedObject::Error %s", _name, e.msg);
    }
    catch (Task::Error e) {
        Syslog::write("Behavior %s - caught Task::Error %s", _name, e.msg);
    }
    catch (...) {
        Syslog::write("Behavior %s - caught exception!\n", _name);
    }
    
    // Some checks which only need to be run once
    if( _state == Behavior::Suspended ) {
        
        // Assume start time < 0.0 means leave it up to the behavior
        if( _startTime < 0.0 ) {
            if( currentTime < _startTime ) {
                setState( Behavior::Suspended );
                return;
            }
        } else {
            if( !shouldBehaviorStart() ) {
                setState( Behavior::Suspended);
                return;
            }
        }
        
        // If we've made it this far, the behavior is active
        
        setState( Behavior::Active );
        _startTime = currentTime;
        
        if( _endTime < 0.0 )
            _endTime = _startTime + _duration;
    }
    
    // At this point, the behavior a) is known to be active
    //                             b) _startTime is set
    //                             c) _endTime is set
    
    if( currentTime > _endTime )
    {
        Syslog::write("Behavior %s - Timed out!", _name);
        setState( Behavior::Finished );
        return;
    }
    
    
    execute();
    
}

void Behavior::execute( void )
{
    Syslog::write("Calling default Behavior::execute -- This shouldn't happen");
}

Boolean Behavior::shouldBehaviorStart( void )
{
    Boolean debug = False;
    
    dprintf("Behavior::shouldBehaviorStart -- Checking to see if %s should run",_name);
    
    if( sequential() &&
       !isOutputEmpty() )
    {
        dprintf(" -- it shouldn't\n");
        return False;
    }
    
    dprintf(" -- it should\n");
    return True;
    
}

void Behavior::setSpeed(DynamicControlIF::SpeedMode mode, double speed) {
    //    Boolean debug = True;
    //    dprintf("Behavior::setSpeed -- speed[%.2lf] m[%d]\n",speed,mode);
    _output.speedMode = mode;
    _output.speed = speed;
}



const char *Behavior::stateMnem()
{
    return stateMnem( _state );
}

const char *Behavior::stateMnem( State theState)
{
    switch (theState) {
        case Active:
            return "Active";
            break;
            
        case Suspended:
            return "Suspended";
            break;
            
        case Finished:
            return "Finished";
            break;
            
        default:
            return "Unknown";
    }
}



Boolean Behavior::isOutputEmpty( DynamicControlIF::Command *cmd )
{
    return ( isHorizontalEmpty( cmd ) &&
            isVerticalEmpty( cmd ) &&
            isSpeedEmpty( cmd ) );
}

Boolean Behavior::isHorizontalEmpty( DynamicControlIF::Command *cmd )
{ return (cmd->horizontalMode == DynamicControlIF::HmInitial); }

Boolean Behavior::isVerticalEmpty( DynamicControlIF::Command *cmd )
{ return (cmd->verticalMode ==DynamicControlIF::VmInitial); }

Boolean Behavior::isSpeedEmpty( DynamicControlIF::Command *cmd )
{ return (cmd->speedMode == DynamicControlIF::SmInitial); }


void Behavior::abortMission() 
{
    _abortMission = True;
    Syslog::write("Abort request issued by behavior \"%s\"\n", name());
}


static void Behavior::setEnvironment(LayeredControl *layeredControl)
{
    _missionClock = layeredControl->missionClock();
    _navigation = layeredControl->navigation();
    _vehicleConfig = layeredControl->vehicleConfig();
    _gps    = layeredControl->gps();
    _ins    = layeredControl->ins();
    
#if 0
    _modem  = layeredControl->modem();
    _reson = layeredControl->reson();
    _ctd = layeredControl->ctd();
#endif
    
}