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

#include <math.h>

#include <map>

#include "../headers/gvector.h"
#include "../headers/DogTypes.h"
#include "../headers/Gaussian2.h"
#include "../headers/WaveLAN.h"
#include "../WorldModel/StuckDetector.h"
#include "TeamMsgMgrEncoder.h"
#include "../Vision/VisionInterface.h"
#include "../CommMgr/CommMgrInterface.h"

using std::map;

class PacketStream;
class PacketStreamCollection;
class WorldModel;

class TeamMsgMgr {
  
  friend class SPOutTeamMsgMgrEncoder;

 public:
  /* These are the types of messags sent between the robots. token_req
     is a request from a supporter asking for the token. token_pass is
     the response handing off the token. no_token tells the supporter 
     that the primary it queried either doesn't have it anymore or
     isn't willing to pass it off.
  */
  enum RoleMessage { MSG_TOKEN_REQ, MSG_TOKEN_PASS, MSG_NO_TOKEN, MSG_NONE };

  // Used by the goalie to tell us that a) it's the goalie and b)
  // when it's clearing.
  bool i_am_goalie;
  bool goalie_clearing;

  // Used by the primary attacker to force us to hang onto the
  // token when the ball is tucked under our chin.
  bool force_hold_token;

  int last_role_message;

 private:

  // These settings are controlled by /MS/config/teamplay.cfg and
  // are loaded in init()

  // Should we be using teamplay?
  bool use_teamplay;
  // Should we use our defend goal color as our team?
  bool use_default_team_id;
  // If not using our goal color as our team id, what should
  // we send?
  int special_team_id;

  StatusMsgEntry my_status;

  // Role assignment timers
  ulong last_saw_token; // when did we last see a robot with the token?
  ulong last_passed_token; // when did we last handoff the token?
  ulong last_requested_token; // when did we last ask for it?
  ulong started_ignore;
  bool okay_to_ignore_role;
  ulong last_rejected; // when did we last receive a no answer?
  ulong time_got_token; // time when we last acquired the token.
  ulong time_my_act_lower; // last time our activation lower than leader

  /* *_PLAYING represent normal states where the robot is the primary
     or supporting attacker. P_YIELDED is when the robot has handed off
     the token and is waiting to make sure the message actually arrived.
     SUPPORTER_ASKED is when the supporter has requested the token and it's
     waiting to receive a hand off from the robot that actually has it.
  */
  enum RoleState { PRIMARY_PLAYING, PRIMARY_YIELDED, SUPPORTER_PLAYING,
		   SUPPORTER_ASKED };

  RoleState role_state;

  // The time we last sent our shared information
  ulong time_last_broadcast;

  // timestamp when we last saw the ball - used to decay our activation
  ulong time_last_saw_ball;
  
  // timestamp when we last kicked the ball - we might want to hand
  // off the token more readily if we know we just tried to kick the
  // ball
  ulong time_last_kicked;

  // Map from robot id to whatever
  map<uint,StatusMsgEntry> teammate;
  typedef map<uint,StatusMsgEntry>::iterator teammate_iterator;

  map<uint, ulong> timestamp; // time last heard from teammate
  typedef map<uint,ulong>::iterator timestamp_iterator;

  // Our teammates' activations based on their shared positions
  // and my local ball estimate.
  map<uint, double> ego_view_activation;
  typedef map<uint,ulong>::iterator ego_act_iterator;

  map<uint, double> time_between_msgs;
  typedef map<uint,double>::iterator between_iterator; 

  WorldModel *my_world_model;

  SPOutTeamMsgMgrEncoder encoder;
  PacketStream *tmmOutStream;

  int my_goal_color;
  // current timestamp (as of last updateSelf)
  ulong my_time;

  // timestamp of the last time updateRoles() was called.
  ulong last_role_update_time;

  int my_token; // our current token value
  int my_id; 
  int my_team_id;
  int max_seen_token; // highest token value that we've seen

  int leader_id; // id of the leader; 0 = me

  int supporter_role;

  // Used in assignRoles() to add some hysteresis to the assignment of
  // teammates' roles. We don't want to switch too quickly between
  // assigning the offensive and defensive supporter; otherwise the
  // defender never get appropriately far away from the ball in some
  // cases, due to oscillation.
  int last_role_for_teammate1;
  int last_role_for_teammate2;

  // Fraction of the last 2 seconds worth of gsensor data that
  // we've been stuck for.
  double stuck_fraction;

  CommMgr::NetStatus net_status;

  public:

  TeamMsgMgr();

  void init();
  void initializeStreams(PacketStreamCollection *out_psc);
  void updateSelf(WorldModel *wm, 
		  VisionInterface::ObjectInfo *obj_info,
		  ulong time);

  void receiveRoleMessage(ulong time,
			  int sender_id, int sender_team, int type,
			  double activation, int token);
  void receiveStatusMsg(ulong time, int sender_id, int sender_team,
			StatusMsgEntry *msg);
  void updateNetStatus(CommMgr::NetStatus status);

  /* Returns the position of the teammate indicated by uniform_number (1-3)
     as a Gaussian. Returns (0, 0) with high variance if not available.
  */ 
  Gaussian2 getTeammatePosn(int uniform_number);

  // Returns a zero length vector if all the info is too old or
  // no one saw the ball. Does not return info from this robot.
  Gaussian2 getBestSharedBallPosn();

  // Returns true iff the given teammate, indicated by uniform number,
  // said that it was ready to receive a pass in its last status
  // message.
  bool teammateReadyForPass(int uniform_number);

  // Behaviors can call this to tell their teammates that they just
  // initiated a pass.
  void setHavePassedRecently(bool have_passed);

  // Returns true iff the given teammate, indicated by uniform number,
  // said that it tried to kick a pass in its last status message.
  bool teammateHasPassedRecently(int uniform_number);

  int countActiveTeammates();
  void sendOutput();
  bool isPrimaryAttacker();
  bool isOffensiveSupporter();
  bool isLeaderGoalie();
  ulong timeSinceGotToken();
  bool gettingMessagesFrom(int id);
  bool sameBallAs(int leader_id);
  bool validTeammate(int id);
  bool networkOkay();
  int idWithHighestAct();
  int getUniformNumFromID(int id);

  // one of COM_DEF_SUPP, COM_OFF_SUPP, COM_OFF_SUPP_LONG (from RoleCommand:: struct)
  int getSupporterRole();

  void setStuckInfo(StuckInfo _stuck_info);
  void setPlayerInfo(int _my_id/*, int _my_goal_color*/);


  double getPrimaryDist();
  vector2d getPrimaryLoc();
  vector2d getOtherSuppLoc();

 private:

  // Queue up a status update for sending over the network -
  // we reset some timers here as well
  void doStatusQueue();

  // is there an entry for this teammate in the teammate map?
  bool knownTeammate(int id);
  double getMyActivation(WorldModel *wm);
  double getMyActivation(WorldModel *wm,
			 vector2d ball,
			 ulong ball_age);
  double calcActivation(WorldModel *wm, int id);

  void roleUpdate();

  // Decay the activations of our teammates as their most recent
  // message ages.
  void decayActivations();
  
  // Set offensive_supp_id and defensive_supp_id because we just
  // got the token.
  void assignRoles();

  void debugPrintOuts();

  // helpers for calculating activations
  double rawActivation(vector2d robot_posn,
		       vector2d ball_posn);
  vector2d getBestBallEst(WorldModel *wm);
  double getRoleScale(double ball_x,
		      bool is_primary,
		      bool is_off_supp);
  double getActTimeDecay(ulong delta);
  double getTimeBetterBonus();
};

#endif