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

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

class ChaseBall : public IndependentBehavior {
public:
  static const int beh_id;

  static char const * const beh_name;

  static const int NumKicks = 5;
  static const int kick_ids[NumKicks];

  static const int NumStates = 5;
  enum State {GOTO_BALL, HEAD_SEARCH_BALL, SPIN_SEARCH_BALL, WALK_SEARCH_BALL, KICK};

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;
  ulong SpinSearchTime;

  MotionCommand out_command;

  BeLookAtObject *LookAtObject;
  BeLookAtObjectInfo *LookAtObjectInfo;
  Turn *TestTurn;

  uint fs_id;
  FeatureSet *fs;

  int kick_index;

  // sign of ball y position when we last saw it
  int ball_side_last_seen;

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

  ChaseBall();
  virtual ~ChaseBall();

  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);
};

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

private:
  MotionCommand out_command;

  uint fs_id;
  FeatureSet *fs;

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

  Getup();
  virtual ~Getup();

  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);
};

class KickTest {
private:
  static const int NumStates = 6;
  enum States {GOTO_BALL, HEAD_SEARCH_BALL, SPIN_SEARCH_BALL_INIT, SPIN_SEARCH_BALL, WALK_SEARCH_BALL, KICK};

  int CurrentState;
  ulong Timer;
  ulong SpinSearchTime;
  int CurrentKickIdx;

  BeLookAtObject *LookAtObject;
  BeLookAtObjectInfo *LookAtObjectInfo;

public:
  KickTest();
  ~KickTest();

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

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

  static const int NumStates = 2;
  enum State {STAND, KICK};

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  MotionCommand out_command;

  BeLookAtObject *LookAtObject;
  BeLookAtObjectInfo *LookAtObjectInfo;

  uint fs_id;
  FeatureSet *fs;

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

  StaticKickTest();
  virtual ~StaticKickTest();

  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);
};

// Localization challenge behavior
class LocalizationTest : public IndependentBehavior {
public:
  static char const * const beh_name;
  
  static const int NumStates=7;
  enum State { INIT, ORDER_POINTS, START_GLOBAL_LOCALIZE, GLOBAL_LOCALIZE, GOTO_WAYPOINT, INPLACE_GLOBAL_LOCALIZE, WAG_TAIL };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  BeGotoPointGlobal *goto_point;
  BeTrackObjects *track_objects;
  bool global_localizing;
  vector2d goal_loc;

  MotionCommand out_command;

  uint fs_id;
  FeatureSet *fs;

  // resulting pointer should be released with delete[]
  void read_waypoints(vector3d **waypoints,int *num_waypoints);
public:
  static EventProcessor *create() {return new LocalizationTest;}

  LocalizationTest();
  virtual ~LocalizationTest();

  void reset();
  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