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

using namespace std;

#include <OPENR/core_macro.h>
#include <OPENR/ObjcommTypes.h>
#include <OPENR/OFbkImage.h>
#include <OPENR/OPENR.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OPENRMessages.h>

#include "MainObject.h"

#include "../Behaviors/constants.h"
#include "../headers/Config.h"
#include "../headers/CircBufPacket.h"
#include "../headers/Dictionary.h"
#include "../headers/Reporting.h"
#include "../headers/SharedMem.h"
#include "../headers/SPInBufferIDs.h"
#include "../headers/SPOutEncoder.h"
#include "../headers/Utility.h"
#include "../headers/WaveLAN.h"
#include "../headers/RoboCupGameControlData.h" // long filename, neh?
#include "../Behaviors/Behaviors.h"
#include "../Motion/MotionInterface.h"
#include "../Vision/Vision.h"
#include "../Vision/VisionInterface.h"
#include "globals.h"
#include "RobotMain.h"

PacketStream *TextOutputStream = NULL;
Motion::MotionLocalizationUpdateQueue *LocUpdateQueueBuffer = NULL;
PacketStream *LogControlStream = NULL;

char * camera_config_filename;

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

// This flag allows us to broadcast role messages even though they
// only have a single destination. This is useful if you sneak
// a linux box in as a ficticious teammate that listens in on
// messages and displays them.
static const bool broadcast_role_msgs = true;

extern MainObject Self;

MainObject::MainObject() :
  gotHeadPos(false),
  GSensorStream(NULL),
  FootSensorStream(NULL),
  DutyCycleStream(NULL)
{
  cout << "--- MainObject::MainObject()" << endl;
  cout << "    invoked." << endl;

  main = &RobotMain::GetObject();

  char design[256];
  OPENR::GetRobotDesign(design);
  if(strcmp(design,"ERS-210")==0){
    model = "ERS210";
    camera_config_filename = "/MS/config/ers210/camera.cfg";
  }else{
    model = "ERS7";
    camera_config_filename = "/MS/config/ers7/camera.cfg";
  }

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

  // Do we need to broadcast queue status or role messages?
  need_to_send_status = false;
  need_to_send_role = false;

  takeSnapshot = false;

  have_played = false;

}

MainObject &
MainObject::GetObject() {
  return Self;
}

char*
MainObject::robotModel() {
  return model;
}

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

  cout << "    finished." << endl;

  return oSUCCESS;
}


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

  main->init();

  // allocate the memory and allow attachement
  void *base_addr=NULL;
  outMemRgn.init(sizeof(PacketStreamCollection)+MainOutputDataSize);
  base_addr = outMemRgn.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 + MainOutputDataSize);

  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;

  int g_sensor_stream_id;
  static const int message_size = 3*sizeof(float);
  g_sensor_stream_id = out_psc->allocateStream(message_size*40,40);
  GSensorStream = out_psc->getStream(g_sensor_stream_id);
  GSensorStream->type   = SPOutEncoder::GSensorLead;
  GSensorStream->binary = true;

  int foot_sensor_stream_id;
  static const int foot_message_size = sizeof(uchar);
  foot_sensor_stream_id = out_psc->allocateStream(foot_message_size*40,40);
  FootSensorStream = out_psc->getStream(foot_sensor_stream_id);
  FootSensorStream->type   = SPOutEncoder::FootSensorLead;
  FootSensorStream->binary = true;

  int duty_cycle_stream_id;
  static const int duty_message_size = 12*sizeof(float);
  duty_cycle_stream_id = out_psc->allocateStream(duty_message_size*40,40);
  DutyCycleStream = out_psc->getStream(duty_cycle_stream_id);
  DutyCycleStream->type   = SPOutEncoder::DutyCycleLead;
  DutyCycleStream->binary = true;

  main->start(out_psc);
  main->teamMsgMgr.initializeStreams(out_psc);
  main->teamMsgMgr.setPlayerInfo(main->robotID);

  needToSendOutBuffer = true;
  sendOutputBuffer();

  requestRegions();

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

  setCameraParam();

  //cout << "#leaving MainObject::DoStart" << endl;

  return oSUCCESS;
}    

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

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

