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

#include "LocalizationPacketDecoder.h"

bool
LocalizationPacketDecoder::decode(GLocalizer *posn,RobotDataPacket *packet) {
  if(packet->length < 7*sizeof(float)) {
    fprintf(stderr,"short localization packet, length %lu\n",packet->length);
    return false;
  }
  
  uchar *buf=packet->data;

  double theta_maj,s_maj,s_min;

  posn->position.mean.x   = grab<float>(&buf);
  posn->position.mean.y   = grab<float>(&buf);
  theta_maj               = grab<float>(&buf);
  s_maj                   = grab<float>(&buf);
  s_min                   = grab<float>(&buf);
  posn->position.setsMajsMin(s_maj,s_min,theta_maj);
  float tempAngle         = grab<float>(&buf);
  posn->heading.mean      = vector2d(cos(tempAngle), sin(tempAngle));
  posn->heading.sMaj      = grab<float>(&buf);

  return true;
}