/* 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 "entry.h"
#include "../headers/AperiosSharedMem.h"
#include "../headers/AperiosMessageStructures.h"
#include "../headers/DogTypes.h"
#include "../headers/MessageStructures.hh"
#include "../headers/SharedMem.h"
#include "SPIn.h"
#include "../headers/SPInBufferIDs.h"
#include "../headers/SPOutEncoder.h"

#include "../Main/globals.h"
#include "../headers/Config.h"

const int spin_buffer_size=1024;

const int Debug = 0;

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

  timerStarted=false;
  needToSendBuffer=false;
  baseAddr=NULL;
  inited = false;
  
  for(int i=0; i<MemoryId::NUM_MEMORY_IDS; i++)
    requestedRegion[i]=false;
  //for (int i=0; i<SPIn_NumBuffers; i++) {
  //  circularBuf[i] = NULL;
  //}
}


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

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

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

  // allocate the memory and allow attachement
  inRgn.init(mySize);
  baseAddr = inRgn.getData();

  if (Debug) {
   cout << "mysize: " << mySize << " memAddr: " << ((void *)baseAddr) << endl;
  }

  //size_t *ptrBase = (size_t *)baseAddr;
  //
  //for (int i=0; i<SPIn_NumBuffers; i++) {
  //  const size_t thisOffset = (numPagesForPtrs + numPagesPerBuffer*i)*page_size;
  //  if (Debug) {
  //    cout << "Buffer: " << i << " offset: " << thisOffset << endl;
  //  }
  //  ptrBase[i] = thisOffset;
  //  uchar *thisBuffer = (((uchar *)baseAddr) + thisOffset);
  //  if (Debug) {
  //    cout << "thisBuffer: " << ((void *)thisBuffer) << endl;
  //  }
  //  circularBuf[i]=(CircularBuffer *)thisBuffer;
  //  if (Debug) {
  //    cout << "start: " << ((void *)(thisBuffer+sizeof(CircularBuffer))) << " end: " << ((void *) (thisBuffer+(numPagesPerBuffer*page_size))) << endl;
  //  }
  //  circularBuf[i]->init(thisBuffer+sizeof(CircularBuffer),thisBuffer+(numPagesPerBuffer*page_size));
  //}
/*
  if (Debug) {
    cout << "putting test string in buffers" << endl;
    for (int i=0; i<SPIn_NumBuffers; i++) {
      //pprintf(circularBuf[i], "Initial test string\n");
    }
    cout << "done." << endl;
  }
*/
  needToSendBuffer=true;
  
  // cout << "SPIN: buffers set up" << endl;
  
  sendInputBuffer();
  
  for (int i=0; i<numOfObserver; ++i){
    if ( observer[i]->AssertReady() != oSUCCESS ){
      cout << "\tSPIn Assert Ready Failed {" << i << "}" << endl; // SPIn assert ready failed
    }
  }   

  requestRegions();

  startTimer();
  
  return oSUCCESS;
}    


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

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

  return oSUCCESS;
}


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

  //
  // Destroy myself
  //
  DeleteComData;

  return oSUCCESS;
}

void
SPIn::requestRegions() {
}

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

void
SPIn::GotMemRegion(const ONotifyEvent &event) {
  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::SPOutId:
    //  {
    //    outputMemRgn=region_info;
    //    
    //    OutputBuf = (CircularBuffer *)outputMemRgn.getData();
    //  }
    //break;
    }
  }

  observer[obsSharedMemRegionInfo]->AssertReady();
}



void
SPIn::ReadyInputBuffer(const OReadyEvent &event) {
  //  sendInputBufferIfReady(event.SenderID());
}