OStatus
MainObject::DoStop(const OSystemEvent& event)
{
  cout << "--- MainObject::DoStop()" << endl;
  cout << "    invoked." << endl;
  
  for (int i=0; i<numOfObserver; ++i){
    observer[i]->DeassertReady();
  }
  
  return oSUCCESS;
}


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

  main->destroy();

  //
  // Destroy myself
  //
  DeleteComData;
  
  outputMemRgn.release();
  inputMemRgn.release();
  motionLocUpdateMemRgn.release();
  logControlMemRgn.release();

  return oSUCCESS;
}

void
MainObject::requestRegions() {
  const int spout_id   =MemoryId::SPOutId;
  const int wlout_id = MemoryId::WLOutId;
  //const int spin_id    =MemoryId::SPInId;
  const int fbk_id     =MemoryId::MotionLocUpdateId;
  const int log_ctrl_id=MemoryId::LoggerControlId;

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

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

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

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

void MainObject::ReadyGeneric(const OReadyEvent &event) { }

void
MainObject::sendMotionCommand(MotionCommand &command) {
  subject[sbjMotionControl]->SetData(&command,sizeof(command));
  subject[sbjMotionControl]->NotifyObservers();
}

void
MainObject::sendWalkParams(WalkParam &params) {
  subject[sbjWalkParams]->SetData(&params,sizeof(params));
  subject[sbjWalkParams]->NotifyObservers();
}

void
MainObject::GotSensorFrame(const ONotifyEvent &event) {
  static EventTimeReporter reporter("MainObject::GotSensorFrame",SecToTime(5.0),SecToTime(100.0),
				    1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  static ulong last_time=0UL;
  ulong cur_time, timestamp;

  cur_time = GetTime();
  timestamp = cur_time; // we modify cur_time below for some reason
  if(cur_time - 8000*3 < last_time) {
    cur_time = last_time + 8000*3 + 1;
  }
  last_time = cur_time;

  for(int i=0; i<event.NumOfData(); i++) {
    
    // Receiving Sensor Data from OVRComm 
    OSensorFrameVectorData* sensor = reinterpret_cast<OSensorFrameVectorData *>(event.RCData(i)->Base());

    // The robot sensor frame that this data comes from (sensor frames are 8ms each)
    // (remember that 4 sensor frames are packed in each message)
    ulong s_frame_no = sensor->info[0].frameNumber;
    
    if(main->sensorData){
      main->sensorData->read(sensor, timestamp, s_frame_no);
      
      main->processSensorFrame(cur_time);

      static int count=0;
      if(config.spoutConfig.dumpGSensor!=0) {
	count = (count + 1) % config.spoutConfig.dumpGSensor;
	if(count==0) {
	  static uchar buf[sizeof(float)*3];
	  uchar *bufp;
	  
	  if(GSensorStream!=NULL) {
	    for(int i=-3; i<=0; i++) {
	      bufp=buf;
	      SPOutEncoder::encodeAs<float>(&bufp,main->sensorData->getFrame(i)->accel.x);
	      SPOutEncoder::encodeAs<float>(&bufp,main->sensorData->getFrame(i)->accel.y);
	      SPOutEncoder::encodeAs<float>(&bufp,main->sensorData->getFrame(i)->accel.z);
	      GSensorStream->writeBinary(buf,sizeof(float)*3,cur_time+8000*i);
	    }
	  }
	}
      }
    
      static int count2=0;
      if(config.spoutConfig.dumpFootSensor!=0) {
	count2 = (count2 + 1) % config.spoutConfig.dumpFootSensor;
	if(count2==0) {
	  if(FootSensorStream!=NULL) {
	    for(int i=-3; i<=0; i++) {
	      uchar val=0U;
	      for(int foot_idx=0; foot_idx<4; foot_idx++)
		val |= (main->sensorData->getFrame(i)->paw[foot_idx] ? 1U<<foot_idx : 0);
	      FootSensorStream->writeBinary(&val,sizeof(uchar),cur_time+8000*i);
	    }
	  }
	}
      }
      
      
      static int count3=0;
      if(config.spoutConfig.dumpDutyCycle!=0) {
	count3 = (count3 + 1) % config.spoutConfig.dumpDutyCycle;
	if(count3==0) {
	  static uchar buf[sizeof(float)*12];
	  uchar *bufp;
	  
	  if(DutyCycleStream!=NULL) {
	    for(int i=-3; i<=0; i++) {
	      bufp=buf;
	      for(int j = 0; j < 12; j++){
		SPOutEncoder::encodeAs<float>(&bufp,main->sensorData->getFrame(i)->legDutyCycles[j]);
	      }
	      DutyCycleStream->writeBinary(buf,sizeof(float)*12,cur_time+8000*i);
	    }
	  }
	}
      }
      
      gotHeadPos = true;
    }
  }

  observer[obsSensorFrame]->AssertReady();

  ProcessImageRegion(NULL);
}

void
MainObject::GotImage(const ONotifyEvent &event) {
  static EventTimeReporter reporter("MainObject::GotImage",SecToTime(5.0),SecToTime(100.0),
				    1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
  
  static RCRegion *data_region = NULL;
  
  /* FRAMERATE PRINTOUTS */
  static EventRateReporter frame_event("frame",1000000UL,50,&TextOutputStream);
  frame_event.addToCount(config.spoutConfig.dumpRate);

  data_region = event.RCData(0);
  ProcessImageRegion(data_region);
  
  observer[obsImage]->AssertReady();
}

void
MainObject::ProcessImageRegion(RCRegion *_data_region)
{
  static EventTimeReporter reporter("MainObject::ProcessImageRegion",SecToTime(5.0),SecToTime(100.0),
				    1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);


  static RCRegion *data_region = NULL;
  static bool processed_data = true; // so we don't process NULL
  bool force_process = false;

  // What is the frame # of the most recent sensor data?
  ulong last_sensor = main->sensorData->getFrame(0)->sensor_frame;

  // If _data_region is not null, new data is available from the camera.
  // Otherwise we're being called from the sensor update so we can
  // process a delayed image (images are held back if the corresponding
  // sensor frame hasn't arrived yet)

  // Deal with the case of having processed the data already and no
  // new image being available.
  if(_data_region==NULL && processed_data)
    return;

  // If we don't have old data laying around, move the new region into
  // our buffer. When we move a region into the buffer, we add a reference
  // so Aperios doesn't free it on us.
  if(processed_data) {
    // We know _data_region is not null due to if test above
    data_region = _data_region;
    data_region->AddReference();
    processed_data = false;
  } else {
    // we have not processed the current buffer. if _data_region is
    // not null we should force the issue to make space in the buffer.
    // if not, no worries, we'll wait until the sensor data comes
    // in or a new image arrives.
    
    if(_data_region!=NULL)
      force_process = true;
  }

  // ASSERT(data_region!=NULL)

  OFbkImageVectorData* fbkImageVectorData;
  fbkImageVectorData =(OFbkImageVectorData *)data_region->Base();
    
  uint32 image_frame = fbkImageVectorData->info[0].frameNumber;

  // If we're not forcing processing, make sure that we have the
  // appropriate sensor frame already.
  if(!force_process && last_sensor < image_frame) {
    return;
  }

  if(gotHeadPos) {
    
    OFbkImageInfo* info = fbkImageVectorData->GetInfo(ofbkimageLAYER_H);
    byte* dat = fbkImageVectorData->GetData(ofbkimageLAYER_H);
    OFbkImage yimage(info, dat, ofbkimageBAND_Y);
    OFbkImage uimage(info, dat, ofbkimageBAND_Cr);
    OFbkImage vimage(info, dat, ofbkimageBAND_Cb);
    
    uchar* bytes_y = (uchar *)(yimage.Pointer());
    uchar* bytes_u = (uchar *)(uimage.Pointer());
    uchar* bytes_v = (uchar *)(vimage.Pointer());
    
    int width  = yimage.Width();
    int height = yimage.Height();
    int skip   = yimage.Skip();

    ulong time = GetTime();
    
    MotionCommand *mcommand;
    
    mcommand = main->processImage(time,bytes_y,bytes_u,bytes_v,width,height,skip,image_frame);
    sendMotionCommand(*mcommand);
  }

  // We are finished with the current data_region. Let's decrement its
  // reference count so it can be deallocated.
  data_region->RemoveReference();
  data_region = NULL;
  processed_data = true;

  // If we forced processing, we need to save _data_region in our buffer
  // and increment its reference count.
  if(force_process) {
    pprintf(TextOutputStream, "forced process\n");
    data_region = _data_region;
    data_region->AddReference();
    processed_data = false;
  }
}

// static void hexout(unsigned int in) {
//   char buf;
//   cout << "0x";
//   if (in == 0) cout << "0";
//   else {
//     while (in != 0) {
//       if (in % 16 > 9) buf = 'A' + in % 16 - 9;
//       else buf = '0' + in % 16;
//       cout << buf;
//       in /= 16;
//     }
//   }
//   cout << endl;
// }

void
MainObject::GotMemRegion(const ONotifyEvent &event) {
  int event_num_data = event.NumOfData();
  //cout << "~MDN" << event_num_data << endl;
  for(int event_data_id=0; event_data_id<event_num_data; event_data_id++) {
    SMMSharedMemRegion region_info;
    //cout << "got reg" << event_data_id << endl;
    region_info.init((SMMSharedMemRegion::MsgForm)event.RCData(event_data_id));
    //cout << "$" << (void *)(event.RCData(event_data_id)->Base()) << "," << region_info.getId() << "&" << endl;
    switch(region_info.getId()) {
    
    case MemoryId::MotionLocUpdateId:
      {
        motionLocUpdateMemRgn = region_info;
        
        LocUpdateQueueBuffer = (MotionLocalizationUpdateQueue *)motionLocUpdateMemRgn.getData();
        memset(LocUpdateQueueBuffer,0,sizeof(MotionLocalizationUpdateQueue));
      }
    break;
    
    case MemoryId::LoggerControlId:
      {
        logControlMemRgn = region_info;
        
        LogControlStream = (PacketStream *)logControlMemRgn.getData();
      }
    break;
    }
  }

  observer[obsSharedMemRegionInfo]->AssertReady();
}


void 
MainObject::GotVelParam(const ONotifyEvent &event){
  static EventTimeReporter reporter("MainObject::GotVelParam",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  Motion::VelParam vp;

  memcpy(&vp,event.Data(0),sizeof(vp));
  
  MaxDX = vp.max_dx;
  MaxDY = vp.max_dy;
  MaxDA = vp.max_da;

  observer[obsVelParam]->AssertReady();
}

void
MainObject::setCameraParam() {
  static OPrimitiveID fbkID = 0;

  if(fbkID == 0){
    if(OPENR::OpenPrimitive("PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1", &fbkID) != oSUCCESS){
      cout << "Open FbkImageSensor failure." << endl;
    }
  }


  //Parameter:
  //	White Balance:
  //		const longword ocamparamWB_INDOOR_MODE  = 1;
  //		const longword ocamparamWB_OUTDOOR_MODE = 2;
  //		const longword ocamparamWB_FL_MODE      = 3; // Fluorescent Lamp
  //
  //	Gain:
  //		const longword ocamparamGAIN_LOW        = 1;
  //		const longword ocamparamGAIN_MID        = 2;
  //		const longword ocamparamGAIN_HIGH       = 3;
  //
  //	Shutter Speed:
  //		const longword ocamparamSHUTTER_SLOW    = 1;
  //		const longword ocamparamSHUTTER_MID     = 2;
  //		const longword ocamparamSHUTTER_FAST    = 3;

  Dictionary cfg_dict;
  cfg_dict.read(camera_config_filename);
  
  longword wb      = ocamparamWB_FL_MODE;
  longword gain    = ocamparamGAIN_MID;
  longword shutter = ocamparamSHUTTER_MID;
  
  {
    const char *whiteBalanceString = "";
    if (cfg_dict.getValueString("whiteBalance",whiteBalanceString)) {
      if (!strcmp(whiteBalanceString, "indoor")) {
        wb = ocamparamWB_INDOOR_MODE;
      } else if (!strcmp(whiteBalanceString, "outdoor")) {
        wb = ocamparamWB_OUTDOOR_MODE;
      } else if (!strcmp(whiteBalanceString, "flourescent")) {
        wb = ocamparamWB_FL_MODE;
      } else {
        wb = ocamparamWB_FL_MODE;
      }
    }
  }

  {
    const char *gainString = "";
    if (cfg_dict.getValueString("gain",gainString)) {
      if (!strcmp(gainString, "low")) {
        gain = ocamparamGAIN_LOW;
      } else if (!strcmp(gainString, "mid")) {
        gain = ocamparamGAIN_MID;
      } else if (!strcmp(gainString, "high")) {
        gain = ocamparamGAIN_HIGH;
      } else {
        gain = ocamparamGAIN_MID;
      }
    }
  }

  {
    const char *shutterSpeedString = "";
    if (cfg_dict.getValueString("shutterSpeed",shutterSpeedString)) {
      if (!strcmp(shutterSpeedString, "slow")) {
        shutter = ocamparamSHUTTER_SLOW;
      } else if (!strcmp(shutterSpeedString, "mid")) {
        shutter = ocamparamSHUTTER_MID;
      } else if (!strcmp(shutterSpeedString, "fast")) {
        shutter = ocamparamSHUTTER_FAST;
      } else {
        shutter = ocamparamSHUTTER_MID;
      }
    }
  }

  /* White Balance */
  OPrimitiveControl_CameraParam owb(wb);
  if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_WHITE_BALANCE, &owb, sizeof(owb), 0, 0) != oSUCCESS){
    cout << "CAM_SET_WHITE_BALANCE : Failed!" << endl;
  }

  /* Gain */
  OPrimitiveControl_CameraParam ogain(gain);
  if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &ogain, sizeof(ogain), 0, 0) != oSUCCESS){
    cout << "CAM_SET_GAIN : Failed!" << endl;
  }

  /* Shutter Speed */
  OPrimitiveControl_CameraParam oshutter(shutter);
  if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_SHUTTER_SPEED, &oshutter, sizeof(oshutter), 0, 0) != oSUCCESS){
    cout << "CAM_SET_SHUTTER_SPEED : Failed!" << endl;
  }
}

