/* 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.
  ========================================================================= */

#include "TeamMsgMgrEncoder.h"
#include "TeamMsgMgr.h"

void 
SPOutTeamMsgMgrEncoder::encodeStatusMsgEntry(uchar **buf,
					     const StatusMsgEntry * s) {
  /* Encode the mean for the position and heading */
  encodeAs<float>(buf, (float)s->shared_info.my_position.mean.x);
  encodeAs<float>(buf, (float)s->shared_info.my_position.mean.y);
  encodeAs<float>(buf, (float)s->shared_info.my_heading.angle());

  /* Encode whether this robot thinks it is the goalie */
  encodeAs<uchar>(buf, (uchar)s->IsGoalie);

  /* Encode whether or not this robot actually saw the ball */
  encodeAs<ulong>(buf, (uchar)s->time_since_saw_ball);

  /* Encode which token this robot is holding */
  encodeAs<int>(buf, s->HaveToken);

  /* Encode the mean for the ball position */
  encodeAs<float>(buf, (float)s->shared_info.ball_position.mean.x);
  encodeAs<float>(buf, (float)s->shared_info.ball_position.mean.y);
}

int SPOutTeamMsgMgrEncoder::encodeTeamMsgMgr(uchar *buf, 
					     TeamMsgMgr *modeller) {
  uchar *orig_buf;
  orig_buf = buf;
  
  /* Encode the entire SharedModelEntry for each of the 8 robots in the
     shared world model */
  TeamMsgMgr::teammate_iterator team_it = modeller->teammate.begin();
  while(team_it != modeller->teammate.end()) {
    StatusMsgEntry temp = (*team_it).second; 
    encodeStatusMsgEntry(&buf, &temp);

    team_it++;
  }

  return buf-orig_buf;
}