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

#include "VisionPacketDecoder.h"

using namespace VisionInterface;

void
VisionPacketDecoder::decode(VObject *obj,uchar **buf) {
  obj->confidence      = grab<float>(buf);
  obj->loc.x           = grab<float>(buf);
  obj->loc.y           = grab<float>(buf);
  obj->loc.z           = grab<float>(buf);
  obj->left            = grab<float>(buf);
  obj->right           = grab<float>(buf);
  obj->distance        = grab<float>(buf);
  obj->confInFrontOfIR = grab<float>(buf);
  obj->edge            = grab<uchar>(buf);
}

bool
VisionPacketDecoder::decode(ObjectInfo *obj_info,RobotDataPacket *packet) {
  if(packet->length < sizeof(short)) {
    fprintf(stderr,"short vision packet, length %lu\n",packet->length);
    return false;
  }

  memset(obj_info,0,sizeof(*obj_info));
  
  uchar *buf=packet->data;
  VObject *objs=(VObject *)obj_info;

  ushort objects_seen=0U;
  int num_objects=0;
  objects_seen = grab<ushort>(&buf);
  num_objects = countBits(objects_seen);

  if(packet->length < sizeof(short)+num_objects*(8*sizeof(float)+sizeof(uchar))) {
    fprintf(stderr,"short vision packet for %d objects, length %lu\n",num_objects,packet->length);
    return false;
  }
  
  for(int i=0; i<NUM_VISION_OBJECTS; i++) {
    if(objects_seen & (1 << i))
      decode(&objs[i],&buf);
  }

  return true;
}

bool
VisionPacketDecoder::decode(RadialObjectMap *vis_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++) {
      vis_map->robjects[obj_idx][angle_idx].confidence = 0.0;
      vis_map->robjects[obj_idx][angle_idx].distance   = RadialFarDistance;
    }
  }

  uchar *buf;
  buf = packet->data;

  ulong angles_seen[(NUM_ANGLES+ulong_bits-1)/ulong_bits];
  for(uint 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++) {
        RadialObjectReading *robj;
        robj = &vis_map->robjects[robj_idx][ang_idx];
        if(objects_seen & (1U<<robj_idx)) {
          robj->confidence = grab<float>(&buf);
          robj->distance   = grab<float>(&buf);
        }
      }
    }
  }

  return true;
}