Platform/Aperios1.3.2/MessageQueueBase.cpp

Go to the documentation of this file.

/**
* @file Platform/Aperios1.3.2/MessageQueueBase.cpp
* 
* Implementation of class MessageQueueBase for Aperios.
*
* @author Martin Lötzsch
*/

#include "MessageQueueBase.h"
#include "GTAssert.h"
#include "SystemCall.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Debugging/Stopwatch.h"
#include <stdlib.h>
#include <string.h>

MessageQueueBase::MessageQueueBase()
: buf(0), queueSize(0)
{
  clear();
}

MessageQueueBase::~MessageQueueBase()
{
  if (buf != 0) delete[] buf;
}

void MessageQueueBase::setSize(int size)
{
  ASSERT(buf == 0);
  buf = (char*)malloc(size);
  queueSize = size;
  ASSERT(buf);
}

void MessageQueueBase::clear()
{
  nextMessagePosition = 0;
  numOfMessages = 0;
  nextMessageSize = 0;
  writingOfLastMessageFailed = false;
  selectedMessageForReadingPosition = 0;
  readPosition = 0;
  lastMessage = 0;
}

int MessageQueueBase::getNumberOfMessages() const
{
  return numOfMessages;
}

void MessageQueueBase::write(const void* p,int size)
{
  if (nextMessagePosition + 12 + nextMessageSize + size >= queueSize)
  {
    writingOfLastMessageFailed = true;
  }
  else
  {
    memcpy(buf + nextMessagePosition + 12 + nextMessageSize, p, size);
    nextMessageSize += size;
  }
}

void MessageQueueBase::finishMessage(MessageID id, unsigned long timeStamp,
    Player::teamColor teamColor,Player::playerNumber playerNumber,
    bool messageWasSentFromAPhysicalRobot)
{
  if (!writingOfLastMessageFailed)
  {
    ASSERT(nextMessageSize > 0);
    memcpy(buf + nextMessagePosition, &nextMessageSize, 4); // write the size of the message
    memcpy(buf + nextMessagePosition + 4, (char*)&id, 1); // write the id of the message
    memcpy(buf + nextMessagePosition + 5, &timeStamp, 4); // write the timeStamp
    memcpy(buf + nextMessagePosition + 9, (char*)&teamColor, 1); // write the team color 
    memcpy(buf + nextMessagePosition + 10,(char*)&playerNumber, 1); // write the player number
    memcpy(buf + nextMessagePosition + 11,(char*)&messageWasSentFromAPhysicalRobot, 1); 
    numOfMessages++;
    nextMessagePosition += nextMessageSize + 12;
  }
  nextMessageSize = 0;
  writingOfLastMessageFailed = false;
}

