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

#include "VisionRawPacketDecoder.h"

using namespace VisionInterface;

bool
VisionRawPacketDecoder::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
VisionRawPacketDecoder::decode(int &width, int &height,
			       RobotDataPacket *packet) {
  uchar *data;
  bool error;

  width = height = 0;

  data = packet->data;
  error = (packet->length < 5*sizeof(float) + 2*sizeof(ushort));
  if(!error) {
    data += 5*sizeof(float);
    
    width = PacketDecoder::grab<ushort>(&data);
    height = PacketDecoder::grab<ushort>(&data);
  }

  return !error;
}

bool
VisionRawPacketDecoder::decode(int image_x_size,int image_y_size,CMVision::image_yuv<uchar> *img,RobotDataPacket *packet) {
  uchar *data;
  int dlength;
  bool error;

  error = false;
  data   = packet->data;

  if(packet->length < 5*sizeof(float))
    return false;

  data += 5*sizeof(float);
  dlength = packet->length - 5*sizeof(float);

  int size;
  size = image_x_size * image_y_size * 3;
  if(dlength != size)
    return false;

  uchar *row_y,*row_u,*row_v;

  for(int y=0; y<image_y_size; 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<image_x_size; x++){
      row_y[x] = *(data  );
      row_u[x] = *(data+1);
      row_v[x] = *(data+2);
      data+=3;
    }
  }

  return true;
}

bool
VisionRawPacketDecoder::decode(int &image_x_size, int &image_y_size,
			       CMVision::image_yuv<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))
    return false;

  data += 5*sizeof(float);
  dlength = packet->length - 5*sizeof(float);

  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);
        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);
        error = true;
      }
    }
  }

  int size;
  size = image_x_size * image_y_size * 3;
  if(dlength != size)
    return false;

  uchar *row_y,*row_u,*row_v;

  for(int y=0; y<image_y_size; 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<image_x_size; x++){
      row_y[x] = *(data  );
      row_u[x] = *(data+1);
      row_v[x] = *(data+2);
      data+=3;
    }
  }

  return true;
}

bool
VisionRawPacketDecoder::decode(int &image_x_size, int &image_y_size, uchar *out_buf,
			       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;

  // Skip over the head/camera angle fields of the packet.
  if(packet->length < 5*sizeof(float))
    return false;

  data += 5*sizeof(float);
  dlength = packet->length - 5*sizeof(float);

  // Okay, if we skipped the head angles, let's read our width and
  // height.
  if(!error) {

    // Do we have enough data to read our 2 values?
    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);
        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);
        error = true;
      }
    }
  }

  // bail if badness occurs. We should truncate our sizes so we
  // don't think anything ended up in out_buf.
  if(error) {
    image_x_size = 0;
    image_y_size = 0;
    return false;
  }


  int size = image_x_size * image_y_size * 3;

  // The only thing left in the packet should be YUV triplets. Let's make sure
  // the remaining packet data is the right size.
  if(dlength != size) {
    // The price of Scott having time to work on his thesis is that people with
    // goofy senses of humor write code.
    fprintf(stderr,"The packet, she does not contain the right amount of data for a raw2img\n");
    return false;
  }

  // We just do a straight copy into our buffer.
  for(int i=0; i<size; i++)
    out_buf[i] = data[i];

  return true;
}