/****************************************************************************/
/* 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 "LayeredControl.h"
#include "Syslog.h"

const Boolean Behavior::Sequential = True;
const Boolean Behavior::NonSequential = False;
MissionClock *Behavior::_missionClock = 0;
NavigationIF *Behavior::_navigation = 0;

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


Behavior::~Behavior()
{
  for (int i = 0; i < attributes.list.size(); i++) {
    Attribute *attribute;
    attributes.list.get(i, &attribute);
    delete attribute;
  }

  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) {
  _output.verticalMode = mode;
  _output.vertical = vertical;
}


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


void Behavior::setHorizontal(DynamicControlIF::HorizontalMode mode, 
			     double horizontal, double north, 
			     double east)
{
  _output.horizontalMode = mode;
  _output.horizontal = horizontal;
  _output.north = north;
  _output.east = east;
}


void Behavior::resolve( void )
{
     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 ) {
	  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 = True;

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