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

#include "DogTypes.h"
#include "Gaussian2.h"
#include "../Motion/MotionInterface.h"

// Maximum message length that we are willing to route from 
// [robot|linux]->robot.  Note, that this does not affect
// robot->linux message size.
//static const ulong MaxRouteMessageLength=1024;

// This is the maximum number of robots that we will ever be
// connected to.
static const int MAX_PEERS = 25;

struct NetMsgHeader
{
  enum IDList { Linux = 0xFE, BCast };

  enum SubsystemList { Sub_Main, Sub_WLO, Sub_NumOfSubsystems };

  enum MessageTypes { MsgText = 1, WLOStream, MsgThresholdSelect, MsgStatus, 
		      MsgBallCollection, MsgBarChallenge, MsgPupPilot, 
		      MsgRoleToken, MsgStatusAndRole, MsgSynch };
  
  ulong timestamp; 

  // This is the total length in bytes of the packet
  // that gets transmitted. (Aperios messages don't tell you
  // how large they are)

  ushort totalLength; 
  uchar senderID;
  uchar senderSubsystem;
  uchar receiverID;
  uchar receiverSubsystem;
  uchar messageType;
  uchar senderTeam;
};


struct MiniG2 {
  float mean_x;
  float mean_y;
  float sMaj, sMin, thetaMaj;
  ulong time;

  void pack(const Gaussian2 &src) {
    mean_x = src.mean.x;
    mean_y = src.mean.y;
    sMaj = src.sMaj;
    sMin = src.sMin;
    thetaMaj = src.thetaMaj;
    time = src.time;
  }

  void unpack(Gaussian2 &dst) const {
    dst.mean.x = mean_x;
    dst.mean.y = mean_y;
    dst.time = time;

    dst.setsMajsMin(sMaj, sMin, thetaMaj);
  }
};

// ------------------------------------------------------- role assignment

struct RoleTokenEntry {
  double Activation;
  double OtherActivation; // info for debugging
  int Token;
  int Receiver;
  int RoleMessage;
};

struct PackedRoleTokenEntry {
  float Activation;
  float OtherActivation;
  int Token;
  char Receiver;
  char RoleMessage;

  void pack(const RoleTokenEntry &src) {
    Activation = src.Activation;
    OtherActivation = src.OtherActivation;
    Token = src.Token;
    Receiver = src.Receiver;
    RoleMessage = src.RoleMessage;
  }

  void unpack(RoleTokenEntry &dst) const {
    dst.Activation = Activation;
    dst.OtherActivation = OtherActivation;
    dst.Token = Token;
    dst.Receiver = Receiver;
    dst.RoleMessage = RoleMessage;
  }
};

// -------------------------------------------------------- shared WM

struct SharedWMEntry {
  Gaussian2 my_position;
  vector2d my_heading;
  Gaussian2 ball_position;
  uchar valid_ball_posn;
};

struct PackedSharedWMEntry {
  MiniG2 my_position;
  float my_heading_x;
  float my_heading_y;
  MiniG2 ball_position;
  uchar valid_ball_posn;

  void pack(const SharedWMEntry &src) {
    my_position.pack(src.my_position);
    my_heading_x = src.my_heading.x;
    my_heading_y = src.my_heading.y;
    ball_position.pack(src.ball_position);
    valid_ball_posn = src.valid_ball_posn;
  }

  void unpack(SharedWMEntry &dst) const {
    my_position.unpack(dst.my_position);
    dst.my_heading.x = my_heading_x;
    dst.my_heading.y = my_heading_y;
    ball_position.unpack(dst.ball_position);
    dst.valid_ball_posn = valid_ball_posn;
  }
};

struct RobotObservation {

  RobotObservation() {
    my_position = Gaussian2();
    robot_posn.set(0,0);
    obs_time = 0;
    robot_color = 0;
    vision_conf = 0;
  }

  // Our [global] position at the time of the observation
  Gaussian2 my_position;

