/****************************************************************************/
/* 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:                                                    */
/* DynoLayeredControl Integration - changes to support DynoLayeredControl
  : Made initialize() public and virtual to be called externally - RGH
  : Added level of indirection for callback() to invoke virtual function
/****************************************************************************/
#include <string.h>
#include "malloc.h"
#include "LayeredControl.h"
#include "TimeP.h"
#include "MissionPlan.h"
#include "Behavior.h"
#include "Ascend.h"
#include "Syslog.h"
#include "EventLogIF.h"
#include "VcsMessage.h"

LayeredControl::LayeredControl(const char *planName, const char *abortPlanName,
			       int period)
  : PeriodicTask(LayeredControlName), _vcsMessages(0),
  _vcsNewBehaviorTimeout(False), _vcsNewBehaviorTimeoutLimit(20),
  _vcsNewBehaviorTimer(0)
{
  Boolean debug = False;

  _systemReady = False;

  _useIns = False;

  try {
    _navigation = new NavigationIF("navigation");
    _dynamicControl = new DynamicControlIF("dynamicControl");
    _missionClock = new MissionClock();
    _vehicleConfig = new VehicleConfigurationIF("_vehicleConfig");
    _gps = new GpsIF("gps");
    if( _vehicleConfig->useIns() == 1. ) _useIns = True;
    if( _useIns ) _ins = new InsIF("kearfott", INS_AHRS_TIMEOUT);
  }
  catch (SharedObjectClient::MissingServer e) {
    Syslog::write("LayeredControl::LayeredControl() - Missing server: %s\n",
		  e.msg);
    throw;
  }
  catch (...) {
    Syslog::write("LayeredControl::LayeredControl() - caught something!\n");
    throw;
  }

  // Subscribe to Navigation "NewOutput" events
  // This event replaces the periodic callback as the
  // trigger for action.
  //
  // Then: addPeriodicCallback(period, (CallbackMethod)LayeredControl::periodicCallback);
  // Now:
  _eventService->subscribe(_navigation, NavigationIF::NewOutput,
       (EventService::Callback )eventCallback);

  // Superseded by Navigation::NewOutput event above.
  //
  // This _eventService->subscribe must be before the interfaces below get
  // opened.  If it is after the interface calls below, this routine may wait
  // for an interface call (the "new Reson6046IF", for example) to time out,
  // which can be 5 or 10 seconds.  While waiting, Dynamic Control may issue
  // it's "Ready" event, but this routine will miss it because it hasn't
  // subscribed yet.  Consequently, it never will call Dynamic Control.
  //_eventService->subscribe(_dynamicControl,
	//	   DynamicControlIF::Ready,
	//	   (EventService::Callback )eventCallback);


  //
  // Now open interfaces whose servers are started in devices.cfg.  In this
  // case we don't want to abort if the server isn't present. We will post a
  // warning.  
  //
  // 3 May 2006 rsm.  We have again moved the auxilliary device interfaces
  // (any device listed in devices.cfg) back into the behaviors, with one
  // difference - they are now declared static in Behavior.h and so are
  // global. Thus, (the idea is that ) any behavior opening an interface will
  // not create a new one if the server is already there.  We did this
  // because the timeout delay (typically 10 seconds) in the case where the
  // requested server was not in devices.cfg was causing the simulation to
  // hang, and also seemed dangerous to do on the vehicle.  
  //
  // Placing the interfaces in the behaviors re-introduces the problem that
  // checkplan doesn't work.  Fix this later some other way.
  //
#if 0
  try
  {
     _ctd = new CtdIF("ctd", "ctdDriver2");
  }
  catch (SharedObjectClient::MissingServer e) 
  {
     Syslog::write("LayeredControl::LayeredControl() - Missing server: %s; "
		   "continuing.\n",
		   e.msg);
     _ctd = NULL;
  }

  try
  {
     _reson = new Reson6046IF(Reson6046IFServerName);
  }
  catch (SharedObjectClient::MissingServer e) 
  {
     Syslog::write("LayeredControl::LayeredControl() - Missing server: %s; "
		   "continuing.\n",
		   e.msg);
     _reson = NULL;
  }
#endif
  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;
}


