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

#include "../Motion/MotionInterface.h"
using namespace Motion;

#include "BePrimaryAttacker.h"
#include "BeLowLevel.h"
#include "state_machine.h"

struct FeatureSet;

class BeSupportingAttacker {
  
 public:
  static const int beh_id;
  
  static char const * const beh_name;
  
  static const int NumStates=4;
  enum State { POSITION, STAND, CATCH, ATTACK };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;
  
 private:
  FSM fsm;
  
  BeTrackObjects *track_objects;
  BeGotoPointGlobal *goto_point;
  BeNavToPointGlobal *navto_point;
  BePrimaryAttacker *primary_attacker;

  double side_to_wait_on;

  ulong last_catch_time;

  bool avoiding_defense_box;
  bool avoiding_opponent_defense_box;
  bool avoiding_obstacles;
  ulong last_avoiding_primary;
  bool avoiding_teammate;
  bool far_from_target;
  bool facing_ball;
  ulong role_change_time;
  int last_primary_avoid_direction;
  int last_role;
  bool be_behind_ball;

  double calcActivation(FeatureSet *features);
  void updateZones(FeatureSet *features);
  void calcGoalPoint(FeatureSet *features, vector2d &point, double &heading);
  int calcPrimaryAvoidanceSide(FeatureSet *features);
  vector2d getBallPosn(FeatureSet *features);

  vector2d getDefensePosn(FeatureSet *fs);
  vector2d getOffensePosn(FeatureSet *fs);

  bool primaryInScrum(FeatureSet* features);
  bool shouldBeOpportunistic(FeatureSet* features);

  vector2d calcTargetForRole(FeatureSet* features);
  vector2d calcTargetForScrum(FeatureSet* features, vector2d target, MotionCommand *command);
  vector2d calcTargetAvoidingTeammates(FeatureSet* features, vector2d target, MotionCommand* command);
  vector2d calcTargetAvoidingPrimary(FeatureSet *features, vector2d target);
  vector2d calcTargetAvoidingDefenseBox(FeatureSet *features, vector2d target);
  vector2d calcTargetAvoidingOpponentDefenseBox(FeatureSet *features, vector2d target);


  void configureWalks(FeatureSet* features, vector2d target);

  int getSupporterRole(FeatureSet* features);



 public:
  BeSupportingAttacker();
  ~BeSupportingAttacker();
  void reset(ulong timestamp);
  void sleep() {fsm.sleep(); track_objects->sleep();}
  
  double operator()(FeatureSet *features, 
		    MotionCommand *command);
  
  uchar *encodeAllNames(ulong time,uchar *buf,int buf_size);
  uchar *encodeTrace(uchar *buf,int buf_size);
};

#endif