/* 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 "SPOutWMDebugEncoder.h"
#include "WorldModel.h"
#include "../headers/Reporting.h"

void SPOutWMDebugEncoder::encodeWMD(uchar ** buf,
				    const WMDebug & wmd) {
  encodeAs<float>(buf,wmd.x);
  encodeAs<float>(buf,wmd.y);
  encodeAs<uchar>(buf,wmd.shape);
  encodeAs<uchar>(buf,wmd.red);
  encodeAs<uchar>(buf,wmd.green);
  encodeAs<uchar>(buf,wmd.blue);
  encodeAs<uchar>(buf,wmd.valid);
}

int SPOutWMDebugEncoder::encodeWMDebug(uchar * buf,
				       WorldModel * wm) {
  
  uchar *orig_buf;
  orig_buf = buf;

  WMDebug wmd;

  // Iterate over the list of debug points sent to the world model
  for (int i=0;i<NUM_WMDEBUG;i++) {
    if (!wm->getWMDebug(wmd,i)) {
      wmd.valid=0;
    }
    encodeWMD(&buf,wmd);
  }

  // Clear the wmdebug queue now that we've sent it

  return buf - orig_buf;
}