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

#include <vector>

using namespace std;

#include "../../../agent/headers/DogTypes.h"
#include "../../../agent/headers/CircBufPacket.h"

#include "../shared/LogReader.h"

#include "FieldExtractor.h"
#include "LogExtractor.h"

static const int MaxPacketSize=100*1024;

LogExtractor::LogExtractor() {
  extractors.reserve(5);
}

bool
LogExtractor::configure(const char *filename) {
  FILE *file;

  file=fopen(filename,"r");
  if (NULL==file) {
    return false;
  }
  bool ret=configure(file);
  fclose(file);

  return ret;
}

bool
LogExtractor::configure(FILE *file) {
  bool ok=true;

  extractors.clear();

  char buf[1024];
  vector<char *> parts;

  parts.reserve(10);

  SyncCounter *counter=new SyncCounter;

  while(fgets(buf,1024,file)) {
    parts.clear();

    if(buf[strlen(buf)-1]=='\n')
      buf[strlen(buf)-1]='\x00';

    // get rid of shell style comments
    char *hash_loc;
    hash_loc = strchr(buf,'#');
    if(hash_loc)
      *hash_loc = '\x00';

    // find components
    char *rest;
    char *delim_loc;
    parts.push_back(buf);
    rest = buf;
    while( (delim_loc=strchr(rest,',')) ) {
      rest=delim_loc+1;
      parts.push_back(rest);
      *delim_loc='\x00';
    }

    // remove leading/trailing white space from components
    for(unsigned int i=0; i<parts.size(); i++) {
      char *non_white_start;
      non_white_start = parts[i] + strspn(parts[i]," \t");
      char *non_white_end;
      if(strlen(parts[i])>0) {
        non_white_end   = parts[i] + strlen(parts[i]) - 1;
        while(strspn(non_white_end," \t")>0 && non_white_end >= non_white_start )
          non_white_end--;
        non_white_end++;
      } else {
        non_white_end = parts[i];
      }
      *non_white_end='\x00';
      parts[i]=non_white_start;
    }

    // process parts
    char *field_type;
    field_type=parts[0];
    if(field_type[0]=='\x00') {
    } else if(strcmp(field_type,"head")==0) {
      if(parts.size() < 3) {
        fprintf(stderr,"not enough fields for head type (%d instead of 3)\n",
                parts.size());
        ok = false;
        continue;
      }
      FieldExtractorHeader *extractor=NULL;
      extractor = new FieldExtractorHeader;
      if(strcmp(parts[1],"*")==0) {
        extractor->setPacketTypeAll();
      } else {
        uint type;
        if(sscanf(parts[1],"%x",&type)!=1) {
          fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
          type=0U;
          ok = false;
        }
        extractor->setPacketType((uchar)type);
      }
      extractor->setFieldHeader(parts[2]);
      extractors.push_back(extractor);
    } else if(strcmp(field_type,"body")==0) {
      if(parts.size() < 4) {
        fprintf(stderr,"not enough fields for body type (%d instead of 4)\n",
                parts.size());
        ok = false;
        continue;
      }
      FieldExtractorBodyGeneric *extractor=NULL;
      uint type;
      if(sscanf(parts[1],"%x",&type)!=1) {
        fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
        type=0U;
        ok = false;
      }
      char *data_type=parts[2];
      if(strcmp(data_type,"uchar")==0) {
        extractor = new FieldExtractorBody<uchar>;
        extractor->setFormat("%u");
      } else if(strcmp(data_type,"short")==0) {
        extractor = new FieldExtractorBody<short>;
        extractor->setFormat("%d");
      } else if(strcmp(data_type,"ulong")==0) {
        extractor = new FieldExtractorBody<ulong>;
        extractor->setFormat("%lu");
      } else if(strcmp(data_type,"float")==0) {
        extractor = new FieldExtractorBody<float>;
        extractor->setFormat("%g");
      } else {
        fprintf(stderr,"unrecognized type '%s'\n",data_type);
        ok = false;
        continue;
      }
      extractor->setPacketType(type);
      extractor->setFieldBody(atoi(parts[3]));
      extractors.push_back(extractor);
    } else if(strcmp(field_type,"bodybitfield")==0) {
      if(parts.size() < 7) {
        fprintf(stderr,"not enough fields for body type bitfield offset (%d instead of 7)\n",
                parts.size());
        ok = false;
        continue;
      }
      FieldExtractorBodyBitfieldGeneric *extractor=NULL;
      uint type;
      if(sscanf(parts[1],"%x",&type)!=1) {
        fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
        type=0U;
        ok = false;
      }
      char *data_type=parts[4];
      if(strcmp(data_type,"uchar")==0) {
        extractor = new FieldExtractorBodyBitfield<uchar>;
        extractor->setFormat("%u");
      } else if(strcmp(data_type,"short")==0) {
        extractor = new FieldExtractorBodyBitfield<short>;
        extractor->setFormat("%d");
      } else if(strcmp(data_type,"ulong")==0) {
        extractor = new FieldExtractorBodyBitfield<ulong>;
        extractor->setFormat("%lu");
      } else if(strcmp(data_type,"float")==0) {
        extractor = new FieldExtractorBodyBitfield<float>;
        extractor->setFormat("%g");
      } else {
        fprintf(stderr,"unrecognized type '%s'\n",data_type);
        ok = false;
        continue;
      }
      extractor->setPacketType(type);
      extractor->setBitfieldSize(atoi(parts[2]));
      extractor->setObjectSize  (atoi(parts[3]));
      extractor->setObjectIdx   (atoi(parts[5]));
      extractor->setFieldBody   (atoi(parts[6])); // actually offset within object
      extractors.push_back(extractor);
    } else if(strcmp(field_type,"bodystring")==0) {
      if(parts.size() < 2) {
        fprintf(stderr,"not enough fields for body type string (%d instead of 2)\n",
                parts.size());
        ok = false;
        continue;
      }
      FieldExtractorBodyString *extractor=NULL;
      extractor = new FieldExtractorBodyString;
      uint type;
      if(sscanf(parts[1],"%x",&type)!=1) {
        fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
        type=0U;
        ok = false;
      }
      extractor->setPacketType(type);
      extractors.push_back(extractor);
    } else if(strcmp(field_type,"bodycamang")==0) {
      if(parts.size() < 2) {
        fprintf(stderr,"not enough fields for body type camera angles (%d instead of 2)\n",
                parts.size());
        ok = false;
        continue;
      }
      FieldExtractorBodyCamAngles *extractor=NULL;
      extractor = new FieldExtractorBodyCamAngles(counter);
      uint type;
      if(sscanf(parts[1],"%x",&type)!=1) {
        fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
        type=0U;
        ok = false;
      }
      extractor->setPacketType(type);
      extractors.push_back(extractor);
    } else if(strcmp(field_type,"bodyrawimg")==0) {
      if(parts.size() < 4) {
        fprintf(stderr,"not enough fields for body type raw image (%d instead of 4)\n",
                parts.size());
        ok = false;
        continue;
      }
      uint width,height;
      if(sscanf(parts[2],"%d",&width)!=1) {
        fprintf(stderr,"unable to convert width to number '%s'\n",parts[2]);
        ok = false;
        continue;
      }
      if(sscanf(parts[3],"%d",&height)!=1) {
        fprintf(stderr,"unable to convert height to number '%s'\n",parts[3]);
        ok = false;
        continue;
      }
      FieldExtractorBodyRawImage *extractor=NULL;
      extractor = new FieldExtractorBodyRawImage(counter,width,height);
      uint type;
      if(sscanf(parts[1],"%x",&type)!=1) {
        fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
        type=0U;
        ok = false;
      }
      extractor->setPacketType(type);
      extractors.push_back(extractor);
    }  else if(strcmp(field_type,"bodyraw2img")==0) {
      if(parts.size() < 4) {
        fprintf(stderr,"not enough fields for body type raw2image (%d instead of 4)\n",
                parts.size());
        ok = false;
        continue;
      }
      uint max_width, max_height;
      if(sscanf(parts[2],"%d",&max_width)!=1) {
        fprintf(stderr,"unable to convert max width to number '%s'\n",parts[2]);
        ok = false;
        continue;
      }
      if(sscanf(parts[3],"%d",&max_height)!=1) {
        fprintf(stderr,"unable to convert max height to number '%s'\n",parts[3]);
        ok = false;
        continue;
      }
      FieldExtractorBodyRaw2Image *extractor=NULL;
      extractor = new FieldExtractorBodyRaw2Image(counter, max_width, max_height);
      uint type;
      if(sscanf(parts[1],"%x",&type)!=1) {
        fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
        type=0U;
        ok = false;
      }
      extractor->setPacketType(type);
      extractors.push_back(extractor);
    }  else if(strcmp(field_type,"bodyrleimg")==0) {
      if(parts.size() < 4) {
        fprintf(stderr,"not enough fields for body type rle image (%d instead of 4)\n",
                parts.size());
        ok = false;
        continue;
      }
      uint width,height;
      if(sscanf(parts[2],"%d",&width)!=1) {
        fprintf(stderr,"unable to convert width to number '%s'\n",parts[2]);
        ok = false;
        continue;
      }
      if(sscanf(parts[3],"%d",&height)!=1) {
        fprintf(stderr,"unable to convert height to number '%s'\n",parts[3]);
        ok = false;
        continue;
      }
      FieldExtractorBodyRLEImage *extractor=NULL;
      extractor = new FieldExtractorBodyRLEImage(counter,width,height);
      uint type;
      if(sscanf(parts[1],"%x",&type)!=1) {
        fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
        type=0U;
        ok = false;
      }
      extractor->setPacketType(type);
      extractors.push_back(extractor);
    } else if(strcmp(field_type,"bodyrle2img")==0) {
      if(parts.size() < 4) {
        fprintf(stderr,"not enough fields for body type rle image (%d instead of 4)\n",
                parts.size());
        ok = false;
        continue;
      }
      uint max_width,max_height;
      if(sscanf(parts[2],"%d",&max_width)!=1) {
        fprintf(stderr,"unable to convert max width to number '%s'\n",parts[2]);
        ok = false;
        continue;
      }
      if(sscanf(parts[3],"%d",&max_height)!=1) {
        fprintf(stderr,"unable to convert max height to number '%s'\n",parts[3]);
        ok = false;
        continue;
      }
      FieldExtractorBodyRLE2Image *extractor=NULL;
      extractor = new FieldExtractorBodyRLE2Image(counter,max_width,max_height);
      uint type;
      if(sscanf(parts[1],"%x",&type)!=1) {
        fprintf(stderr,"unable to convert type to hex number '%s'\n",parts[1]);
        type=0U;
        ok = false;
      }
      extractor->setPacketType(type);
      extractors.push_back(extractor);
    } else {
      fprintf(stderr,"field type of '%s' not recognized\n",field_type);
      ok = false;
    }
  }

  return ok;
}

void
LogExtractor::extract(const char *in_filename, const char *out_filename) {
  FILE *in_file,*out_file;

  in_file = fopen(in_filename,"r");
  if(!in_file) {
    fprintf(stderr,"unable to open '%s' for reading\n",in_filename);
    return;
  }
  out_file = fopen(out_filename,"w");
  if(!out_file) {
    fprintf(stderr,"unable to open '%s' for writing\n",out_filename);
    return;
  }

  extract(in_file,out_file);

  fclose(in_file);
  fclose(out_file);
}

void
LogExtractor::extract(FILE *in_file, FILE *out_file) {
  RobotDataPacket packet;
  packet.data = new uchar[MaxPacketSize];

  LogReader log_reader;

  log_reader.setSource(in_file);
  log_reader.seekToStart();

  char buf[1024];
  bool printed;
  while(log_reader.readNextPacket(&packet,MaxPacketSize)) {
    printed = false;
    for(uint ext_idx=0; ext_idx<extractors.size(); ext_idx++) {
      if((*extractors[ext_idx])(buf,&packet)) {
        printed = true;
        printf("%s ",buf);
      }
    }
    if(printed)
      printf("\n");
  }
}