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

LayeredControl::LayeredControl(const char *planName, const char *abortPlanName,
			       int period)
  : PeriodicTask(LayeredControlName)
{
  Boolean debug = False;

  _systemReady = False;

  addPeriodicCallback(period, (CallbackMethod )LayeredControl::callback);

  try {
    _navigation = new NavigationIF("navigation");
    _dynamicControl = new DynamicControlIF("dynamicControl");
    _missionClock = new MissionClock();
  }
  catch (SharedObjectClient::MissingServer e) {
    Syslog::write("LayeredControl::LayeredControl() - Missing server: %s\n",
		  e.msg);
    throw;
  }
  catch (...) {
    Syslog::write("LayeredControl::LayeredControl() - caught something!\n");
    throw;
  }

  _eventService->subscribe(_dynamicControl,
			   DynamicControlIF::Ready,
			   (EventService::Callback )eventCallback);

  try {
    _input = new LayeredControlInput(SharedData::Read, True);
  }
  catch (...) {
    Syslog::write("LayeredControl::LayeredControl() - "
		  "LayeredControlInput constructor failed\n");

    return;
  }


  try {
    _output = new LayeredControlOutput(SharedData::Write, True);
  }
  catch (...) {
    Syslog::write("LayeredControl::LayeredControl() - "
		  "LayeredControlOutput constructor failed\n");

    return;
  }

  // Initialize command structure
  resetOutput(&_resolvedCommand);

  lastCallback = 0.0;

  initialize(planName, abortPlanName);
}


LayeredControl::~LayeredControl()
{
  delete _navigation;
  delete _dynamicControl;
  delete _missionClock;
  delete _input;
  delete _output;
}


void LayeredControl::log(int millisec)
{
}
  

void LayeredControl::status(char *str)
{
}


void LayeredControl::currentData(char *str)
{
}


void LayeredControl::initialize(const char *planName, 
				const char *abortPlanName)
{
  // Load environment for all Behaviors
  Behavior::setEnvironment(this);

  _missionStarted = False;

  // Initialize mission state
  _missionState = LayeredControlIF::Normal;

  MissionPlan missionPlan;

  if (missionPlan.load(abortPlanName, &_abortBehaviorStack) == -1) {
    // Failed to load "abort" mission plan.
    Syslog::write("*** LayeredControl::initialize() - "
		  "couldn't load ABORT plan \"%s\"\n", abortPlanName);

    initiateShutdown();
  }
  else if (missionPlan.load(planName, &_normalBehaviorStack) == -1) {
    // Abort mission
    Syslog::write("*** LayeredControl::initialize() - "
		  "couldn't load NORMAL plan \"%s\"\n", planName);

    initiateShutdown();
  }

}


void LayeredControl::callback()
{
  Boolean debug = False;

  if (!_systemReady)
    // Rest of vehicle system not yet ready
    return;

  if (!_missionStarted) {
    // Just starting mission now; reset clock 
    _missionClock->reset();
  }

  // Check for abort request from server
  _input->read();
  if (_input->data.abort && _missionState == LayeredControlIF::Normal) {
    Syslog::write("LayeredControl::callback() - "
		  "got abort request from server");

    initiateAbort();
  }

  // Point to behavior stack corresponding to current state
  BehaviorStack *behaviorStack = currentBehaviorStack();

  if (behaviorStack == 0) {
    // Unknown state
    Syslog::write("LayeredControl::callback() - "
		  "Bad behavior stack, unknown state!\n");
    return;
  }

  Boolean stackFinished = False;
  Boolean abortRequested = False;

  resetOutput( &_workingCommand );
  
  // Generate command for each active behavior in stack
  executeBehaviors(behaviorStack, &abortRequested, &stackFinished );
  
  if( stackFinished ) {
       advanceState(StackFinished);
       return;
  }
  else if (abortRequested) {
       // Go to next mission state
       advanceState(AbortRequested);
       return;
  }

  copyOutput( &_resolvedCommand, &_workingCommand );

  dprintf("LayeredControl::callback() - "
	  "verticalMode=%d, horizontalMode=%d, speedMode=%d\n", 
	  _resolvedCommand.verticalMode, _resolvedCommand.horizontalMode,
	  _resolvedCommand.speedMode);

  dprintf("LayeredControl::callback() - vertical=%.2f, horizontal=%.2f, "
	  "speed=%.2f\n", 
	  _resolvedCommand.vertical, _resolvedCommand.horizontal,
	  _resolvedCommand.speed);

  // Send command to DynamicControl
  _dynamicControl->setCommand(&_resolvedCommand);

  if (!_missionStarted) {

    // Mission just started
    _missionStarted = True;

    // Notify subscribers of mission start
    triggerEvent(LayeredControlIF::MissionStarted);

    Syslog::write("*** Mission started ***\n");
  }
  _output->data.elapsedSeconds = _missionClock->seconds();
  _output->write();
}


