/* 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 "../../agent/headers/CircBufPacket.h"
#include "../../agent/Localization/GLocalization.h"
#include "../../agent/Motion/MotionInterface.h"

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

#include "../log_processing/shared/BehaviorPacketDecoder.h"
#include "../log_processing/shared/LocalizationPacketDecoder.h"
#include "../log_processing/shared/MotionPacketDecoder.h"
#include "../log_processing/shared/VisionPacketDecoder.h"
#include "../log_processing/shared/VisionRawPacketDecoder.h"
#include "../log_processing/shared/VisionRLEPacketDecoder.h"
#include "../log_processing/shared/WorldModelPacketDecoder.h"
#include "../log_processing/shared/TrackerPacketDecoder.h"
#include "../log_processing/shared/WMDebugPacketDecoder.h"

#include "RobotViewData.h"

#include "DataInterface.h"

extern const int BadColor=9;
extern const int ImageXSize=176;
extern const int ImageYSize=144;

using namespace Motion;

static const int MaxPacketLength = 100*1024; // 100kB

DataInterface::DataInterface() {
  currentLog = NULL;

  packet = new RobotDataPacket;
  packet->data = new uchar[MaxPacketLength];
  time_packet = new RobotDataPacket;
  time_packet->data = new uchar[MaxPacketLength];

  allRobotViewData = new AllRobotViewData;
  memset(allRobotViewData,0,sizeof(*allRobotViewData));

  locDecoder         = new LocalizationPacketDecoder ;
  motDecoder         = new MotionPacketDecoder       ;
  visDecoder         = new VisionPacketDecoder       ;
  wmDecoder          = new WorldModelPacketDecoder   ;  
  trackerDecoder     = new TrackerPacketDecoder;
  wmDebugDecoder     = new WMDebugPacketDecoder;
  visRLEDecoder      = new VisionRLEPacketDecoder(BadColor);
  visRawDecoder      = new VisionRawPacketDecoder();
  behNamesDecoder    = new BehaviorNamesPacketDecoder;
  behTraceDecoder    = new BehaviorTracePacketDecoder;

  worldViewActive   = false;
  egoViewActive     = false;
  evVisRadMapActive = true ;
  evModRadMapActive = true ;
  visRLEActive      = false;

  log_idx=-1;
}

void
DataInterface::setLog(const char *filename) {
  if (NULL != currentLog) {
    currentLog->closeSource();
    delete currentLog;
  }
  currentLog=new LogReader;
  FILE *log_file;
  log_file = fopen(filename,"r");
  if(!log_file) {
    fprintf(stderr,"error opening log file '%s' for reading\n",filename);
  } else {
    currentLog->setSource(log_file);
  }
}

void
DataInterface::cacheLog(const char *filename) {
  if (NULL != currentLog) {
    currentLog->closeSource();
    delete currentLog;
  }
  currentLog=new LogReader;
  FILE *log_file;
  log_file = fopen(filename,"r");
  if(!log_file) {
    fprintf(stderr,"error opening log file '%s' for reading\n",filename);
  } else {
    currentLog->setSource(log_file,MaxPacketLength);
  }
}

void
DataInterface::rewindLog() {
  if(currentLog) {
    currentLog->seekToStart();
  }
}

class WaveServerConfig;
extern void ProcessPacket(int robot_id,WaveServerConfig *config,RobotDataPacket *packet);

void
DataInterface::gotoLogPosition(int idx) {
  if (currentLog) {
    if (currentLog->readNextPacket(packet,MaxPacketLength,idx)) {
      allRobotViewData->robotsActive[0]=true;
      ProcessPacket(0,NULL,packet);
    }
  }
}

unsigned int DataInterface::logLength() {
  if (currentLog) {
    return currentLog->size();
  } else {
    return 0;
  }
}

bool DataInterface::activeLog() {
  if (currentLog) {
    return true;
  } else {
    return false;
  }
}

unsigned long DataInterface::getPacketTimestamp(int idx) {
  unsigned long retval = 0;
  if (-1==idx) {
    return packet->timestamp;
  }
  if (currentLog) {
    if (currentLog->readNextPacket(time_packet,MaxPacketLength,idx)) {
      retval = time_packet->timestamp;
    }
  }  
  return retval;
}

void
DataInterface::stepLog() {
  if(currentLog) {
    bool got_full_set=false;
    while(!got_full_set) {
      if(currentLog->readNextPacket(packet,MaxPacketLength)){
        allRobotViewData->robotsActive[0] = true;
        got_full_set = true;
        //printf("processing packet of type %x at time %lu from log\n",packet->dataType,packet->timestamp);
        ProcessPacket(0,NULL,packet);
      }else{
        break;
      }
    }
  }
}

void
DataInterface::stepLogMove() {
  if(currentLog) {
    bool moved=false;
    while(!moved) {
      if(currentLog->readNextPacket(packet,MaxPacketLength)){
        allRobotViewData->robotsActive[0] = true;
        //printf("processing packet of type %x at time %lu from log\n",packet->dataType,packet->timestamp);
        ProcessPacket(0,NULL,packet);
        if(packet->dataType == PacketDecoder::MotionUpdateLead) {
          MotionPacketDecoder decoder;
          Motion::MotionLocalizationUpdate mlu;
          if(decoder.decode(&mlu,packet)) {
            moved = (mlu.pos_delta.length()!=0 || mlu.heading_delta.angle());
          }
        }
      }else{
        break;
      }
    }
  }
}

bool
DataInterface::getWorldViewActive() {
  return worldViewActive;
}

void
DataInterface::setWorldViewActive(bool new_state) {
  worldViewActive = new_state;
}

bool
DataInterface::getEgoViewActive() {
  return egoViewActive;
}

void
DataInterface::setEgoViewActive(bool new_state) {
  egoViewActive = new_state;
}

bool
DataInterface::getEVVisRadMapActive() {
  return evVisRadMapActive;
}

void
DataInterface::setEVModRadMapActive(bool new_state) {
  evModRadMapActive = new_state;
}

bool
DataInterface::getEVModRadMapActive() {
  return evModRadMapActive;
}

void
DataInterface::setEVVisRadMapActive(bool new_state) {
  evVisRadMapActive = new_state;
}

bool
DataInterface::getVisRLEActive() {
  return visRLEActive;
}

void
DataInterface::setVisRLEActive(bool new_state) {
  visRLEActive = new_state;
}

void
DataInterface::quit() {
  delete currentLog;
  delete packet->data;
  delete packet;

  delete allRobotViewData;

  delete locDecoder;
  delete motDecoder;
  delete visDecoder;
  delete wmDecoder ;
  delete trackerDecoder;
  delete wmDebugDecoder;
}