/****************************************************************************/
/* 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"

// 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

*/
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);

  ~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();

protected:


  double lastCallback;

  ///////////////////////////////////////////////////////////////////
  // Initialize; read MissionPlan files
  void initialize(const char *planName, const char *abortPlanName);

  ///////////////////////////////////////////////////////////////////
  // 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
  void 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
  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;

  MissionClock *_missionClock;

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

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

  LayeredControlInput *_input;
  LayeredControlOutput *_output;

  LayeredControlIF::MissionState _missionState;
};

#endif