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

#include "../shared/VisionRLEPacketDecoder.h"
#include "../shared/VisionRawPacketDecoder.h"

char const * const FieldExtractorHeader::PTypeStr     ="type";
char const * const FieldExtractorHeader::PLengthStr   ="length";
char const * const FieldExtractorHeader::PTimestampStr="timestamp";
char const * const FieldExtractorHeader::HeaderFieldString[3]=
  {PTypeStr,PLengthStr,PTimestampStr};

FieldExtractorHeader::FieldExtractorHeader() {
  field=P_TYPE;
  all=true;
  packetType='\x00';
}

void
FieldExtractorHeader::setFieldHeader(enum HeaderField field_param) {
  field=field_param;
}

void
FieldExtractorHeader::setFieldHeader(const char *field_param) {
  for(uint field_idx=0; 
      field_idx < sizeof(HeaderFieldString)/sizeof(HeaderFieldString[0]);
      field_idx++) {
    if(strcmp(field_param,HeaderFieldString[field_idx])==0) {
      setFieldHeader((HeaderField)field_idx);
      return;
    }
  }

  fprintf(stderr,"unrecognized header field '%s'\n",field_param);
}

void
FieldExtractorHeader::setPacketTypeAll() {
  all=true;
}

void
FieldExtractorHeader::setPacketType(uchar type) {
  all = false;
  packetType = type;
}

bool
FieldExtractorHeader::operator()(char *string_rep,RobotDataPacket *packet) {
  if(!(all || packet->dataType == packetType))
    return false;

  switch(field) {
  case P_TYPE:
    sprintf(string_rep,"%d",packet->dataType);
    break;
  case P_LENGTH:
    sprintf(string_rep,"%lu",packet->length);
    break;
  case P_TIMESTAMP:
    sprintf(string_rep,"%lu",packet->timestamp);
    break;
  }

  return true;
}

FieldExtractorBodyGeneric::FieldExtractorBodyGeneric() {
  offset = 0;
  packetType = '\x00';
}

void
FieldExtractorBodyGeneric::setFormat(const char *format_param) {
  format = format_param;
}

void
FieldExtractorBodyGeneric::setFieldBody(int offset_param) {
  offset = offset_param;
}

void
FieldExtractorBodyGeneric::setPacketType(uchar type) {
  packetType = type;
}


FieldExtractorBodyBitfieldGeneric::FieldExtractorBodyBitfieldGeneric() {
  objectSize = 0;
  bitfieldSize = 0;
}

void
FieldExtractorBodyBitfieldGeneric::setBitfieldSize(uint size) {
  bitfieldSize = size;
}

void
FieldExtractorBodyBitfieldGeneric::setObjectSize(uint size) {
  objectSize = size;
}

void
FieldExtractorBodyBitfieldGeneric::setObjectIdx(uint idx) {
  objectIdx = idx;
}

FieldExtractorBodyString::FieldExtractorBodyString() {
  packetType = 0;
}

void
FieldExtractorBodyString::setPacketType(uchar type) {
  packetType = type;
}

bool
FieldExtractorBodyString::operator()(char *string_rep,RobotDataPacket *packet) {
  if(packet->dataType != packetType)
    return false;

  memcpy(string_rep,packet->data,packet->length);
  string_rep[packet->length]='\x00';

  return true;
}

SyncCounter::SyncCounter()
{
  value = 0;
  lastTimestamp = ~0UL;
}

int
SyncCounter::getValue() {
  return value;
}

int
SyncCounter::updateValue(ulong timestamp) {
  if(lastTimestamp!=~0UL && timestamp!=lastTimestamp)
    value++;

  lastTimestamp = timestamp;

  return value;
}

FieldExtractorBodyCamAngles::FieldExtractorBodyCamAngles(SyncCounter *counter_param) {
  packetType = 0;
  counter = counter_param;
  decoder = new VisionRLEPacketDecoder(0);

  for(int i=0; i<5; i++)
    angles[i] = 0.0;
}