// This is a short adapter which allows us to treat Aperios messages
// from CommMgr like they came in as raw UDP packets.
void MainObject::GotMessage(const ONotifyEvent &event) {

  for (int i = 0; i < event.NumOfData(); i++) {
    NetMsgHeader *header = (NetMsgHeader*)event.Data(i);
    NetReceiveInput((byte *)header, header->totalLength);
  }
  
  observer[obsGotMessage]->AssertReady();
}

void MainObject::NetStatus(const ONotifyEvent &event) {
  for (int i = 0; i < event.NumOfData(); i++) {
    CommMgr::NetStatus *s = (CommMgr::NetStatus*) event.Data(i);
    main->teamMsgMgr.updateNetStatus(*s);
  }
  observer[obsNetStatus]->AssertReady();
}


void
MainObject::NetReceiveInput(byte *data, int Bytes)
{
  static EventTimeReporter reporter("MainObject::NetReceiveInput",SecToTime(5.0),SecToTime(100.0),
				    1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  NetMsgHeader *header;
  StatusMsg *status_msg;
  PackedStatusAndRoleMsg *packed_s_and_r_msg;
  StatusAndRoleMsg status_and_role_msg;
  RoleTokenMsg *role_msg;
  uchar *msg_data;

  // handle data buffer
  header = (NetMsgHeader *)data;

  msg_data = ((uchar *)header) + sizeof(NetMsgHeader);

#if 0
  pprintf(TextOutputStream, "%d bytes from %d to %d type = %d\n",
	  header->totalLength,
	  header->senderID,
	  header->receiverID,
	  header->messageType);
#endif  

  switch(header->messageType) {
      
    case NetMsgHeader::MsgRoleToken:
      
      role_msg = (RoleTokenMsg *)header;
      
      if(role_msg->header.receiverID==main->robotID ||
	 role_msg->header.receiverID==NetMsgHeader::BCast) {
	
	main->teamMsgMgr.receiveRoleMessage(role_msg->header.timestamp,
					    role_msg->header.senderID,
					    role_msg->header.senderTeam,
					    role_msg->data.RoleMessage,
					    role_msg->data.Activation,
					    role_msg->data.Token);
      }
      break;
      
    case NetMsgHeader::MsgStatusAndRole:
      
      // HACK
      packed_s_and_r_msg = (PackedStatusAndRoleMsg *)header;
      packed_s_and_r_msg->unpack(status_and_role_msg);
      //      status_and_role_msg = (StatusAndRoleMsg *)header;
      // /HACK

      main->teamMsgMgr.receiveStatusMsg(status_and_role_msg.header.timestamp,
					status_and_role_msg.header.senderID,
					status_and_role_msg.header.senderTeam,
					&(status_and_role_msg.status_data));
            
      if(status_and_role_msg.role_data.Receiver==main->robotID ||
	 status_and_role_msg.role_data.Receiver==NetMsgHeader::BCast) {
	
	main->teamMsgMgr.receiveRoleMessage(status_and_role_msg.header.timestamp,
					    status_and_role_msg.header.senderID,
					    status_and_role_msg.header.senderTeam,
					    status_and_role_msg.role_data.RoleMessage,
					    status_and_role_msg.role_data.Activation,
					    status_and_role_msg.role_data.Token);
      }
      
      break;
      
    case NetMsgHeader::MsgThresholdSelect:
      {
	int threshold_id;
	threshold_id = (int)(*msg_data);
	pprintf(TextOutputStream, "setting thresholds to %d\n",threshold_id);
	main->getVision()->setThreshold(threshold_id);
      }
      break;
      
    case NetMsgHeader::MsgStatus:
      status_msg = (StatusMsg *)header;

      main->teamMsgMgr.receiveStatusMsg(status_msg->header.timestamp,
                                        status_msg->header.senderID,
					status_msg->header.senderTeam,
                                        &(status_msg->data));
      break;
  }
}

void
MainObject::queueRoleMessage(int receiver, int type, double activation,
			     int token, double other_act) 
{

  role_queue.Receiver = receiver;
  role_queue.RoleMessage = type;
  role_queue.Activation = activation;
  role_queue.Token = token;
  role_queue.OtherActivation = other_act;

  need_to_send_role = true;
}

void
MainObject::queueStatusMessage(StatusMsgEntry *my_data) 
{
  memcpy(&status_queue, my_data, sizeof(StatusMsgEntry));
  need_to_send_status = true;
}

void MainObject::SendStatusAndRoleMsgs(int team_id) 
{
  static EventTimeReporter reporter("MainObject::SendStatusAndRoleMsgs",SecToTime(5.0),SecToTime(100.0),
				    1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  if(need_to_send_status && !need_to_send_role) {
    StatusMsg msg;
    
    msg.buildHeader(main->robotID, team_id);
    
    memcpy(&msg.data, &status_queue,
	   sizeof(StatusMsgEntry));
  
    subject[sbjBCastMessage]->SetData(&msg, sizeof(msg));
    subject[sbjBCastMessage]->NotifyObservers();
    
    need_to_send_status = false;
  } else if(need_to_send_role && !need_to_send_status) {
    RoleTokenMsg msg;
    
    msg.buildHeader(main->robotID, team_id, role_queue.Receiver);
    
    memcpy(&msg.data, &role_queue, sizeof(RoleTokenEntry));
    
    // should be sbjSendMessage, but BCast for debugging
    subject[sbjBCastMessage]->SetData(&msg, sizeof(msg));
    subject[sbjBCastMessage]->NotifyObservers();

    need_to_send_role = false;
  } else if(need_to_send_status && need_to_send_role) {

    StatusAndRoleMsg msg;
    
    msg.buildHeader(main->robotID, team_id);
    
    memcpy(&msg.status_data, &status_queue, sizeof(StatusMsgEntry));
    memcpy(&msg.role_data, &role_queue, sizeof(RoleTokenEntry));
    
    // HACK
    PackedStatusAndRoleMsg packed_msg;
    packed_msg.pack(msg);
    // /HACK

    subject[sbjBCastMessage]->SetData(&packed_msg, sizeof(packed_msg));
    subject[sbjBCastMessage]->NotifyObservers();

    need_to_send_status = false;
    need_to_send_role = false;
  }
}

void
MainObject::GameMgrInput(const ONotifyEvent &event)
{
  RoboCupGameControlData *msg;
  
  for(int i=0; i<event.NumOfData(); i++) {
    msg = (RoboCupGameControlData *)event.Data(i);
    
    
    main->robot_state = msg->state;
    
    if(msg->kickoff == ROBOCUP_KICKOFF_OWN)
      main->have_kickoff = true;
    else if(msg->kickoff == ROBOCUP_KICKOFF_OPPONENT)
      main->have_kickoff = false;
    
    if(msg->teamColor==ROBOCUP_TEAMCOLOR_RED) {
      main->world_model.teamColor = TEAM_COLOR_RED;
      main->world_model.goalColor = DEFEND_YELLOW_GOAL;
    } else if(msg->teamColor==ROBOCUP_TEAMCOLOR_BLUE) {
      main->world_model.teamColor = TEAM_COLOR_BLUE;
      main->world_model.goalColor = DEFEND_CYAN_GOAL;
    }
  }

  observer[obsGameMgrInput]->AssertReady();
}