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

#include "BeLowLevel.h"
#include "Behaviors.h"
#include "Behavior.h"
#include "FeatureSet.h"
#include "BePrimaryAttacker.h"
#include "state_machine.h"

#include "../headers/Util.h"
#include "../headers/Utility.h"
#include "../WorldModel/WorldModel.h"
#include "BehaviorPacketEncoder.h"
#include "Attacker.h"

class Goalie : public IndependentBehavior {

public:
  static char const * const beh_name;

  static const int beh_id;

  static const int NumStates=8;
  enum State {WAITING, BLOCKING, POSITIONING, RETURNING, APPROACH, STAND, AIM, HOLD_BALL};


  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

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

  BeTrackObjects *track_objects;
  BePrimaryAttacker *pri_attacker;
  BeApproachBall *approach;
  BeTurn *turn;
  BeGotoPointGlobal *goto_point;
  uint fs_id;  // the id of the FeatureSet event processor from the event system
  uint sensor_id; // SensorData even processor id

  FeatureSet *fs;
  SensorData *sd;

  ulong approach_last_failed;
  ulong stuck_time;

  bool shouldClear(FeatureSet* features, 
                   bool is_clearing);
  bool shouldBlock(FeatureSet* features);
  bool ball_been_far_recently;
  ulong ball_far_time;

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

  Goalie();
  virtual ~Goalie();
  void reset(ulong timestamp);
  void sleep() {fsm.sleep(); track_objects->sleep(); pri_attacker->sleep();}

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

  virtual bool initConnections();
  virtual bool setupEventMgr();
  virtual const MotionCommand *get(ulong time);
  virtual uchar *encodeAllNames(ulong time,uchar *buf,int buf_size);
  virtual uchar *encodeTrace(uchar *buf,int buf_size);
};

//extern BeGoalie *Goalie;

#endif