/* 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 INCLUDED_VisualServo_h
#define INCLUDED_VisualServo_h

#include "state_machine.h"
#include "Behavior.h"
#include "../Main/Events.h"

namespace Motion {
  class MotionCommand;
}

class Vision;

class VisualServo : public IndependentBehavior {
public:
  static char const * const beh_name;

  static const int NumStates = 2;
  enum State { CORRECTING, SASTISFIED };

  static char const * const state_names[NumStates];
  
  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

private:
  uint vis_id;
  Vision *vision;
  Motion::MotionCommand *command;
  EventCacheTracker cache_tracker;

public:
  static EventProcessor *create() {return new VisualServo;}

  VisualServo();
  virtual ~VisualServo();

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

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

#endif