/* 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 "../headers/SPOutEncoder.h"
#include "MotionInterface.h"
#include "SPOutMotionEncoder.h"

using namespace Motion;

int
SPOutMotionEncoder::encodeMotionLocalizationUpdate(uchar *buf,
                                                   MotionLocalizationUpdate *mlu) {
  uchar *orig_buf;
  orig_buf = buf;

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

  return buf-orig_buf;
}