/* 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.
  ========================================================================= */

#ifndef OPENR_STUBGEN
#define OPENR_STUBGEN
#endif

#include <iostream>

#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/MessageStructures.hh"
#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 "SPOut.h"

const bool OutputOn = true;

static const int MaxPacketLength=100*1024; // needs to be at least 77k for raw images

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

  timerStarted=false;
  needToSendBuffer=false;
  baseAddr=NULL;
  
  mainOutPSC = NULL;
  motionOutPSC = NULL;
  loggerOutPSC = NULL;
  wlOutPSC = NULL;
  mux = new PacketMuxRoundRobin();

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

  quickCheck = true;
  gotMessage = true;
}


OStatus
SPOut::DoInit(const OSystemEvent& event)
{
  //cout << "--- SPOut::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
SPOut::DoStart(const OSystemEvent& event)
{
  //cout << "--- SPOut::DoStart()" << endl;
  //cout << "    invoked." << endl;

  sError error;
  // round the memory size to a number of pages
  size_t mySize = spout_buffer_size;
  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;

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

  requestRegions();

  startTimer();

  return oSUCCESS;
}    

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

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

  return oSUCCESS;
}


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

  //
  // Destroy myself
  //
  DeleteComData;

  return oSUCCESS;
}

void
SPOut::requestRegions() {
  const int main_out_id  =MemoryId::MainOutId;
  const int motion_out_id=MemoryId::MotionOutId;
  const int logger_out_id=MemoryId::LoggerOutId;
  const int wl_out_id    =MemoryId::WLOutId;

  if(!mainOutMemRgn.validData() && !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(!motionOutMemRgn.validData() && !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();
  }
  if(!loggerOutMemRgn.validData() && !requestedRegion[logger_out_id]) {
    requestedRegion[logger_out_id]=true;
    RequestInfo request_info;
    request_info.region_id=logger_out_id;
    request_info.obs_id=observer[obsSharedMemRegionInfo]->GetObserverID();
    subject[sbjRequestRegion]->SetData(&request_info,sizeof(RequestInfo));
    subject[sbjRequestRegion]->NotifyObservers();
  }
  if(!wlOutMemRgn.validData() && !requestedRegion[wl_out_id]) {
    requestedRegion[wl_out_id]=true;
    RequestInfo request_info;
    request_info.region_id=wl_out_id;
    request_info.obs_id=observer[obsSharedMemRegionInfo]->GetObserverID();
    subject[sbjRequestRegion]->SetData(&request_info,sizeof(RequestInfo));
    subject[sbjRequestRegion]->NotifyObservers();
  }
}

void
SPOut::ReadyRequestRegion(const OReadyEvent &event) {
  gotMessage = true;

}

void
SPOut::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));
    //cout << "=+" << (void *)event.RCData(event_data_id)->Base() << endl;
    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;
    case MemoryId::LoggerOutId:
      {
        loggerOutMemRgn=region_info;
        
        loggerOutPSC = (PacketStreamCollection *)loggerOutMemRgn.getData();
        mux->addPSC(loggerOutPSC);
      }
    break;
    case MemoryId::WLOutId:
      {
        wlOutMemRgn=region_info;
        
        wlOutPSC = (PacketStreamCollection *)wlOutMemRgn.getData();
        mux->addPSC(wlOutPSC);
      }
    break;
    }
  }

  observer[obsSharedMemRegionInfo]->AssertReady();
}

void
SPOut::sendHeader(RobotDataPacket *packet) {
  uchar buf_8bit[8]; // 1 extra to make it nice and aligned
  uchar buf_7bit[8];

  //cout << "hi there" << packet->length << "length" << packet->timestamp << "ts" << endl;
  sendByte((uchar)'\x80');
  sendByte((uchar)packet->dataType);
  sendByte((uchar)((packet->length     ) & 0x7F));
  sendByte((uchar)((packet->length >> 7) & 0x7F));
  memset(buf_8bit,0,sizeof(buf_8bit));
  memcpy(buf_8bit,&packet->timestamp,sizeof(packet->timestamp));
  SPOutEncoder::encode7bits(buf_7bit,buf_8bit);
  for(int i=0; i<5; i++)
    sendByte((uchar)(buf_7bit[i]));
}

void
SPOut::sendTextData(uchar *data, int length) {
  for(int i=0; i<length; i++)
    sendByte(data[i]);
}

void
SPOut::sendBinaryData(uchar *data, int length) {
  uchar buf_8bit[8]; // 1 extra to make it nice and aligned
  uchar buf_7bit[8];

  while(length > 7) {
    memcpy(buf_8bit,data,7);
    SPOutEncoder::encode7bits(buf_7bit,buf_8bit);
    for(int i=0; i<8; i++)
      sendByte((uchar)buf_7bit[i]);
    length -= 7;
    data += 7;
  }

  memcpy(buf_8bit,data,length);
  memset(buf_8bit+length,0,7-length);
  SPOutEncoder::encode7bits(buf_7bit,buf_8bit);
  for(int i=0; i<((8*length+6)/7); i++)
    sendByte((uchar)buf_7bit[i]);
}

void
SPOut::sendPacket(RobotDataPacket *packet) {
  sendHeader(packet);

  switch(packet->contentType) {
  case RobotDataPacket::TEXT:
    sendTextData(packet->data,packet->length);
    break;
  case RobotDataPacket::BINARY:
    sendBinaryData(packet->data,packet->length);
    break;
  default:
    // EMPTY buffer!
    // shouldn't get here
    break;
  }
}

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

  if(OutputOn) {
    static EventTimeReporter reporter("SPOut::ProcessBuffer",SecToTime(5.0),SecToTime(100.0),100UL,NULL);
    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 : 5*1000000;
    while(cont && mux->getNextPacket(&packet,MaxPacketLength)) {
      static EventTimeReporter reporter("SPOut process packet",SecToTime(5.0),SecToTime(100.0),100000UL,NULL);
      EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

      sendPacket(&packet);
      
      cont = (GetTime() - start_time <= check_cutoff);
      //cont = false;
    }
  }

  startTimer();
}

void
SPOut::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;
  }
}