void
SPIn::sendInputBuffer() {
//  // cout << "SPIn::sendInputBufferIfReady() invoked." << endl;
//  OSubject* thisSubj = subject[sbjRegisterRegion];
//
//  if(inRgn.validData() && needToSendBuffer) {
//    if (Debug) {
//      cout << "SPIn sending memID" << endl;
//    }
//    needToSendBuffer=false;
//    inRgn.setId(MemoryId::SPInId);
//    thisSubj->SetData(inRgn.getMsgForm());
//    thisSubj->NotifyObservers();
//    if (Debug) {
//      cout << "SPIn SharedMemID sent" << endl;
//    }
//  }
}

void
SPIn::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 << "SPIn::startTimer error :" << error << ":" << endl;
  }

}

void
SPIn::ProcessBuffer(void *msg) {
//  if (Debug > 1) {
//    cout << "SPIn processing cin" << endl;
//  }
//  timerStarted=false;
//  
//  if (!inited) {
//    const int initStringLength = strlen(SPIn_InitString);
//    int count = 0;
//    char ch;
//    
//    // check for initial string
//    
//    while (count < initStringLength) {
//      if (Debug > 1) {
//        cout << "SPIn: waiting for char" << endl;
//      }
//      
//      cin.get(ch);
//      
//      if (Debug > 1) {
//        cout << "init: gotchar: " << ch << endl;
//      }
//      
//      if (ch != SPIn_InitString[count]) {
//	if (Debug > 1) cout << "SPIn incorrect init char" << endl;
//        break;
//      }
//      
//      count++;
//    }
//    
//    if (count < initStringLength) {
//      if (Debug > 1) cout << "SPIn init failed" << endl;
//    } else {
//      if (Debug > 1) cout << "SPIn inited" << endl;
//      /* Now we send a message to the client indicating that we are ready to
//	 accept data */
//      circularBuf[SPIn_SPOutBufferID]->write("??\xff");
//      //pprintf(TextOutputStream, "%c\xfe",SPOutEncoder::AckLead);
//      inited = true;
//    }
//
//  } else if (baseAddr!=NULL) {
//    //cout << "checking buffer" << endl;
//    //cout << "buffer length " << circularBuf->spaceUsed() << endl;
//    
//    if (Debug > 1) {
//      cout << "SPIn: waiting for buffer ID" << endl;
//    }
//
//    char bufID;
//    
//    cin.get(bufID);
//    
//    if (Debug > 1) {
//      cout << "SPIn: got buffer ID char" << endl;
//    }
//    // Read in the buffer
//    static char chbuf[spin_buffer_size+3];
//    
//    if (Debug > 1) {
//      cout << "reading into buffer: " << ((int)bufID) << endl;
//    }
//    
//    cin.get(chbuf, spin_buffer_size, '\n');
//    char spare;
//    
//    cin.get(spare); // read the terminator off the input buffer
//
//    int len = strlen(chbuf);
//
//    chbuf[len++] = '\n';
//    chbuf[len] = '\0';
//
//    if ((bufID >= '0') && (bufID < '0'+SPIn_NumBuffers)) bufID -= '0';
//    if ((bufID < 0) || (bufID >= SPIn_NumBuffers)) {
//      /* Throw out the useless buffer.
//	 However, if it looks like the client is trying to init some more,
//	 reply again. */
//      if (bufID == SPIn_InitString[0])
//	// && !strcmp(chbuf, SPIn_InitString+1))
//	circularBuf[SPIn_SPOutBufferID]->write("??\xff");
//      else
//	pprintf(TextOutputStream, "Errant channel %d (%c)\n", (int)bufID, bufID);
//    } else {      
//      chbuf[len-1] = '\xff';
//      chbuf[len] = '\0';
//      if (!circularBuf[bufID]->write((uchar *)chbuf)) {
//        pprintf(TextOutputStream, "sentence discarded: buffer full\n");
//      } else if (Debug > 1) {
//        cout << "LFT " << circularBuf[bufID]->spaceUsed() << endl;
//      }
//    }
//  }
//
//  startTimer();
//  
//  if (Debug > 1) {
//    cout << "SPIn::processBuffer done" << endl;
//  }
}