/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : LayeredControl.h                                              */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#ifndef _LAYEREDCONTROL_H
#define _LAYEREDCONTROL_H

#include "Behavior.h"
#include "PeriodicTask.h"
#include "LayeredControlIF.h"
#include "Navigation.h"
#include "DynamicControlIF.h"
#include "MissionClock.h"
#include "DynamicArray.h"
#include "LayeredControlInput.h"
#include "LayeredControlOutput.h"
#include "VehicleConfigurationIF.h"
#include "GpsIF.h"
#include "InsIF.h"
//#include "CtdIF.h"
//#include "Reson6046IF.h"

// DEBUG PURPOSES ONLY!!!
#include "SimulatorIF.h"


#define LayeredControlName "layeredControl"

/*
CLASS 
LayeredControl

DESCRIPTION

LayeredControl invokes Behaviors specified by the MissionPlan,
and resolves commands generated by currently-executing Behaviors to one
final output command, which is input to the DynamicController.  

Currently implements a limited version of "state-configured layered control".
Main states are NormalState, AbortingMission, and ShuttingDown.

AUTHOR
Tom O'Reilly

DynoLayeredControl Integration - changes made to support DynoLayeredControl
  : virtual void initialize() MUST be called after creating the object - RGH
  : periodicCallback() used to allow virtual callback() routines
*/

class VcsMessage;

class LayeredControl : public PeriodicTask {


public:


  ///////////////////////////////////////////////////////////////////
  // Log data to file
  virtual void log(int millisec);
  
  ///////////////////////////////////////////////////////////////////
  // Stringify current status
  virtual void status(char *str);

  ///////////////////////////////////////////////////////////////////
  // Stringify "current data"
  virtual void currentData(char *str);

  ///////////////////////////////////////////////////////////////////
  // Constructor
  LayeredControl(const char *planName, const char *abortPlanName, 
		 int period);

  virtual ~LayeredControl();

     ///////////////////////////////////////////////////////////////////
     // Reset output command 
     static void resetOutput(DynamicControlIF::Command *cmd);
     
     ///////////////////////////////////////////////////////////////////
     // 
     static Boolean isOutputEmpty( DynamicControlIF::Command * );
     static Boolean isVerticalEmpty( DynamicControlIF::Command *cmd );
     static Boolean isHorizontalEmpty( DynamicControlIF::Command *cmd );
     static Boolean isSpeedEmpty( DynamicControlIF::Command *cmd );
     
     static void copyOutput( DynamicControlIF::Command *dest,
			     DynamicControlIF::Command *src );


  ///////////////////////////////////////////////////////////////////
  // Navigation interface
  NavigationIF *navigation();

  ///////////////////////////////////////////////////////////////////
  // Mission clock interface
  MissionClock *missionClock();

  ///////////////////////////////////////////////////////////////////
  // Vehicle configuration file interface
  VehicleConfigurationIF *vehicleConfig();

  ///////////////////////////////////////////////////////////////////
  // Gps interface
  GpsIF *gps();

  ///////////////////////////////////////////////////////////////////
  // Ins (Kearfott) interface
  InsIF *ins();

  ///////////////////////////////////////////////////////////////////
  // Reson interface
  Reson6046IF *reson();

  ///////////////////////////////////////////////////////////////////
  // CTD IF
  CtdIF *ctd();

  ///////////////////////////////////////////////////////////////////
  // Initialize; read MissionPlan files - must be called externally
  // by LayeredControl object owner
  virtual void initialize(const char *planName, const char *abortPlanName);

  ///////////////////////////////////////////////////////////////////
  // Set the amount of time layered control will sit idle with no
  // sequential behaviors in the stack before aborting
  //
  virtual void setVcsNewBehaviorTimeoutLimit(unsigned limit_in_seconds)
  { _vcsNewBehaviorTimeoutLimit = limit_in_seconds; }

protected:

  double lastCallback;

  ///////////////////////////////////////////////////////////////////
  // BehaviorStacks for mission states. Priority of each Behavior is
  // specified by position in the array; lowest array index is
  // highest priority. Each MissionState value corresponds to a
  // behavior stack.
  BehaviorStack _normalBehaviorStack;
  BehaviorStack _abortBehaviorStack;

  ///////////////////////////////////////////////////////////////////
  // Execute and "resolve" behaviors - supports derived classes
  virtual void callback();
  
  void periodicCallback(); // Called by framework to invoke virtual callback()

  ///////////////////////////////////////////////////////////////////
  // Point to BehaviorStack corresponding to current MissionState
  BehaviorStack *currentBehaviorStack();

  ///////////////////////////////////////////////////////////////////
  // Execute each runnable behavior; i.e. generate commands
  void executeBehaviors(BehaviorStack *behaviorStack, 
			Boolean *abortRequested,
			Boolean *stackFinished );

  ///////////////////////////////////////////////////////////////////
  // Final resolved command
  DynamicControlIF::Command _resolvedCommand;
  DynamicControlIF::Command _workingCommand;

  ///////////////////////////////////////////////////////////////////
  // Initiate mission abort - support derived classes
  virtual void initiateAbort();

  ///////////////////////////////////////////////////////////////////
  // Initiate vehicle shut-down.
  void initiateShutdown();

  ///////////////////////////////////////////////////////////////////
  // Load default abort stack
  void loadDefaultAbort();

  enum StateAdvanceCondition {
    StackFinished,
    AbortRequested
  };

  ///////////////////////////////////////////////////////////////////
  // Advance the MissionState
  void advanceState(StateAdvanceCondition condition);

  Boolean _missionStarted;
  Boolean _systemReady; 

  NavigationIF *_navigation;
  DynamicControlIF *_dynamicControl;
  GpsIF *_gps;
  InsIF *_ins;

  MissionClock *_missionClock;
  VehicleConfigurationIF *_vehicleConfig;
  
//CtdIF *_ctd;
//Reson6046IF *_reson;

  Boolean _useIns;
  
  ///////////////////////////////////////////////////////////////////
  // Called when subscribed-to events occur
  void eventCallback(TaskInterface *taskInterface, EventCode eventCode);

  Boolean UpdateVcsNewBehaviorTimeout();
  void ResetVcsNewBehaviorTimer()
  { _vcsNewBehaviorTimer = 0; _vcsNewBehaviorTimeout = False; } 

  // DEBUG PURPOSES ONLY!!!
  // SimulatorIF *_simulator;

  LayeredControlInput *_input;
  LayeredControlOutput *_output;
  VcsMessage  *_vcsMessages;
  Boolean  _vcsNewBehaviorTimeout;
  long _vcsNewBehaviorTimer;
  unsigned _vcsNewBehaviorTimeoutLimit;
  
  
  LayeredControlIF::MissionState _missionState;
};

#endif