  // The offset from our global position to this robot. The offset
  // is in global coords, obviously.
  vector2d robot_posn;

  double vision_conf;

  // We specify how old an observation is rather than giving it
  // a timestamp when we share data as the robots' clocks are
  // not synced. The receiver can subtract obs_time from the
  // *corrected* timestamp in the packet header to get an actual
  // timestamp for the observation.
  ulong obs_time;
  uchar robot_color;
};

struct PackedRobotObservation {
  MiniG2 my_position;
  float robot_posn_x;
  float robot_posn_y;
  float vision_conf;
  ulong obs_time;
  uchar robot_color;

  void pack(const RobotObservation &src) {
    my_position.pack(src.my_position);
    robot_posn_x = src.robot_posn.x;
    robot_posn_y = src.robot_posn.y;
    vision_conf = src.vision_conf;
    obs_time = src.obs_time;
    robot_color = src.robot_color;
  }

  void unpack(RobotObservation &dst) const {
    my_position.unpack(dst.my_position);
    dst.robot_posn.x = robot_posn_x;
    dst.robot_posn.y = robot_posn_y;
    dst.vision_conf = vision_conf;
    dst.obs_time = obs_time;
    dst.robot_color = robot_color;
  }

};


// -------------------------------------------------------- status messages

struct RoleCommand {
  // OFF_SUPP is the normal offensive supporter that plays as a wingman.
  // DEF_SUPP plays defense
  // OFF_SUPP_LONG only comes into play when the goalie is clearing and plays
  // far into the other half.
  enum Commands { COM_OFF_SUPP, COM_DEF_SUPP, COM_OFF_SUPP_LONG };

  char ID; // id of dog command applies to
  char Command; // the actual command
}; // 2 bytes


const int num_shared_robot_obs = 3;
struct StatusMsgEntry {

  StatusMsgEntry() {

    HaveToken = -1;
    Activation = 0;
    time_since_saw_ball = timestamp = 0;
    IsGoalie = GoalieClearing = NumTeammates = 
      ReadyForPass = HavePassedRecently = IsPenalized = 0;
  }

  SharedWMEntry shared_info;

  RobotObservation robot_obs[num_shared_robot_obs];

  /* Our activation - i.e. how much we want to be 
     primary attacker and go for the ball. */
  double Activation;

  ulong time_since_saw_ball; 

  // *sigh* We have a timestamp field in the header that gets sent
  // over the network. But we want that information in loads of
  // places that don't keep the header around.
  ulong timestamp;

  /* Do we currently have the token that indicates we're the
     primary attacker? */
  int HaveToken;

  // If we're the primary attacker, we tell our lackeys what they
  // should be doing here.
  RoleCommand Commands[3];
  
  /* Set to true if this player is the goalie. 
     I.e. we shouldn't yield to them unless 
     the ball is in the defense zone, in which
     case attackers can't touch it.
  */
  uchar IsGoalie;

  /* Set to true if this player is the goalie and it is
     currently clearing the ball.
  */
  uchar GoalieClearing;

  uchar NumTeammates;

  /* Set to true if this player thinks it is ready to catch a pass. */
  uchar ReadyForPass;

  /* Set to true if this player just attempted to pass to a
     teammate. */
  uchar HavePassedRecently;

  /* Set to true if we are being penalized by the RoboCup game
     manager software. */
  uchar IsPenalized;
};

struct PackedStatusMsgEntry {

  PackedSharedWMEntry shared_info;
  PackedRobotObservation robot_obs[num_shared_robot_obs];
  float Activation;
  ulong time_since_saw_ball; 
  ulong timestamp;
  int HaveToken;
  RoleCommand Commands[3];
  uchar IsGoalie;
  uchar GoalieClearing;
  uchar NumTeammates;
  uchar ReadyForPass;
  uchar HavePassedRecently;
  uchar IsPenalized;