void MessageQueueBase::removeRepetitions()
{

  int messagesPerType[numOfMessageIDs];
#ifdef NEWDEBUGGING
  unsigned long lastProcessFinishedFrameNumber = 0;
#else
  int fieldDrawings[Drawings::numberOfFieldDrawings];
  int imageDrawings[Drawings::numberOfImageDrawings];
  memset(fieldDrawings,0,sizeof(fieldDrawings));
  memset(imageDrawings,0,sizeof(imageDrawings));
#endif
  int stopWatches[Stopwatch::numberOfStopwatchEventIDs];
  memset(messagesPerType,0,sizeof(messagesPerType));
  memset(stopWatches,0,sizeof(stopWatches));
  selectedMessageForReadingPosition=0;
  int i;
  for (i=0;i<numOfMessages;i++)
  {
#ifdef NEWDEBUGGING
    if( getMessageID() == idProcessFinished)
    {
      lastProcessFinishedFrameNumber = getTimeStamp();
      messagesPerType[getMessageID()]++;
    }
#else
    if(getMessageID() == idDebugDrawingFinished)
    {
      if(Drawings::TypeOfDrawing(getData()[1]) == Drawings::drawingOnField)
        fieldDrawings[int(getData()[0])]++;
      else
        imageDrawings[int(getData()[0])]++;
    }
#endif
    else if(getMessageID() == idStopwatch)
      stopWatches[int(getData()[0])]++;
    else
      messagesPerType[getMessageID()]++;
    selectedMessageForReadingPosition += getMessageSize() + 12;
  }
  selectedMessageForReadingPosition=0;
  nextMessagePosition = 0;
  int numOfDeleted=0;
  for (i=0; i < numOfMessages; i++)
  {
    int mlength = getMessageSize() + 12;
    bool copy;
    switch (getMessageID())
    {
     case idDebugDrawing2:
#ifdef NEWDEBUGGING
      copy = ( getTimeStamp() == lastProcessFinishedFrameNumber);
      break;
#else
      if(Drawings::TypeOfDrawing(getData()[2]) == Drawings::drawingOnField)
        copy = fieldDrawings[int(getData()[1])] <= 1;
      else
        copy = imageDrawings[int(getData()[1])] <= 1;
      break;
     case idDebugDrawingFinished:
      if(Drawings::TypeOfDrawing(getData()[1]) == Drawings::drawingOnField)
        copy = --fieldDrawings[int(getData()[0])] == 0;
      else 
        copy = --imageDrawings[int(getData()[0])] == 0;
      break;
#endif
    case idStopwatch:
      copy = --stopWatches[int(getData()[0])] == 0;
      break;
    case idText:
      //allow up to 20 idTexts in realtime mode, because several of that kind may be send simultaneously:
      copy = --messagesPerType[getMessageID()] <= 20;
      break;
    case idDebugRequest:
    case idDebugResponse:
    case idStreamSpecification:
    case idDebugDataResponse:
    case idDrawingManager:
    case idModuleSolutionTable:
      copy = true;
      break;
    default:
      copy = --messagesPerType[getMessageID()] == 0;
    }
    if(copy)
    {
      //this message is important, it is the last of its type, it shall be copied
      for (int l=0;l<mlength;l++)
      {
        buf[nextMessagePosition++] = buf[selectedMessageForReadingPosition++];
      }
    }
    else
    {
      numOfDeleted++;
      selectedMessageForReadingPosition += mlength;
    }
  }
  readPosition=0;
  numOfMessages -= numOfDeleted;
  selectedMessageForReadingPosition = 0;
  lastMessage = 0;
}

void MessageQueueBase::setSelectedMessageForReading(int message)
{
  ASSERT(message >= 0);
  ASSERT(message < numOfMessages);

  int m = message;
  if(m >= lastMessage)
  {
    ASSERT(lastMessage < numOfMessages);
    m -= lastMessage;
  }
  else
    selectedMessageForReadingPosition = 0;
  for (int i=0; i < m; i++)
  {
    selectedMessageForReadingPosition += getMessageSize() + 12;
  }
  readPosition = 0;
  lastMessage = message;
}

void MessageQueueBase::read(void* p,int size)
{
  ASSERT(readPosition + size <= getMessageSize());
  memcpy(p,buf + selectedMessageForReadingPosition + 12 + readPosition, size);
  readPosition += size;
} 

const char* MessageQueueBase::getData() const
{
  return buf + selectedMessageForReadingPosition + 12;
}

int MessageQueueBase::getMessageSize() const
{
  int size;
  memcpy(&size, buf + selectedMessageForReadingPosition, 4);
  return size;
}


void MessageQueueBase::resetReadPosition()
{
  readPosition = 0;
}

unsigned long MessageQueueBase::getTimeStamp() const
{
  unsigned long timeStamp;
  memcpy(&timeStamp, buf + selectedMessageForReadingPosition + 5, 4);
  return timeStamp;
}

unsigned long MessageQueueBase::getTimeStamp(int message)
{
  setSelectedMessageForReading(message);
  return getTimeStamp();
}

MessageID MessageQueueBase::getMessageID() const
{
  char id;
  memcpy(&id, buf + selectedMessageForReadingPosition + 4, 1);

  int i = id;
  i &= 0xff;
  return (MessageID)i;
}

Player::teamColor MessageQueueBase::getTeamColor() const
{
  char color;
  memcpy(&color, buf + selectedMessageForReadingPosition + 9, 1);
  return (Player::teamColor)color;
}

Player::playerNumber MessageQueueBase::getPlayerNumber() const
{
  char number;
  memcpy(&number, buf + selectedMessageForReadingPosition + 10, 1);
  return (Player::playerNumber)number;
}

bool MessageQueueBase::getMessageWasSentFromAPhysicalRobot() const
{
  char b;
  memcpy(&b, buf + selectedMessageForReadingPosition + 11, 1);
  return (bool)b;
}


bool MessageQueueBase::eof() const
{
  return readPosition == getMessageSize();
}

---