void
FieldExtractorBodyCamAngles::setPacketType(uchar type) {
  packetType = type;
}

bool
FieldExtractorBodyCamAngles::operator()(char *string_rep,RobotDataPacket *packet) {
  if(packet->dataType != packetType)
    return false;

  decoder->decode(angles,packet);

  counter->updateValue(packet->timestamp);

  char filename[256];
  FILE *out_file;

  sprintf(filename,"i%04d.inf",counter->getValue());
  if((out_file=fopen(filename,"wt"))!=NULL) {
    fprintf(out_file,"%f %f %f %f %f\n",angles[0],angles[1],angles[2],angles[3],angles[4]);
    fclose(out_file);
  }

  strcpy(string_rep,filename);

  return true;
}

const static char ColorTable[][3] = {
  {0x00,0x00,0x00}, /* black */
  {0xff,0x80,0x00}, /* orange */
  {0x00,0x80,0x00}, /* green */
  {0xff,0x00,0x80}, /* pink */
  {0x00,0xff,0xff}, /* cyan */
  {0xff,0xff,0x00}, /* yellow */
  {0x00,0x00,0xff}, /* blue */
  {0xff,0x00,0x00}, /* red */
  {0xff,0xff,0xff}, /* white */
  {0x80,0x80,0x80}, /* gray */
};
static const int BAD_COLOR=9;

FieldExtractorBodyRLEImage::FieldExtractorBodyRLEImage(SyncCounter *counter_param,int image_x_size,int image_y_size) {
  packetType = 0;
  counter = counter_param;
  decoder = new VisionRLEPacketDecoder(BAD_COLOR);
  imageXSize = image_x_size;
  imageYSize = image_y_size;
  size = imageXSize*imageYSize;
  colors = new uchar[size];
  for(int i=0; i<size; i++)
    colors[i] = BAD_COLOR;
  img = new uchar[size*3];
}

void
FieldExtractorBodyRLEImage::setPacketType(uchar type) {
  packetType = type;
}

bool
FieldExtractorBodyRLEImage::operator()(char *string_rep,RobotDataPacket *packet) {
  if(packet->dataType != packetType)
    return false;

  decoder->decode(imageXSize,imageYSize,colors,packet);

  for(int i=0; i<size; i++)
    if(colors[i]<=BAD_COLOR)
      memcpy(&img[i*3],&ColorTable[colors[i]],3);

  counter->updateValue(packet->timestamp);

  char filename[256];
  FILE *out_file;

  sprintf(filename,"c%04d.ppm",counter->getValue());
  if((out_file=fopen(filename,"wb"))!=NULL) {
    fprintf(out_file,"P6\n%d %d\n%d\n",imageXSize,imageYSize,255);
    fwrite(img,sizeof(uchar)*3,size,out_file);
    fclose(out_file);
  }
  
  strcpy(string_rep,filename);

  return true;
}

FieldExtractorBodyRLE2Image::FieldExtractorBodyRLE2Image(SyncCounter *counter_param,
							 int max_image_x_size,int max_image_y_size)
{
  packetType = 0;
  counter = counter_param;
  decoder = new VisionRLEPacketDecoder(BAD_COLOR);
  maxImageXSize = max_image_x_size;
  maxImageYSize = max_image_y_size;
  size = maxImageXSize*maxImageYSize;
  colors = new uchar[size];
  for(int i=0; i<size; i++)
    colors[i] = BAD_COLOR;
  img = new uchar[size*3];
}

void
FieldExtractorBodyRLE2Image::setPacketType(uchar type) {
  packetType = type;
}

