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

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

class GetNearBall : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeGetBall *get_ball; 
  BeGetBallInfo gb_info;
  BeGetNearBall *gnb;
  
  ulong start_time;
  static const ulong timeout = 20000000L;

  MotionCommand out_command;

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

  GetNearBall();
  virtual ~GetNearBall();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);


  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class SearchBallClose : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  FindBallNear *fbn; 
   
  ulong start_time;
  static const ulong timeout = 5000000L;

  MotionCommand out_command;

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

  SearchBallClose();
  virtual ~SearchBallClose();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);


  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class KickClear : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeApproachBall *approach;
  BeTurn *turn;

  double approach_retval;
  double turn_retval;
  double turn_angle;
  Kick kick;

  ulong start_time;
  static const ulong timeout = 7000000L;

  MotionCommand out_command;

  static const int NumStates = 5;
  enum State { APPROACH, STAND, AIM, HOLD_BALL, KICK};

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

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

  KickClear();
  virtual ~KickClear();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

// You shouldn't refer to a KickToPoint from within a decision
// tree. This class is here only to prevent needless duplication of
// code in other behavior sequences. Use ShootOnGoal or PassToTeammate
// instead, or write your own class that uses KickToPoint directly.
class KickToPoint : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeApproachBall *approach;
  BeTurn *turn;
  bool kicked;
  Kick kick;
  double turn_angle;

  vector2d target;
  int kick_mask;
  bool kickThroughTarget;

  double approach_retval;
  double turn_retval;
  ulong start_time;
  static const ulong timeout = 8000000L;

  MotionCommand out_command;

  static const int NumStates = 4;
  enum State { APPROACH, STAND, AIM, KICK};

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

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

  KickToPoint();
  virtual ~KickToPoint();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  // target should be a vector, in global coordinates, of where we
  // intend to kick to.  kickThroughTarget should be true if you'd
  // like the kick to go through the target, or false if you'd like
  // the kick to go to the target. It's used by KickLib to choose
  // which kick to use; if we want to kick *to* a point near us, it
  // should choose a weaker kick than if we want to kick *through* it.
  void setTarget(vector2d target, bool kickThroughGoal);
  //sets the kick mask to be used when selecting a kick from KickLib
  void setKickMask(int mask);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/
class PassToTeammate : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  MotionCommand out_command;

  bool isNewSequence;

  KickToPoint *kickToPoint;

  static const int NumStates = 1;
  enum State { SHOOT };

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

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

  PassToTeammate();
  virtual ~PassToTeammate();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/
class ShootOnGoal : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  MotionCommand out_command;

  KickToPoint *kickToPoint;

  static const int NumStates = 1;
  enum State { SHOOT };

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

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

  ShootOnGoal();
  virtual ~ShootOnGoal();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/
class InterceptBall : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  int sequenceTime;
  bool done;

  MotionCommand out_command;

  static const int NumStates = 1;
  enum State { INTERCEPT };

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

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

  InterceptBall();
  virtual ~InterceptBall();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class KickNearObstacle : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeApproachBall *approach;
  BeTurn *turn;
  BeTrackObjects *track_objects;
  bool kicked;
  bool turn_side;
  bool ball_far;
  bool should_punch;
  double turn_thresh;
 

  ulong start_time;
  ulong start_time_circle;
  static const ulong timeout = 4000000L;
  static const ulong circle_timeout = 2500000L;
  double approach_retval;
  double turn_retval;

  MotionCommand out_command;

  static const int NumStates = 2;
  enum State { PUNCH, CIRCLE};

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

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

  KickNearObstacle();
  virtual ~KickNearObstacle();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class Dribble : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeApproachBall *approach;
  BeTurn *turn;
  BeDribble *dribble;

  double dribble_retval;
  bool dribble_done;
  vector2d target_pt;
  ulong start_time;
  static const ulong timeout = 4000000L;
  double approach_retval;
  double turn_retval;

  MotionCommand out_command;

  static const int NumStates = 3;
  enum State { APPROACH, DRIBBLE, WALK};

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

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

  Dribble();
  virtual ~Dribble();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class WalkDribble : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeTrackObjects *track_objects;

  vector2d gpoint;
  bool side_set;

  MotionCommand out_command;

  static const int NumStates = 2;
  enum State { APPROACH_SIDE, WALK };

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

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

  WalkDribble();
  virtual ~WalkDribble();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class TurnWithBall : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeApproachBall *approach;

  double approach_retval;
  double direction;
  bool holding;

  Kick kick;
  int num_saw_goal_frames;
  ulong start_time;
  static const ulong timeout = 7000000L;

  MotionCommand out_command;

  static const int NumStates = 4;
  enum State { APPROACH, AIM, KICK, ABORT};

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

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

  TurnWithBall();
  virtual ~TurnWithBall();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class WallTurn : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeTrackObjects *track_objects;

  bool turned;

  ulong start_time;
  static const ulong timeout = 3000000L;

  MotionCommand out_command;

  static const int NumStates = 2;
  enum State { APPROACH, TURN };

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

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

  WallTurn();
  virtual ~WallTurn();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class WallTurnSoft : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  BeTrackObjects *track_objects;

  bool turned;

  ulong start_time;
  static const ulong timeout = 3000000L;

  MotionCommand out_command;

  static const int NumStates = 2;
  enum State { APPROACH, TURN };

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

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

  WallTurnSoft();
  virtual ~WallTurnSoft();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class StandScanMarkers : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;
  
  ulong start_time;
  static const ulong timeout = 1300000;

  MotionCommand out_command;

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

  StandScanMarkers();
  virtual ~StandScanMarkers();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);


  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};


/*===========================================================*/

class FrontWall : public BehaviorSequence {

 public:

  static const int beh_id;
  static char const * const beh_name;
  static const int NumStates=2;
  enum State {BUMP, PUSH};

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

  FSM fsm;

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  MotionCommand out_command;

  BeTrackObjects* track_objects;

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

  FrontWall();
  virtual ~FrontWall();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

/*===========================================================*/

class Default : public BehaviorSequence {

 public:

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

private:
  EventManager *event_mgr;

  uint fs_id;
  EventProcessor *fs_ep;
  FeatureSet *fs;

  MotionCommand out_command;

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

  Default();
  virtual ~Default();

  double operator()(FeatureSet *features, Motion::MotionCommand *Command);

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual double status(FeatureSet *features);
  virtual void reset(ulong time);

  virtual bool update(ulong time,const EventList *events){return true;}
  virtual const MotionCommand *get(ulong time);
};

#endif