/* 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 <Types.h>
#include <OPENR/core_macro.h>
#include <OPENR/ObjcommTypes.h>
#include <OPENR/OPENR.h>
#include <OPENR/OPENRMessages.h>

#include "../headers/AperiosMessageStructures.h"
#include "../headers/AperiosSharedMem.h"
#include "../headers/MessageStructures.hh"
#include "SharedMemMgr.h"

static const bool debug=false;

SharedMemMgr::SharedMemMgr()
{
  cout << "--- SharedMemMgr::SharedMemMgr()" << endl;
  cout << "    invoked." << endl;
  
  regionConnectors=new RegionConnector[MaxRegions];
  for(int i=0; i<MaxRegions; i++) {
    regionConnectors[i].first=NULL;
  }
}

OStatus
SharedMemMgr::DoInit(const OSystemEvent& event)
{
  //cout << "--- SharedMemMgr::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[obsRequestRegion ]->SetBufCtrlParam(0,1,16);
  observer[obsRegisterRegion]->SetBufCtrlParam(0,1,16);

  return oSUCCESS;
}


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

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

  return oSUCCESS;
}    


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

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

  return oSUCCESS;
}


OStatus
SharedMemMgr::DoDestroy(const OSystemEvent& event)
{
  cout << "--- SharedMemMgr::DoDestroy()" << endl;
  cout << "    invoked." << endl;
  
  //
  // Destroy myself
  //
  DeleteComData;
  
  return oSUCCESS;
}

void
SharedMemMgr::ReadyForRegion(const OReadyEvent &event) {
//  ObserverID obs_id=event.SenderID();
//
//  for(int reg_conn_idx=0; reg_conn_idx<MaxRegions; reg_conn_idx++) {
//    RegionConnector *reg_conn=&regionConnectors[reg_conn_idx];
//    if(reg_conn->first!=0) {
//      list<ObserverID>::iterator obs_iter;
//      for(obs_iter=reg_conn->second.begin();
//          obs_iter!=reg_conn->second.end();
//          obs_iter++) {
//        if(*obs_iter == obs_id) {
//          cout << "MMRSZ" << obs_id.oid.GetAddress() << endl;
//          RegionInfo reg_info;
//          reg_info.region_id=reg_conn_idx;
//          reg_info.aperios_region_id=reg_conn->first;
//          if(subject[sbjSendRegion]->IsReady(obs_id)) {
//            subject[sbjSendRegion]->SetData(&reg_info, sizeof(RegionInfo));
//            subject[sbjSendRegion]->NotifyObserver(obs_id);
//            reg_conn->second.erase(obs_iter);
//            break;
//          }
//        }
//      }
//    }
//  }
}

void
SharedMemMgr::RequestRegion(const ONotifyEvent &event) {
  if(debug) cout << "!RNOD" << event.NumOfData() << endl;

  int event_num_data = event.NumOfData();
  for(int event_data_id=0; event_data_id<event_num_data; event_data_id++) {
    RequestInfo request_info=*((RequestInfo *)event.Data(event_data_id));

    int region_id=request_info.region_id;
    
    if (region_id >= MaxRegions) {
      cout << "region_id: " << region_id << " >= MaxRegions: " << MaxRegions << " in SharedMemMgr::RequestRegion." << endl;
      return;
    }
    
    RegionConnector *region_connector=&regionConnectors[region_id];
    
    if(region_connector->first == 0) {
      if(debug) cout << "MMPB" << region_id << "Y" << request_info.obs_id.oid.GetAddress() << endl;
      region_connector->second.push_back(request_info.obs_id);
    }
    else {
      if(debug) {
        SMMSharedMemRegion region_info;
        region_info.init(region_connector->first);
        cout << "MMSR" << region_id << "X" << request_info.obs_id.oid.GetAddress() << "R" << (void *)(region_connector->first->Base()) 
             << "%%" << region_info.getId() << "^^" << endl;
      }
      subject[sbjSendRegion]->SetData(request_info.obs_id,region_connector->first);
      subject[sbjSendRegion]->NotifyObserver(request_info.obs_id);
    }
  }
    
  observer[obsRequestRegion]->AssertReady();
}

void
SharedMemMgr::RegisterRegion(const ONotifyEvent &event) {
  if(debug) cout << "@RGRG" << event.NumOfData() << endl;

  int event_num_data = event.NumOfData();
  for(int event_data_id=0; event_data_id<event_num_data; event_data_id++) {
    SMMSharedMemRegion region;
    int region_id;
    
    region.init((SharedMemRegion::MsgForm)event.RCData(event_data_id));
    region_id = region.getId();
    
    if (region_id < 0) {
      cout << "region_id: " << region_id << " < 0 in SharedMemMgr::RegisterRegion." << endl;
#if defined(PLATFORM_APERIOS_SDK)
      cout << "sender was " << event.SenderID().GetSubjectID().GetOID().GetAddress() << " (bad)" << endl;
#else
      cout << "sender was " << event.SenderID().GetOID().GetAddress() << " (bad)" << endl;
#endif
      return;
    }
    
    if (region_id >= MaxRegions) {
      cout << "region_id: " << region_id << " >= MaxRegions: " << MaxRegions << " in SharedMemMgr::RegisterRegion." << endl;
      return;
    }

    RegionConnector *region_connector=&regionConnectors[region_id];
    
    if(debug) cout << "MMGRW" << region_id << "RC" << (void *)region.getMsgForm() << "loc" << (void *)(region.getMsgForm()->Base()) << endl;
    region_connector->first=SharedMemRegion::cloneMsgForm(region.getMsgForm());
    if(debug) cout << "first" << (void *)(region_connector->first->Base()) << endl;
    
    if(!region_connector->second.empty() ) {
      list<ObserverID>::iterator obs_iter;
      for(obs_iter = region_connector->second.begin();
          obs_iter != region_connector->second.end();
          ) {
        list<ObserverID>::iterator old_iter;
        old_iter=obs_iter++;
        if(debug)
          cout << "MMGSRV" << region_id 
               << "oid" << (*old_iter).oid.GetAddress() 
               << endl;
        subject[sbjSendRegion]->SetData(*old_iter,region.getMsgForm());
        subject[sbjSendRegion]->NotifyObserver(*old_iter);
        region_connector->second.erase(old_iter);
      }
    }
  }

  observer[obsRegisterRegion]->AssertReady();
}