LayeredControl::~LayeredControl()
{
  delete _output;
  delete _input;
  delete _navigation;
  delete _dynamicControl;
  delete _missionClock;
  delete _vehicleConfig;
  delete _gps;
  if(_useIns) delete _ins;

  //free up behavior stacksss. This is necessary to closethe log files
  int i;
  for (i = _normalBehaviorStack.size() - 1; i>=0; i--) {
        Behavior *behavior;
	_normalBehaviorStack.get(i, &behavior);
        delete behavior;
  }

  for (i = _abortBehaviorStack.size()-1; i>=0;i--) {
	Behavior *behavior;
	_abortBehaviorStack.get(i,&behavior);
	delete behavior;
  }
//if(_reson) delete _reson;
}


void LayeredControl::log(int millisec)
{
}
  

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


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


// initialize() must be called externally (no longer in ctor)
//
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::periodicCallback()
{
  this->callback(); // Invoke callback() from virtual function table
}

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

  dprintf("Inside LayeredControl::callback()\n");

  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
  try {
    executeBehaviors(behaviorStack, &abortRequested, &stackFinished );
  }
  catch (Exception e) {
    Syslog::write("LayeredControl::callback() - Caught exception %s\n",
                e.msg);
    throw;
  }
  
  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();
  
  static int dbcount = 0;
/*
  if ((++dbcount % 7)==0) {
    Syslog::write("LayeredControl _resolvedCommand - "
          "vertMode=%d vert=%.2f\n\thorzlMode=%d horz=%.2f\n\tspeedMode=%d speed=%.2f\n",
          _resolvedCommand.verticalMode, _resolvedCommand.vertical,
          _resolvedCommand.horizontalMode, _resolvedCommand.horizontal,
          _resolvedCommand.speedMode, _resolvedCommand.speed);
    dbcount = 0;
  }
*/
}