void LayeredControl::executeBehaviors(BehaviorStack *behaviorStack,
				      Boolean *abortRequested,
				      Boolean *stackFinished )
{
  Boolean debug = False;
  *abortRequested = False;

  Behavior::State prevState, newState;
  int newActiveIndex = -1;
  char *newActiveName = 0;

  // Start at low-priority end of stack and work towards high priorities
  for (int priority = behaviorStack->size() - 1; priority >= 0; priority--) {

       Behavior *behavior;
       behaviorStack->get(priority, &behavior);
       
       dprintf("LayeredControl::executeBehaviors -- Trying behavior %d: %s", 
	       priority, behavior->name() );

       if ( behavior->state() == Behavior::Finished )
	    continue;
       
       // Initialize fields of behavior's output command

       behavior->input( &_workingCommand );
       
       prevState = behavior->state();

       // Execute the behavior, i.e. generate command
       dprintf("Executing Behavior \"%s\"...\n", behavior->name());
       behavior->resolve();

       newState = behavior->state();

       if( newState != prevState ) {
	    Syslog::write("LayeredControl::execute() -- "
			  "(t = %lf) Behavior %s has changed to state %s\n", 
			  _missionClock->seconds(),
			  behavior->name(),
			  behavior->stateMnem() );
			  

	    if (newState == Behavior::Active) {
	      newActiveIndex = priority;
	      newActiveName = (char *)behavior->name();
	    }
       }
       
       behavior->output( &_workingCommand );
       
       if (behavior->abortRequested()) {
	    // Behavior issued mission abort request
	    *abortRequested = True;
	    return;
       }
  }


  // If the stack is empty after processing the stack,
  // consider it "done"
  if( isOutputEmpty( &_workingCommand ) ) {
       Syslog::write("Stack empty after processing.  Aborting.");
       *stackFinished = True;
  }
  else if (newActiveIndex >= 0) {
    // A behavior just became active; update output object for server
    _output->data.behaviorIndex = newActiveIndex;
    strncpy(_output->data.behaviorName, newActiveName, 
	    sizeof(_output->data.behaviorName)-1);
    _output->data.behaviorName[sizeof(_output->data.behaviorName)-1] = '\0';
    _output->write();
  }

  return;
}

void LayeredControl::resetOutput(DynamicControlIF::Command *cmd)
{
  cmd->horizontalMode = DynamicControlIF::HmInitial;
  cmd->verticalMode = DynamicControlIF::VmInitial;
  cmd->speedMode = DynamicControlIF::SmInitial;
  cmd->horizontal = cmd->north = cmd->east = 0.;
  cmd->vertical = 0.;
  cmd->speed = 0.;
}

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

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

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

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

void LayeredControl::copyOutput( DynamicControlIF::Command *dest,
				 DynamicControlIF::Command *src )
{
     memcpy( (void *)dest, (void *)src, 
	     sizeof( DynamicControlIF::Command) );
}



BehaviorStack *LayeredControl::currentBehaviorStack() 
{
  // Point to BehaviorStack corresponding to current mission state.
  // (Right now we only recognize two states; "Normal" and "Aborting")
  switch (_missionState) {

  case LayeredControlIF::Normal:
    return &_normalBehaviorStack;
    break;

  case LayeredControlIF::Aborting:
    return &_abortBehaviorStack;
    break;
  }

  return 0;
}


void LayeredControl::initiateAbort()
{
  Syslog::write("Initiating Mission Abort!\n");
  _missionState = LayeredControlIF::Aborting;
  _output->data.missionState = LayeredControlIF::Aborting;
  _output->write();
}



void LayeredControl::loadDefaultAbort()
{
  _abortBehaviorStack.clear();
  Behavior *ascend = new Ascend();
  _abortBehaviorStack.add(&ascend);
}


void LayeredControl::initiateShutdown()
{
  // Initiate shutdown of the vehicle. For now, we just exit; this
  // will be sensed by Supervisor, which will then shut down all tasks.
  exit(1);
}



void LayeredControl::advanceState(StateAdvanceCondition condition)
{
  LayeredControlIF::MissionState newState;

  switch (_missionState) {

  case LayeredControlIF::Normal:

    // Normal stack is done; just abort for now
    newState = LayeredControlIF::Aborting;
    Syslog::write("*** LayeredControl - initiating mission abort... ***");
    initiateAbort();
    newState = LayeredControlIF::Aborting;
    triggerEvent(LayeredControlIF::MissionStatusChange);
    break;

  case LayeredControlIF::Aborting:
    newState = LayeredControlIF::ShuttingDown;
    Syslog::write("*** LayeredControl - initiating vehicle shutdown... ***");
    initiateShutdown();
    triggerEvent(LayeredControlIF::MissionStatusChange);
    break;

  case LayeredControlIF::ShuttingDown:
    newState = LayeredControlIF::Done;
    break;

  default:
    Syslog::write("LayeredControl::advanceState() - unknown state!\n");
    exit(1);
  }

  _missionState = newState;
  _output->data.missionState = _missionState;
  _output->write();
}



NavigationIF *LayeredControl::navigation()
{
  return _navigation;
}



MissionClock *LayeredControl::missionClock()
{
  return _missionClock;
}


void LayeredControl::eventCallback(TaskInterface *taskInterface,
				   EventCode eventCode)
{
  if (taskInterface == _dynamicControl) {

    if (eventCode == DynamicControlIF::Ready) {
      // DynamicControl and Navigation are ready, so start mission
      _systemReady = True;
    }
  }
}