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

using namespace VisionInterface;

bool
VisionRLEPacketDecoder::decode(double *angles,RobotDataPacket *packet) {
  uchar *data;
  bool error;

  data = packet->data;
  error = (packet->length < 20);
  if(!error) {
    double body_angle,body_height,tilt,pan,roll;
    
    body_angle  = PacketDecoder::grab<float>(&data);
    body_height = PacketDecoder::grab<float>(&data);
    tilt        = PacketDecoder::grab<float>(&data);
    pan         = PacketDecoder::grab<float>(&data);
    roll        = PacketDecoder::grab<float>(&data);
    
    //printf("body_angle=%9f body_height=%9f tilt=%9f pan=%9f roll=%9f\n",
    //       body_angle,body_height,tilt,pan,roll);
    
    angles[0] = body_angle ;
    angles[1] = body_height;
    angles[2] = tilt       ;
    angles[3] = pan        ;
    angles[4] = roll       ;
  }

  return !error;
}

bool
VisionRLEPacketDecoder::decode(int image_x_size,int image_y_size,uchar *img,RobotDataPacket *packet) {
  uchar *data;
  int dlength;
  bool error;

  error = false;
  data   = packet->data;

  if(packet->length < 5*sizeof(float))
    error = true;

  data += 20;
  dlength = packet->length - 20;

  int cur_x=0,cur_y=0;
  for(; dlength>=3 && cur_y<image_y_size && !error; ) {
    char color;
    color = (char)data[0];
    //printf("c%u",color);
    uchar x_c;
    x_c = data[1];
    int x=(int)(unsigned char)x_c;
    //printf("l%d",length);
    uchar length_c;
    length_c = data[2];
    int length=(int)(unsigned char)length_c;
    //printf("l%d",length);

    data+=3;
    dlength-=3;
    
    if(x < cur_x) {
      fprintf(stderr,"backwards x movement\n");
      fprintf(stderr,"color %d(%x) cur_x %d x %d(%x) length %d(%x)\n",color,color,cur_x,x,x,length,length);
      error=true;
      break;
    }

    // fill in black if necessary
    for(; cur_x < x; cur_x++)
      img[cur_y*image_x_size+cur_x]=0;

    // fill in color
    if(cur_x+length > image_x_size) {
      fprintf(stderr,"run past end of line\n");
      fprintf(stderr,"color %d(%x) cur_x %d x %d(%x) length %d(%x)\n",color,color,cur_x,x,x,length,length);
      error=true;
      break;
    }
    for(; length>0; length--,cur_x++)
      img[cur_y*image_x_size+cur_x]=color;
    if(cur_x==image_x_size) {
      cur_y++;
      cur_x=0;
      //printf("\nline%d :",cur_y);
    }
  }
  //printf("\n");

  if(cur_y!=image_y_size) {
    int pos=(data - packet->data - 20);
    fprintf(stderr,"early end of data, data stopped at x=%d y=%d, run row %d col %d\n",cur_x,cur_y,pos/32,pos%32);
    error = true;
  }

  if(dlength!=0) {
    fprintf(stderr,"not enough data, expected %d more bytes\n",dlength);
    error = true;
  }

  if(error) {
    fprintf(stderr,"error decoding image\n");
    if(cur_x>=image_x_size){
      cur_y++;
      cur_x=0;
    }
    if(cur_y<image_y_size)
      memset(&img[cur_y*image_x_size+cur_x],badColor,((image_y_size-1-cur_y)*image_x_size+(image_x_size-1-cur_x))*sizeof(*img));
  }

  return !error;
}

bool VisionRLEPacketDecoder::decode(int &image_x_size,int &image_y_size,uchar *img,int max_image_x_size,int max_image_y_size,RobotDataPacket *packet)
{
  uchar *data;
  int dlength;
  bool error;

  image_x_size=0;
  image_y_size=0;

  error = false;
  data   = packet->data;

  if(packet->length < 5*sizeof(float))
    error = true;

  data += 20;
  dlength = packet->length - 20;

  if(!error){
    if(dlength < (int)(2*sizeof(ushort))){
      fprintf(stderr,"vision packet too short, missing image size\n");
      error = true;
    }else{
      image_x_size = PacketDecoder::grab<ushort>(&data);
      image_y_size = PacketDecoder::grab<ushort>(&data);
      dlength -= 2*sizeof(ushort);
      if(image_x_size > max_image_x_size){
        fprintf(stderr,"image width of %d is larger than maximum width of %d\n",image_x_size,max_image_x_size);
        image_x_size = max_image_x_size;
        error = true;
      }
      if(image_y_size > max_image_y_size){
        fprintf(stderr,"image height of %d is larger than maximum height of %d\n",image_y_size,max_image_y_size);
        image_y_size = max_image_y_size;
        error = true;
      }
    }
  }

  int cur_x=0,cur_y=0;
  for(; dlength>=3 && cur_y<image_y_size && !error; ) {
    char color;
    color = (char)data[0];
    //printf("c%u",color);
    uchar x_c;
    x_c = data[1];
    int x=(int)(unsigned char)x_c;
    //printf("l%d",length);
    uchar length_c;
    length_c = data[2];
    int length=(int)(unsigned char)length_c;
    //printf("l%d",length);

    data+=3;
    dlength-=3;
    
    if(x < cur_x) {
      fprintf(stderr,"backwards x movement\n");
      fprintf(stderr,"color %d(%x) cur_x %d x %d(%x) length %d(%x)\n",color,color,cur_x,x,x,length,length);
      error=true;
      break;
    }

    // fill in black if necessary
    for(; cur_x < x; cur_x++)
      img[cur_y*image_x_size+cur_x]=0;

    // fill in color
    if(cur_x+length > image_x_size) {
      fprintf(stderr,"run past end of line\n");
      fprintf(stderr,"color %d(%x) cur_x %d x %d(%x) length %d(%x)\n",color,color,cur_x,x,x,length,length);
      error=true;
      break;
    }
    for(; length>0; length--,cur_x++)
      img[cur_y*image_x_size+cur_x]=color;
    if(cur_x==image_x_size) {
      cur_y++;
      cur_x=0;
      //printf("\nline%d :",cur_y);
    }
  }
  //printf("\n");

  if(cur_y!=image_y_size) {
    int pos=(data - packet->data - 20);
    fprintf(stderr,"early end of data, data stopped at x=%d y=%d, run row %d col %d\n",cur_x,cur_y,pos/32,pos%32);
    error = true;
  }

  if(dlength!=0) {
    fprintf(stderr,"not enough data, expected %d more bytes\n",dlength);
    error = true;
  }

  if(error) {
    fprintf(stderr,"error decoding image\n");
    if(cur_x>=image_x_size){
      cur_y++;
      cur_x=0;
    }
    if(cur_y<image_y_size)
      memset(&img[cur_y*image_x_size+cur_x],badColor,((image_y_size-1-cur_y)*image_x_size+(image_x_size-1-cur_x))*sizeof(*img));
  }

  return !error;
}