/* 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 <stdio.h>
#include "../../../agent/headers/CircBufPacket.h"
#include "../../../agent/headers/Gaussian2.h"

#include "WorldModelPacketDecoder.h"

void
WorldModelPacketDecoder::decode(Gaussian2 *g,uchar **data) {
  g->mean.x   = grab<float>(data);
  g->mean.y   = grab<float>(data);
  g->thetaMaj    = grab<float>(data);
  g->sMaj = grab<float>(data);
  g->sMin = grab<float>(data);
  *g = Gaussian2(0, g->mean, g->sMaj, g->sMin, g->thetaMaj, false);
  // fprintf(stderr, "%f %f\n", g->mean.x, g->mean.y);
}

void WorldModelPacketDecoder::decode(uchar *v,uchar **data) {
  *v = grab<uchar>(data);
}

bool
WorldModelPacketDecoder::decode(Gaussian2 *objects,uchar * valid,RobotDataPacket *packet) {
  if(packet->length < NumGaussians*(5*sizeof(float))+sizeof(uchar)) {
    fprintf(stderr,"short world model packet, length %lu\n",packet->length);
    return false;
  }
  
  // fprintf(stderr, "%lu\n", packet->timestamp);
  
  uchar *buf=packet->data;
  for(int i=0; i<NumGaussians; i++) {
    decode(&valid[i],&buf);
    decode(&objects[i],&buf);
    objects[i].time = packet->timestamp;
  }
  //fprintf(stderr, "\n");

  return true;
}