/* 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 "WorldModelEncoder.h"
#include "WorldModel.h"
#include "../headers/Reporting.h"

void SPOutWorldModelEncoder::encodeGaussian2(uchar **buf,
					     const Gaussian2 * g) {
  encodeAs<float>(buf, (float)g->mean.x);
  encodeAs<float>(buf, (float)g->mean.y);
  encodeAs<float>(buf, (float)g->thetaMaj);  
  encodeAs<float>(buf, (float)g->sMaj);
  encodeAs<float>(buf, (float)g->sMin);
}

void SPOutWorldModelEncoder::encodeValid(uchar **buf,
					 uchar valid) {
  encodeAs<uchar>(buf,(uchar)valid);
}

int SPOutWorldModelEncoder::encodeWorldModel(uchar *buf, 
                                             WorldModel *modeller) {
  uchar *orig_buf;
  orig_buf = buf;

  Gaussian2 temp;

  // Ball, red robot, and blue robot as seen from our camera
  //  if (modeller->ball_posn(temp)) {
  //    encodeValid(&buf,(uchar)1); } else { encodeValid(&buf,(uchar)0); }
  temp = modeller->ball_estimate();
  encodeValid(&buf,(uchar)1);
  encodeGaussian2(&buf, &temp);

  if (modeller->red_robot_observation(temp)) {
    encodeValid(&buf,(uchar)1);  } else { encodeValid(&buf,(uchar)0); }
  encodeGaussian2(&buf, &temp);

  if (modeller->blue_robot_observation(temp)) {
    encodeValid(&buf,(uchar)1);  } else { encodeValid(&buf,(uchar)0); }
  encodeGaussian2(&buf, &temp);

  // Teammate positions
  for (int i=0;i<3;i++) {
    if (modeller->teammate_pos_observation(temp,i)){
      encodeValid(&buf,(uchar)1);  } else { encodeValid(&buf,(uchar)0); }
    encodeGaussian2(&buf, &temp);
  }

  // Balls seen from teammates
  for (int i=0;i<3;i++) {
    if (modeller->teammate_ball_observation(temp,i)){
      encodeValid(&buf,(uchar)1);  } else { encodeValid(&buf,(uchar)0); }
    encodeGaussian2(&buf, &temp);
  }

  // Red robots seem from teamamtes
  for (int i=0;i<3;i++) {
    if (modeller->teammate_red_robot_observation(temp,i)) {
      encodeValid(&buf,(uchar)1);  } else { encodeValid(&buf,(uchar)0); }
    encodeGaussian2(&buf, &temp);
  }

  // Blue robots seen from teammates
  for (int i=0;i<3;i++) {
    if (modeller->teammate_blue_robot_observation(temp,i)) {
      encodeValid(&buf,(uchar)1);  } else { encodeValid(&buf,(uchar)0); }
    encodeGaussian2(&buf, &temp);
  }

  return buf - orig_buf;
}