Проблемы с кольцевым буфером в общей памяти

Пишу программу производители - потребители на С использованием общей памяти POSIX. Столкнулся с проблемой выхода за границы. Помогите найти ошибку в логике. или может стоит сделать как-нибудь лучше? https://github.com/fozboom/UNIX-programming/tree/main/lab4_shared_memory

Вот участок кода для добавления и извлечения элементов из очереди, он ломается

void addMessageToQueue(CircularQueue *queue, Message *message) {
  if (isQueueFull(queue)) {
    printf("Queue is full\n");
    return;
  }
  if (queue->tailPosition >= (char *)queue->messages + SHM_SIZE) {
    printf("Queue is full\n");
    queue->tailPosition = (char *)queue->messages;
  }
  memcpy(queue->tailPosition, message, sizeof(Message));
  queue->tailPosition += sizeof(Message);

  if (queue->tailPosition >= (char *)queue->messages + SHM_SIZE) {
    printf("Queue is full\n");
    queue->tailPosition = (char *)queue->messages;
  }
  memcpy(queue->tailPosition, message->data, message->size);
  queue->tailPosition += message->size;

  queue->currentSize++;
  queue->countAddedMessages++;
}

Message *removeMessageFromQueue(CircularQueue *queue) {
  if (isQueueEmpty(queue)) {
    printf("Queue is empty\n");
    return NULL;
  }

  Message *message = (Message *)queue->headPosition;

  Message *messageCopy = (Message *)malloc(sizeof(Message));
  messageCopy->type = message->type;
  messageCopy->hash = message->hash;
  messageCopy->size = message->size;
  messageCopy->data = (char *)malloc(message->size);
  if (queue->headPosition >= (char *)queue->messages + SHM_SIZE) {
    queue->headPosition = (char *)queue->messages;
  }
  memcpy(messageCopy->data, queue->headPosition + sizeof(Message),
         message->size);

  queue->headPosition += sizeof(Message) + message->size;

  queue->currentSize--;
  queue->countRemovedMessages++;

  return messageCopy;
}

И вот сама очередь сообщений

typedef struct {
  Message *messages;
  char *headPosition;
  char *tailPosition;
  int maxCapacity;
  int countAddedMessages;
  int countRemovedMessages;
  int currentSize;
} CircularQueue;

typedef struct {
  uint8_t type;
  uint16_t hash;
  uint8_t size;
  char *data;
} Message;