  void pack(const StatusMsgEntry &src) {
    shared_info.pack(src.shared_info);
    for(int i=0; i<num_shared_robot_obs; i++)
      robot_obs[i].pack(src.robot_obs[i]);
    Activation = src.Activation;
    time_since_saw_ball = src.time_since_saw_ball; 
    timestamp = src.timestamp;
    HaveToken = src.HaveToken;
    for(int i=0; i<3; i++)
      Commands[i] = src.Commands[i];
    IsGoalie = src.IsGoalie;
    GoalieClearing = src.GoalieClearing;
    NumTeammates = src.NumTeammates;
    ReadyForPass = src.ReadyForPass;
    HavePassedRecently = src.HavePassedRecently;
    IsPenalized = src.IsPenalized;
  }

  void unpack(StatusMsgEntry &dst) const {
    shared_info.unpack(dst.shared_info);
    for(int i=0; i<num_shared_robot_obs; i++)
      robot_obs[i].unpack(dst.robot_obs[i]);
    dst.Activation = Activation;
    dst.time_since_saw_ball = time_since_saw_ball; 
    dst.timestamp = timestamp;
    dst.HaveToken = HaveToken;
    for(int i=0; i<3; i++)
      dst.Commands[i] = Commands[i];
    dst.IsGoalie = IsGoalie;
    dst.GoalieClearing = GoalieClearing;
    dst.NumTeammates = NumTeammates;
    dst.ReadyForPass = ReadyForPass;
    dst.HavePassedRecently = HavePassedRecently;
    dst.IsPenalized = IsPenalized; 
  }
};

// ------------------------------------------------------------- wrappers

struct RoleTokenMsg {
  
  NetMsgHeader header;
  RoleTokenEntry data;

  void buildHeader(uchar sender_id, uchar sender_team, uchar receiver_id)
  {
    header.totalLength = sizeof(RoleTokenMsg);
    header.senderID        = sender_id;
    header.senderSubsystem = NetMsgHeader::Sub_Main;
    header.receiverID        = receiver_id;
    header.receiverSubsystem = NetMsgHeader::Sub_Main;
    header.messageType = NetMsgHeader::MsgRoleToken;
    header.timestamp = 0;
    header.senderTeam = sender_team;
  }
};

struct StatusMsg {
  NetMsgHeader header;
  StatusMsgEntry data;
  
  void buildHeader(uchar sender_id, uchar sender_team)
  {
    header.totalLength = sizeof(StatusMsg);
    header.senderID        = sender_id;
    header.senderSubsystem = NetMsgHeader::Sub_Main;
    header.receiverID        = NetMsgHeader::BCast;
    header.receiverSubsystem = NetMsgHeader::Sub_Main;
    header.messageType = NetMsgHeader::MsgStatus;
    header.timestamp = 0;
    header.senderTeam = sender_team;
  }
};

struct StatusAndRoleMsg {

  NetMsgHeader header;
  RoleTokenEntry role_data;
  StatusMsgEntry status_data;

  void buildHeader(uchar sender_id, uchar sender_team)
  {
    header.totalLength = sizeof(StatusAndRoleMsg);
    header.senderID        = sender_id;
    header.senderSubsystem = NetMsgHeader::Sub_Main;
    header.receiverID        = NetMsgHeader::BCast;
    header.receiverSubsystem = NetMsgHeader::Sub_Main;
    header.messageType = NetMsgHeader::MsgStatusAndRole;
    header.timestamp = 0;
    header.senderTeam = sender_team;
  }
};

struct PackedStatusAndRoleMsg {
  NetMsgHeader header;
  PackedRoleTokenEntry role_data;
  PackedStatusMsgEntry status_data;

  void pack(const StatusAndRoleMsg &src) {
    header = src.header;
    role_data.pack(src.role_data);
    status_data.pack(src.status_data);
  }

  void unpack(StatusAndRoleMsg &dst) const {

    dst.header = header;
    role_data.unpack(dst.role_data);
    status_data.unpack(dst.status_data);
  }

};

#endif