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

#include "RadialModelPacketDecoder.h"

using namespace RadialModel;

bool
RadialModelPacketDecoder::decode(RadialModelMap *model_map,RobotDataPacket *packet) {
  static const int ulong_bits = 8*sizeof(ulong);

  for(int obj_idx=0; obj_idx<NUM_ROBJECTS; obj_idx++) {
    for(int angle_idx=0; angle_idx<NUM_ANGLES; angle_idx++) {
      RadialModelObject *model_obj;

      model_obj = model_map->query(obj_idx,angle_idx);

      model_obj->timeSeen   = 0UL;
      model_obj->confidence = 0.0;
      model_obj->distance   = RadialFarDistance;
    }
  }

  uchar *buf;
  buf = packet->data;

  ulong angles_seen[(NUM_ANGLES+ulong_bits-1)/ulong_bits];
  for(int i=0; i<sizeof(angles_seen)/sizeof(angles_seen[0]); i++)
    angles_seen[i]=grab<ulong>(&buf);

  for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++) {
    if(angles_seen[ang_idx/ulong_bits] & (1UL<<(ang_idx%ulong_bits))) {
      ushort objects_seen;

      objects_seen = grab<ushort>(&buf);

      for(int robj_idx=0; robj_idx<NUM_ROBJECTS; robj_idx++) {
        RadialModelObject *robj;
        robj = model_map->query(robj_idx,ang_idx);
        if(objects_seen & (1U<<robj_idx)) {
          robj->timeSeen   = grab<ulong>(&buf);
          robj->confidence = grab<float>(&buf);
          robj->distance   = grab<float>(&buf);
        }
      }
    }
  }

  return true;
}