/* 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 "../Vision/Vision.h"
#include "SPOutVisionEncoder.h"

uchar *
SPOutVisionEncoder::encodeAngles(uchar *buf,
                                 double body_angle,double body_height,
                                 double tilt,double pan,double roll) {
  encodeAsFloat(&buf,body_angle );
  encodeAsFloat(&buf,body_height);
  encodeAsFloat(&buf,tilt);
  encodeAsFloat(&buf,pan );
  encodeAsFloat(&buf,roll);

  return buf;
}

int
SPOutVisionEncoder::encodeVisionRaw(uchar *buf,
                                    double body_angle,double body_height,
                                    double tilt,double pan,double roll,
                                    CMVision::image_yuv<uchar> &img)
{
  const uchar *row_y,*row_u,*row_v;

  uchar *orig_buf;
  orig_buf = buf;

  buf = encodeAngles(buf,body_angle,body_height,tilt,pan,roll);

  int height,width;

  height = img.height;
  width  = img.width;

  encodeAs<ushort>(&buf,width );
  encodeAs<ushort>(&buf,height);

  for(int y=0; y<height; y++) {
    row_y = img.buf_y + y*img.row_stride;
    row_u = img.buf_u + y*img.row_stride;
    row_v = img.buf_v + y*img.row_stride;
    for(int x=0; x<width; x++) {
      *(buf  )=row_y[x];
      *(buf+1)=row_u[x];
      *(buf+2)=row_v[x];
      buf+=3;
    }
  }

  return buf-orig_buf;
}

int
SPOutVisionEncoder::encodeVisionRaw(uchar *buf,
                                    double body_angle,double body_height,
                                    double tilt,double pan,double roll,
                                    CMVision::image_yuv<uchar> &img,
                                    CMVision::image_yuv<uchar> &mask)
{
  const uchar *row_y,*row_u,*row_v;
  const uchar *mask_y, *mask_u, *mask_v;

  uchar *orig_buf;
  orig_buf = buf;

  buf = encodeAngles(buf,body_angle,body_height,tilt,pan,roll);

  int height,width;

  height = img.height;
  width  = img.width;

  encodeAs<ushort>(&buf,width );
  encodeAs<ushort>(&buf,height);

  int temp_y, temp_u, temp_v;

  for(int y=0; y<height; y++) {
    row_y = img.buf_y + y*img.row_stride;
    row_u = img.buf_u + y*img.row_stride;
    row_v = img.buf_v + y*img.row_stride;

    mask_y = mask.buf_y + y*mask.row_stride;
    mask_u = mask.buf_u + y*mask.row_stride;
    mask_v = mask.buf_v + y*mask.row_stride;
    
    int mask_offset = 0;
    for(int x=0; x<width; x++) {
      temp_y = row_y[x]; 
      temp_u = row_u[x];
      temp_v = row_v[x];
      
      temp_y *= mask_y[mask_offset];
      temp_u *= mask_u[mask_offset];
      temp_v *= mask_v[mask_offset];

      temp_y = temp_y >> mask_shift;
      temp_u = temp_u >> mask_shift;
      temp_v = temp_v >> mask_shift;

      mask_offset++;

      char y_b = (char)mask_y[mask_offset];  // [2*col+1];
      char u_b = (char)mask_u[mask_offset];  // [2*col+1];
      int v_b = -mask_v[mask_offset];  // [2*col+1];
      temp_y += y_b;
      temp_u += u_b;
      temp_v += v_b;

      // We'll want mask_col to point at the A term for the next
      // iteration of the loop
      mask_offset++;

      // We need to clamp our values back in the normal range for
      // a byte. Please, Mr. Compiler, please use a conditional
      // move instead of a jump.

      if(temp_y < 0) temp_y = 0;
      if(temp_y > 255) temp_y = 255;

      if(temp_u < 0) temp_u = 0;
      if(temp_u > 255) temp_u = 255;

      if(temp_v < 0) temp_v = 0;
      if(temp_v > 255) temp_v = 255;

      *(buf  )= (uchar)temp_y;
      *(buf+1)= (uchar)temp_u;
      *(buf+2)= (uchar)temp_v;
      buf+=3;
    }
  }
  
  return buf-orig_buf;
}

uchar *
SPOutVisionEncoder::encodeVisionRun(uchar *buf, run *run) {
  buf[0]=(uchar)run->color;
  buf[1]=(uchar)run->x;
  buf[2]=(uchar)run->width;
  return buf+3;
}

int
SPOutVisionEncoder::encodeVisionRLE(uchar *buf,
                                    double body_angle,double body_height,
                                    double tilt,double pan,double roll,
                                    int image_x_size,int image_y_size,
                                    int num_runs, run *runs)
{
  uchar *orig_buf;
  orig_buf = buf;

  buf = encodeAngles(buf,body_angle,body_height,tilt,pan,roll);

  encodeAs<ushort>(&buf,image_x_size);
  encodeAs<ushort>(&buf,image_y_size);
  for(; num_runs>0; num_runs--, runs++) {
    buf=encodeVisionRun(buf,runs);
  }
  return buf-orig_buf;
}

int
SPOutVisionEncoder::encodeVisionObjs(uchar *buf, ObjectInfo *obj_info) {
  static const double min_print=0.1;

  VObject *objs=(VObject *)obj_info;

  uchar *orig_buf;
  orig_buf = buf;

  unsigned short objects_seen=0U;
  int num_seen=0;
  for(int i=0; i<NUM_VISION_OBJECTS; i++) {
    if(objs[i].confidence > min_print) {
      objects_seen |= 1<<i;
      num_seen++;
    }
  }
  addToEncoding(&buf,(uchar *)&objects_seen,sizeof(objects_seen));

  for(int i=0; i<NUM_VISION_OBJECTS; i++) {
    VObject *obj=&objs[i];
    if(objects_seen & (1<<i)) {
      encodeAsFloat(&buf,obj->confidence);
      encodeAsFloat(&buf,obj->loc.x);
      encodeAsFloat(&buf,obj->loc.y);
      encodeAsFloat(&buf,obj->loc.z);
      encodeAsFloat(&buf,obj->left);
      encodeAsFloat(&buf,obj->right);
      encodeAsFloat(&buf,obj->distance);
      encodeAsFloat(&buf,obj->confInFrontOfIR);
      addToEncoding(&buf,(uchar *)&obj->edge,sizeof(obj->edge));
    }
  }

  return buf-orig_buf;
}

int
SPOutVisionEncoder::encodeVisionRadialMap(uchar *buf, RadialObjectMap *vis_map) {
  static const double min_print=0.1;
  static const int ulong_bits = 8*sizeof(ulong);

  uchar *orig_buf;
  orig_buf = buf;

  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]=0UL;
  for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++) {
    if(vis_map->robjects[ROBJECT_VISIBLE_START][ang_idx].confidence > min_print) {
      angles_seen[ang_idx/ulong_bits] |= 1UL<<(ang_idx%ulong_bits);
    }
  }
  for(uint i=0; i<sizeof(angles_seen)/sizeof(angles_seen[0]); i++)
    encodeAs<ulong>(&buf,angles_seen[i]);

  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 = 0U;
      for(int robj_idx=0; robj_idx<NUM_ROBJECTS; robj_idx++) {
        if(vis_map->robjects[robj_idx][ang_idx].confidence > min_print) {
          objects_seen |= 1U<<robj_idx;
        }
      }

      encodeAs<ushort>(&buf,objects_seen);
      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)) {
          encodeAs<float>(&buf,robj->confidence);
          encodeAs<float>(&buf,robj->distance);
        }
      }
    }
  }

  return buf-orig_buf;
}