/* LICENSE:
  =========================================================================
    CMPack'03 Source Code Release for OPEN-R SDK v1.0
    Copyright (C) 2003 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#ifndef OPENR_STUBGEN
#define OPENR_STUBGEN
#endif

#include <algorithm>
#include <iostream>

using std::min;

#include <OPENR/core_macro.h>
#include <OPENR/ObjcommTypes.h>

#include <OPENR/OPENR.h>
#include <OPENR/OPENRMessages.h>

#include "../headers/AperiosMessageStructures.h"
#include "../headers/CircBufPacket.h"
#include "../headers/Config.h"
#include "../headers/DogTypes.h"
#include "../headers/FileSystem.h"
#include "../headers/PacketMux.h"
#include "../headers/Reporting.h"
#include "../headers/SharedMem.h"
#include "../headers/SPOutEncoder.h"
#include "../headers/SystemUtility.h"
#include "../headers/Utility.h"
#include "Logger.h"
#include "LoggerInterface.h"

const bool LogOn = true;
static const int LogBufferSize=8000000;

static const int MaxPacketLength=100*1024; // 100k (need 77k for raw images)
static const int ControlBufferSize = 1024; // 1k
static const int MaxControlMessages = 10;

PacketStream *TextOutputStream = NULL;

static const int LoggerOutputDataSize = 64*1024; // 64KB
static const int TotalTextOutput = 32*1024; // 32KB
static const int MaxTextMessages = 256; // maximum number of in flight text messages

Logger::Logger() 
{
  cout << "--- Logger::Logger()" << endl;
  cout << "    invoked." << endl;

  timerStarted=false;

  needToRegisterControl = false;
  needToRegisterText = false;
  logControlStream = NULL;
  nextControlPacket = 0;

  for(int i=0; i<MemoryId::NUM_MEMORY_IDS; i++)
    requestedRegion[i]=false;

  mainOutPSC   = NULL;
  motionOutPSC = NULL;
  mux = new PacketMuxFIFO();

  quickCheck = true;
  gotMessage = true;

  sError result;
  result = NewRegion(LogBufferSize, reinterpret_cast<void**>(&logBuffer)); 
  if (result != sSUCCESS) { 
    logBuffer = static_cast<uchar*>(NULL); 
    cout << "Unable to allocate log buffer of size " << LogBufferSize << endl;
  } 
  logBufferPos = logBuffer;
}


OStatus
Logger::DoInit(const OSystemEvent& event)
{
  //cout << "--- Logger::DoInit()" << endl;
  //cout << "    invoked." << endl;

  config.init();
  config.config("/MS/config/config.cfg");


  //
  // Initialize myself
  //
  NewComData; 
  SetComData;
 
  // make sure the library doesn't drop data "for" us
  //   on this reliable communication channel
  observer[obsSharedMemRegionInfo]->SetBufCtrlParam(0,1,16);

  return oSUCCESS;
}


OStatus
Logger::DoStart(const OSystemEvent& event)
{
  //cout << "--- Logger::DoStart()" << endl;
  //cout << "    invoked." << endl;

  // our setup

  // allocate the memory for output and allow attachement
  void *base_addr=NULL;
  logTextRgn.init(sizeof(PacketStreamCollection)+LoggerOutputDataSize);
  base_addr = logTextRgn.getData();

  PacketStreamCollection *out_psc;
  uchar *start_address;

  out_psc=(PacketStreamCollection *)base_addr;
  start_address = ((uchar *)base_addr) + sizeof(PacketStreamCollection); 
  out_psc->init(start_address, start_address + LoggerOutputDataSize);

  int text_output_stream_id;
  text_output_stream_id = out_psc->allocateStream(TotalTextOutput,MaxTextMessages);
  TextOutputStream = out_psc->getStream(text_output_stream_id);
  TextOutputStream->type   = SPOutEncoder::TextLead;
  TextOutputStream->binary = false;

  needToRegisterText=true;
  sendOutputBuffer();

  // round the memory size up to a number of pages
  sError error;
  size_t mySize = sizeof(PacketStream)+ControlBufferSize;
  size_t units;
  error = GetPageSize(&units);
  if (error != sSUCCESS) {
    cout << "error: " << error << " getting page size in SharedMem" << endl;
    return oFAIL;
  }
  int numUnits = mySize/units;
  if ((mySize % units) != 0)
    numUnits++;
  mySize = units * numUnits;

  // allocate the memory and allow attachement
  base_addr = NULL;
  logControlRgn.init(mySize);
  base_addr = logControlRgn.getData();

  logControlStream = (PacketStream *)base_addr;

  start_address = ((uchar *)base_addr) + sizeof(PacketStream); 
  logControlStream->init(start_address, start_address + ControlBufferSize, MaxControlMessages);
  logControlStream->type = (uchar)'\xC0';
  logControlStream->binary = true;

  needToRegisterControl=true;
  sendControlBuffer();

  requestRegions();

  for (int i=0; i<numOfObserver; ++i){
    if ( observer[i]->AssertReady() != oSUCCESS ){
      cout << "\tSOARF{" << i << "}" << endl; // Logger assert ready failed
    }
  }   

  startTimer();

  return oSUCCESS;
}    

OStatus
Logger::DoStop(const OSystemEvent& event)
{
  cout << "--- Logger::DoStop()" << endl;
  cout << "    invoked." << endl;

  for (int i=0; i<numOfObserver; ++i){
    observer[i]->DeassertReady();
  }

  return oSUCCESS;
}


OStatus
Logger::DoDestroy(const OSystemEvent& event)
{
  cout << "--- Logger::DoDestroy()" << endl;
  cout << "    invoked." << endl;

  //
  // Destroy myself
  //
  DeleteComData;

  return oSUCCESS;
}

void
Logger::ReadyRegisterRegion(const OReadyEvent &event) {
  gotMessage = true;
}

void
Logger::sendOutputBuffer() {
  //cout << "SharedMem::SendMemBlockID() invoked: memID " << regionId << "." << endl;
  OSubject* thisSubj = subject[sbjRegisterRegion];

  if(logTextRgn.validData() && needToRegisterText) {
    needToRegisterText=false;
    logTextRgn.setId(MemoryId::LoggerOutId);
    thisSubj->SetData(logTextRgn.getMsgForm());
    thisSubj->NotifyObservers();
    //cout << "SharedMemID sent" << endl;
  }
}

void
Logger::sendControlBuffer() {
  //cout << "SharedMem::SendMemBlockID() invoked: memID " << regionId << "." << endl;
  //cout << "sending LogControlBuffer " << logControlRgn.getBase() << endl;
  OSubject* thisSubj = subject[sbjRegisterRegion];

  if(logControlRgn.validData() && needToRegisterControl) {
    needToRegisterControl=false;
    logControlRgn.setId(MemoryId::LoggerControlId);
    thisSubj->SetData(logControlRgn.getMsgForm());
    thisSubj->NotifyObservers();
    //cout << "SharedMemID sent" << endl;
  }
}

void
Logger::requestRegions() {
  const int main_out_id  =MemoryId::MainOutId;
  const int motion_out_id=MemoryId::MotionOutId;

  if(!requestedRegion[main_out_id]) {
    requestedRegion[main_out_id]=true;
    RequestInfo request_info;
    request_info.region_id=main_out_id;
    request_info.obs_id=observer[obsSharedMemRegionInfo]->GetObserverID();
    subject[sbjRequestRegion]->SetData(&request_info,sizeof(RequestInfo));
    subject[sbjRequestRegion]->NotifyObservers();
  }
  if(!requestedRegion[motion_out_id]) {
    requestedRegion[motion_out_id]=true;
    RequestInfo request_info;
    request_info.region_id=motion_out_id;
    request_info.obs_id=observer[obsSharedMemRegionInfo]->GetObserverID();
    subject[sbjRequestRegion]->SetData(&request_info,sizeof(RequestInfo));
    subject[sbjRequestRegion]->NotifyObservers();
  }
}

void
Logger::ReadyRequestRegion(const OReadyEvent &event) {
}

void
Logger::GotMemRegion(const ONotifyEvent &event) {
  gotMessage = true;
    
  int event_num_data = event.NumOfData();
  for(int event_data_id=0; event_data_id<event_num_data; event_data_id++) {
    SMMSharedMemRegion region_info;
    region_info.init((SMMSharedMemRegion::MsgForm)event.RCData(event_data_id));
    switch(region_info.getId()) {
    case MemoryId::MainOutId:
      {
        mainOutMemRgn=region_info;
      
        mainOutPSC = (PacketStreamCollection *)mainOutMemRgn.getData();
        mux->addPSC(mainOutPSC);
      }
    break;
    case MemoryId::MotionOutId:
      {
        motionOutMemRgn=region_info;

        motionOutPSC = (PacketStreamCollection *)motionOutMemRgn.getData();
        mux->addPSC(motionOutPSC);
      }
    break;
    }
  }

  observer[obsSharedMemRegionInfo]->AssertReady();
}

void
Logger::writeLog(char *location) {
  if(LogOn) {
    pprintf(TextOutputStream,"writing log to \"%s\"\n",location);
    
    HFS::FILE *log_file;
    log_file = HFS::fopen(location,"w");
    HFS::fwrite((const void *)logBuffer,1,logBufferPos-logBuffer,log_file);
    HFS::fclose(log_file);

    pprintf(TextOutputStream,"done writing log\n",location);

    //OPENR::Shutdown(OBootCondition(obcbPAUSE_SW));
    *((unsigned long *)0x00000000UL)=0xDEADDEADUL;
  }
}

void
Logger::processControlStream() {
  static RobotDataPacket packet;
  static bool initialized=false;
  if(!initialized) {
    packet.data = new uchar[1024];
    initialized=true;
  }

  if(logControlStream) {
    if(nextControlPacket < logControlStream->firstValidPacket)
      nextControlPacket = logControlStream->firstValidPacket;
    while(nextControlPacket <= logControlStream->lastPacket) {
      if(logControlStream->read(nextControlPacket,&packet,1024)) {
        LogControl control;
        memcpy(&control,packet.data,min((ulong)sizeof(control),packet.length));
        if(control.command == LogControl::WriteLog)
          writeLog("/MS/log");
      }
      nextControlPacket++;
    }
  }
}

void
Logger::logPacket(RobotDataPacket *packet) {
  int header_length = 
    sizeof((uchar)packet->dataType)+
    sizeof((ulong)packet->length)+
    sizeof(packet->timestamp);
  if(logBufferPos - logBuffer < LogBufferSize - MaxPacketLength - header_length) {
    uchar type=(uchar)packet->dataType;
    memcpy(logBufferPos,&type,sizeof(type));
    logBufferPos+=sizeof(type);

    ulong data_length=min((ulong)packet->length,(ulong)MaxPacketLength);
    memcpy(logBufferPos,&data_length,sizeof(data_length));
    logBufferPos+=sizeof(data_length);

    memcpy(logBufferPos,&packet->timestamp,sizeof(packet->timestamp)); 
    logBufferPos+=sizeof(packet->timestamp);

    memcpy(logBufferPos,packet->data,data_length);
    logBufferPos += data_length;
  }
}

void
Logger::ProcessBuffer(void *msg) {
  //cout << "processing buffer" << endl;
  timerStarted=false;

  if(LogOn) {
    static EventTimeReporter reporter("Logger::ProcessBuffer",SecToTime(5.0),SecToTime(100.0),100UL,&TextOutputStream);
    EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

    static RobotDataPacket packet;
    static bool initialized=false;

    if(!initialized) {
#ifdef PLATFORM_APERIOS
      NewRegion(sizeof(uchar)*MaxPacketLength, reinterpret_cast<void**>(&packet.data)); 
#else
      packet.data = new uchar[MaxPacketLength];
#endif
      packet.contentType = RobotDataPacket::EMPTY;
      initialized = true;
    }

    quickCheck = gotMessage;
    gotMessage = false;
    
    ulong start_time = GetTime();
    bool cont=true;
    ulong check_cutoff = quickCheck ? 1*1000000 : 10*1000000;
    while(cont && mux->getNextPacket(&packet,MaxPacketLength)) {
      static EventTimeReporter reporter("Logger process packet",SecToTime(5.0),SecToTime(100.0),100000UL,&TextOutputStream);
      EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

      logPacket(&packet);
      processControlStream();
      
      cont = !(GetTime() - start_time > check_cutoff);
    }

    processControlStream();
  }

  startTimer();
}

void
Logger::startTimer() {
  if(timerStarted)
    return;
  timerStarted=true;
  
  int dummy;

  EventID eid;
  RelativeTime period(0,100);
  TimeEventInfoWithRelativeTime time_info(TimeEventInfo::NonPeriodic, period);

  sError error;
  error=SetTimeEvent(&time_info,myOID_,Extra_Entry[entryProcessBuffer],&dummy,0,&eid);
  if(error!=sSUCCESS) {
    cout << "error :" << error << ":" << endl;
  }
}