/* 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/Motion/MotionInterface.h"

#include "MotionPacketDecoder.h"

using namespace Motion;

bool
MotionPacketDecoder::decode(MotionLocalizationUpdate *mlu,RobotDataPacket *packet) {
  if(packet->length < 2*sizeof(short)+8*sizeof(float)+sizeof(uchar)) {
    fprintf(stderr,"short motion update packet, length %lu\n",packet->length);
    return false;
  }
  
  uchar *buf=packet->data;

  mlu->timestamp = packet->timestamp;
  mlu->state_type         = grab<short>(&buf);
  mlu->state              = grab<short>(&buf);
  mlu->pos_delta.x        = grab<float>(&buf);
  mlu->pos_delta.y        = grab<float>(&buf);
  mlu->heading_delta.x    = grab<float>(&buf);
  mlu->heading_delta.y    = grab<float>(&buf);
  mlu->body.height        = grab<float>(&buf);
  mlu->body.angle         = grab<float>(&buf);
  mlu->body.neck_offset.x = grab<float>(&buf);
  mlu->body.neck_offset.y = grab<float>(&buf);
  mlu->stable             = grab<uchar>(&buf);

  return true;
}