/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#ifndef SpinDog_h
#define SpinDog_h

#include "state_machine.h"
#include "Behavior.h"
#include "FeatureSet.h"
#include "../Main/Events.h"
#include "../Motion/MotionInterface.h"

class SpinDog : public IndependentBehavior {

 public:

  // Every behavior has a unique numeric ID
  static const int beh_id;
  
  // Every behavior also has a unique name that you place in
  // run.cfg to tell the behavior system to run it. This doesn't
  // *need* to match the class name, but it's a good idea that
  // it does. Remember that the name needs to be unique.
  static char const * const beh_name;

  // Behaviors are finite state machines. Don't worry about this
  // too much for now. In this behavior, we'll have 2 states -
  // spinning counter-clockwise and spinning clockwise.
  static const int NumStates = 2;
  enum State { SPIN_CCW, SPIN_CW };

  // Name our states in a human-readable form. It's possible to
  // send the name of the current state over the network to
  // a workstation to figure out what the robot is actually
  // doing at a given time. It's also useful to look at transitions
  // between states and detect when your behavior system is in
  // an infinite loop.
  static char const * const state_names[NumStates];
  
  // This is just for convenience.
  typedef FiniteStateMachine<State,ulong> FSM;

private:

  // This is the finite state machine that tracks what our current
  // state is, which transitions we've followed, etc.
  FSM fsm;

  // You can add whatever variables here you need to track your state.

  // This is a buffer that will hold the motion commands that we will
  // tell the robot to execute each frame.
  MotionCommand out_command;

  // the id of the FeatureSet event processor from the event system
  uint fs_id;

  // a pointer to the FeatureSet even processor itself - we'll call
  // it's get method in order to obtain up to date information about
  // the world.
  FeatureSet *fs;

public:

  // This is important. It's how the behavior system gets an
  // instance of our behavior.
  static EventProcessor *create() { return new SpinDog; }

  SpinDog();
  virtual ~SpinDog();

  void reset(ulong time);
  double operator()(FeatureSet *FS, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual bool update(ulong time,const EventList *events);
  virtual const MotionCommand *get(ulong time);
};

#endif