/* 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 <iostream>
#include <fstream>
using namespace std;

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

#include "LogReader.h"

LogReader::LogReader() {
  logFile = NULL;
}

LogReader::~LogReader() {
  if (NULL!=logFile) {
    closeSource();
  }
  if (!packet_q.empty()) {
    for (uint i=0;i<packet_q.size();i++) {
      delete packet_q[i]->data;
      delete packet_q[i];
    }
    packet_q.clear();
  }
}

void
LogReader::setSource(FILE *log_file,uint data_length) {

  int count=0;
  logFile = log_file;
  if (0!=data_length) {
    bool done=false;
    while (!done) {
      RobotDataPacket * p = new RobotDataPacket();
      if(readNextPacket(p,data_length,-1,true)) {
	packet_q.push_back(p);
	count++;
      } else {
	delete p;
	done=true;
      }
    }
  }
}

void
LogReader::seekToStart() {
  rewind(logFile);
}

void
LogReader::closeSource() {
  fclose(logFile);
  logFile=NULL;
}

bool
LogReader::readNextPacket(RobotDataPacket *packet,
			  uint data_length,
			  int idx,
			  bool initialize_packet_data) {
  //======================================================================
  // 
  // Retrieve the next data frame from the logfile
  //
  if (-1 == idx) {

    static uint header_length = 
      sizeof(packet->dataType)+
      sizeof(packet->length)+
      sizeof(packet->timestamp);
    
    uchar buf[header_length];
    if(fread(buf,sizeof(uchar),header_length,logFile) < header_length) {
      return false;
    }

    memcpy(&packet->dataType, &buf[0],sizeof(packet->dataType));
    memcpy(&packet->length,   &buf[sizeof(packet->dataType)],sizeof(packet->length));
    memcpy(&packet->timestamp,&buf[sizeof(packet->dataType)+
				   sizeof(packet->length)],sizeof(packet->timestamp));
    
    if (initialize_packet_data) {
      packet->data = new uchar[packet->length];
    } else {
      if(data_length < packet->length) {
	fprintf(stderr,"data length of %u to small for packet of length %lu\n",
		data_length,packet->length);
	return false;
      }
    }
    
    if(fread(packet->data,sizeof(uchar),packet->length,logFile) < packet->length) {
      if (initialize_packet_data) {
	delete [] packet->data;
      }
      return false;
    }
    
    return true;
  } else {
    //======================================================================
    // 
    // Retrieve the cached data frame instead
    //
    // Check the index
    if (packet_q.empty() || (0 > idx) || ((int)packet_q.size() <= idx)) {
      return false;
    }
    // Fill the packet with the data
    memcpy(&packet->dataType,&packet_q[idx]->dataType,sizeof(packet->dataType));
    memcpy(&packet->length,&packet_q[idx]->length,sizeof(packet->length));
    memcpy(&packet->timestamp,&packet_q[idx]->timestamp,sizeof(packet->timestamp));
    memcpy(packet->data,packet_q[idx]->data,packet->length);
    return true;
  }
}