// Check time and set new behavior timeout flag accordingly.
// Return timeout flag (true means we've timed-out)
//
Boolean LayeredControl::UpdateVcsNewBehaviorTimeout()
{
  if (0 == _vcsNewBehaviorTimer)
  {
    //time(&_vcsNewBehaviorTimer);
    // Timer is zero, set it to current time
    struct timespec ts;
    Time::gettime(&ts);
    _vcsNewBehaviorTimer = ts.tv_sec;
  }
  else
  {
    //time(&now);
    // Check to see if we've timed-out waiting for a new behavior
    //
    struct timespec ts;
    Time::gettime(&ts);
    long now = ts.tv_sec;
    if ((now - _vcsNewBehaviorTimer) > _vcsNewBehaviorTimeoutLimit)
    {
      Syslog::write("LayeredControl::UpdateVcsNewBehaviorTimeout() - timeout!");
      _vcsNewBehaviorTimeout = True;
    }
  }
  return _vcsNewBehaviorTimeout;  
}

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

  Behavior::State prevState, newState;
  int newActiveIndex = -1;
  int newActiveId    = -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\", id=%d, %s...\n", behavior->name(),
        behavior->id(), behavior->stateMnem());
       behavior->resolve();

       newState = behavior->state();
        
       if( newState != prevState ) {
	    Syslog::write("LayeredControl::execute() -- "
			  "(t = %lf) Behavior %s:%d has changed to state %s\n", 
			  _missionClock->seconds(),
			  behavior->name(),
			  behavior->id(),
			  behavior->stateMnem() );
			  
      if (newState == Behavior::Finished) {
        // We only need to send these messages when we're in DynoLayeredControl mode
        if (_vcsMessages)
        {
          Syslog::write("LayeredControl - sending BehaviorFinished:%d msg to VcsServer",
                behavior->id());
          VcsMessage::Message msg(VcsMessage::BehaviorFinished, behavior->id());
          _vcsMessages->write(&msg);
        }
      }
      
	    if (newState == Behavior::Active) {
	      newActiveIndex = priority;
	      newActiveId    = behavior->id();
	      newActiveName = (char *)behavior->name();

          // We only need to send these messages when we're in
          // DynoLayeredControl mode (_vcsMessages is non-NULL)
          //
          if (_vcsMessages)
          {
            vcsLog = False;
            Syslog::write("LayeredControl - %s BehaviorStarted:%d msg to VcsServer",
            behavior->name(), behavior->id());
            VcsMessage::Message msg(VcsMessage::BehaviorStarted, behavior->id());
            _vcsMessages->write(&msg);
          }
	    }
      }
       
       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 _vcsMessages != NULL then we're in Dyno mode where we do not
  // abort when the stack is empty
  //
  if( isOutputEmpty( &_workingCommand )) {
  
    if(!_vcsMessages) {
      // Normal mode (not DynoLayeredControl)
      Syslog::write("Stack empty after processing.  Aborting.");
      *stackFinished = True;
    }
    else {
      // Dyno mode. Stack is empty, but let's wait and give the
      // AMC an opportunity to pop a new behavior on the stack
      //
      if (!vcsLog) {
        vcsLog = True;
        Syslog::write("\n***\n\t\tStack empty after processing. Waiting on VCS...\n");
      }
      
      // Has stack been empty too long?
      if (UpdateVcsNewBehaviorTimeout())
      {
        Syslog::write("LayeredControl - VCS too slow submitting new behavior");
        Syslog::write("Stack empty after processing.  Aborting.");
        *stackFinished = True;  // Now Dyno is really finished
      }
    }       
  }
  else
  {
    ResetVcsNewBehaviorTimer(); 
    if (newActiveIndex >= 0)
    {
      // A behavior just became active; update output object for server
      _output->data.behaviorIndex = newActiveIndex;
      _output->data.behaviorId    = newActiveId;
      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");
//  VcsMessage::Message msg(VcsMessage::MissionAborted, -1);
//  _vcsMessages->write(&msg);
  
  if( _dynamicControl->newGains() ) _dynamicControl->revertGains();

  _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()
{
    Boolean debug=True;
    dprintf("\n****LayeredControl::initiateShutdown - enter\n");
    
	// do shutdown here
    dprintf("\n****LayeredControl::initiateShutdown - exit to system\n");
    exit(0);
    
// original code
//     Initiate shutdown of the vehicle. For now, we just exit; this
//     will be sensed by Supervisor, which will then shut down all tasks.
//  exit(0);
}



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... ***");
    triggerEvent(LayeredControlIF::MissionStatusChange);
    initiateShutdown();
    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;
}



VehicleConfigurationIF *LayeredControl::vehicleConfig()
{
  return _vehicleConfig;
}



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

GpsIF *LayeredControl::gps()
{
   return _gps;
}

InsIF *LayeredControl::ins()
{
   return _ins;
}

#if 0

Reson6046IF* LayeredControl::reson()
{
  return _reson;
}

CtdIF *LayeredControl::ctd()
{
  return _ctd;
}

#endif

void LayeredControl::eventCallback(TaskInterface *taskInterface,
				   EventCode eventCode)
{
   Boolean debug = False;

   //Syslog::write("LayeredControl::eventCallback() - triggered");
   if (taskInterface == _navigation && eventCode == NavigationIF::NewOutput) 
   {
      _systemReady = True;
      // Just invoke the periodicCallback as before
      try {
        periodicCallback();
      }
      catch (Exception e) {
        Syslog::write("LayeredControl::eventCallback() - Caught exception %s\n",
                    e.msg);
        throw;
      }
      
      // Trigger DynamicControl
      // Notify subscribers of new navigation output
      triggerEvent(LayeredControlIF::OutputGenerated);
   }
}