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

#include "state_machine.h"
#include "Behavior.h"
#include "FeatureSet.h"
#include "../Main/Events.h"
#include "../Motion/MotionInterface.h"
#include "../headers/Sensors.h"
#include "../Logger/LoggerInterface.h"
#include "../Vision/VisionInterface.h"
#include "../Motion/Kinematics.h"
#include "../Motion/RobotConstants.h"

class Camera : public IndependentBehavior {

  Kinematics kin;

 public:

  static const int beh_id;
  
  static char const * const beh_name;

  static const int NumStates = 5;
  enum State { JOYSTICK, WAIT_FOR_PIC, TOOK_PIC, WRITE_LOG, WAIT_FOR_WRITE };

  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;

  MotionCommand out_command;

  uint fs_id;
  uint sd_id;

  FeatureSet *fs;
  SensorData *sd;
public:

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

  Camera();
  virtual ~Camera();

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

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

#endif