bool
FieldExtractorBodyRLE2Image::operator()(char *string_rep,RobotDataPacket *packet) {
  int image_x_size=0,image_y_size=0;
  bool ok=true;

  if(packet->dataType != packetType)
    return false;

  ok = decoder->decode(image_x_size,image_y_size,colors,maxImageXSize,maxImageYSize,packet);
  if(!ok){
    fprintf(stderr,"problem decoding image\n");
    return false;
  }

  for(int i=0; i<image_x_size*image_y_size; i++)
    if(colors[i]<=BAD_COLOR)
      memcpy(&img[i*3],&ColorTable[colors[i]],3);

  counter->updateValue(packet->timestamp);

  char filename[256];
  FILE *out_file;

  sprintf(filename,"c%04d.ppm",counter->getValue());
  if((out_file=fopen(filename,"wb"))!=NULL) {
    fprintf(out_file,"P6\n%d %d\n%d\n",image_x_size,image_y_size,255);
    fwrite(img,sizeof(uchar)*3,image_x_size*image_y_size,out_file);
    fclose(out_file);
  }
  
  strcpy(string_rep,filename);

  return true;
}

FieldExtractorBodyRawImage::FieldExtractorBodyRawImage(SyncCounter *counter_param,int image_x_size,int image_y_size) {
  packetType = 0;
  counter = counter_param;
  imageXSize = image_x_size;
  imageYSize = image_y_size;
  size = imageXSize*imageYSize;
  img = new uchar[size*3];
  for(int i=0; i<size*3; i++)
    img[i]=0;
}

void
FieldExtractorBodyRawImage::setPacketType(uchar type) {
  packetType = type;
}

bool
FieldExtractorBodyRawImage::operator()(char *string_rep,RobotDataPacket *packet) {
  if(packet->dataType != packetType)
    return false;

  counter->updateValue(packet->timestamp);

  string_rep[0]='\x00';

  bool error;
  error = packet->length < (unsigned)(20+size*3);
  if(!error) {
    char filename[256];
    FILE *out_file;
    
    sprintf(filename,"i%04d.ppm",counter->getValue());
    if((out_file=fopen(filename,"wb"))!=NULL) {
      fprintf(out_file,"P6\n%d %d\n%d\n",imageXSize,imageYSize,255);
      fwrite(packet->data+20,sizeof(uchar)*3,size,out_file);
      fclose(out_file);
    }
  
    strcpy(string_rep,filename);
  }

  return !error;
}

FieldExtractorBodyRaw2Image::FieldExtractorBodyRaw2Image(SyncCounter *counter_param,
							 int max_image_x_size,
							 int max_image_y_size) {
  packetType = 0;
  counter = counter_param;
  maxImageXSize = max_image_x_size;
  maxImageYSize = max_image_y_size;
  max_size = maxImageXSize*maxImageYSize;
  img = new uchar[max_size*3];
  for(int i=0; i<max_size*3; i++)
    img[i]=0;
  decoder = new VisionRawPacketDecoder();
}

void
FieldExtractorBodyRaw2Image::setPacketType(uchar type) {
  packetType = type;
}

bool
FieldExtractorBodyRaw2Image::operator()(char *string_rep,RobotDataPacket *packet) {

  int image_x_size = 0, image_y_size = 0;
  bool okay = false;

  if(packet->dataType != packetType)
    return false;

  okay = decoder->decode(image_x_size, image_y_size, img, 
			 maxImageXSize, maxImageYSize, packet);
  if(!okay){
    fprintf(stderr,"problem decoding raw image\n");
    return false;
  }

  counter->updateValue(packet->timestamp);
  
  string_rep[0]='\x00';
  
  char filename[256];
  FILE *out_file;
  
  sprintf(filename,"i%04d.ppm",counter->getValue());
  if((out_file=fopen(filename,"wb"))!=NULL) {
    fprintf(out_file,"P6\n%d %d\n%d\n",image_x_size, image_y_size, 255);
    fwrite(img, sizeof(uchar)*3, image_x_size*image_y_size,out_file);
    fclose(out_file);
  }
  
  strcpy(string_rep,filename);
  
  return true;
}