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

void WMDebugPacketDecoder::decode(WMDebug * wmd,uchar **data) {
  wmd->x     = grab<float>(data);
  wmd->y     = grab<float>(data);
  wmd->shape = grab<uchar>(data);
  wmd->red   = grab<uchar>(data);
  wmd->green = grab<uchar>(data);
  wmd->blue  = grab<uchar>(data);
  wmd->valid = grab<uchar>(data);
}

bool WMDebugPacketDecoder::decode(WMDebug *objects,RobotDataPacket *packet) {

  if(packet->length < 10*WMDEBUG_SIZE) {
    fprintf(stderr,"short wmdebug packet, length %lu, should be %lu\n",packet->length,10*WMDEBUG_SIZE);
    return false;
  }
  
  uchar *buf=packet->data;
  for(int i=0; i<10; i++) {
    decode(&objects[i],&buf);
  }

  return true;
}