queue.hpp

// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: 2022 Ivan Baidakou
 
#pragma once
 
#include "message.hpp"
#include <cassert>
#include <cstddef>
#include <limits>
#include <tuple>
#include <type_traits>
#include <utility>
 
namespace rotor_light {
 
struct ItemQueueBase;
 
struct ItemGuard {
  ItemGuard() : message{nullptr} {}
 
  ItemGuard(Message *message_, ItemQueueBase *item_queue_)
      : message{message_}, item_queue{item_queue_} {}
  ItemGuard(const ItemGuard &) = delete;
  ItemGuard(ItemGuard &&) = delete;
 
  ItemGuard &operator=(const ItemGuard &) = default;
 
  inline operator bool() { return message; }
 
  inline Message &operator*() { return *message; }
 
  Message *message;
 
  ItemQueueBase *item_queue;
};
 
struct ItemQueueBase {
  ItemQueueBase(size_t item_size, size_t items_count);
  ItemQueueBase(const ItemQueueBase &) = delete;
  ItemQueueBase(ItemQueueBase &&) = delete;
 
  Message *next();
 
  template <typename MessageType, typename... Args>
  MessageType *put(Args... args) {
    static_assert(std::is_base_of<Message, MessageType>::value,
                  "type should be inherited from Message");
    assert((sizeof(MessageType) <= item_size) &&
           "no storage for the message, increase ItemSize");
    if (items_size == items_count) {
      return nullptr;
    }
    auto ptr = buff_ptr + item_size * free_index;
    if (free_index == items_size - 1) {
      free_index = 0;
    } else {
      ++free_index;
    }
    auto message = new (ptr) MessageType(std::forward<Args>(args)...);
    message->type = MessageType::type_id;
    ++items_count;
    return message;
  }
 
  void release();
 
protected:
  friend struct Message;
  void post_constructor(char *buff);
 
private:
  char *buff_ptr;
  Index item_size;
  Index items_size;
  volatile Index items_count;
  volatile Index free_index;
  volatile Index occupied_index;
};
 
template <typename Storage, size_t ItemsCount>
struct ItemQueue : ItemQueueBase {
  static_assert(Storage::item_size > 0, "queue size have to be positive");
 
  ItemQueue() : ItemQueueBase(Storage::item_size, ItemsCount) {
    post_constructor(reinterpret_cast<char *>(&storage));
  }
 
private:
  using Item = typename Storage::Item;
  Item storage[ItemsCount];
};
 
struct QueueBase {
  QueueBase(ItemQueueBase **queues, size_t queue_count);
  QueueBase(const QueueBase &) = delete;
  QueueBase(QueueBase &&) = delete;
 
  template <typename MessageType, typename... Args>
  bool put(size_t queue_index, Args... args) {
    assert((queue_index < queue_count) && "valid queue/priority");
    return queues[queue_index]->put<MessageType>(std::forward<Args>(args)...);
  }
 
  ItemGuard next();
 
private:
  ItemQueueBase **queues;
  uint8_t queue_count;
};
 
template <typename Storage, size_t... Counts> struct Queue : QueueBase {
  static_assert(Storage::item_size > sizeof(Message),
                "not enough storage for a message");
  using Queues = std::tuple<ItemQueue<Storage, Counts>...>;
 
  static constexpr size_t Size = std::tuple_size<Queues>::value;
  static_assert(Size > 0, "there should be at least one queue");
 
  Queue() : QueueBase(queues, Size) { fill_queue<0>(); }
 
private:
  template <size_t Index> void fill_queue() {
    auto &queue = std::get<Index>(queues_storage);
    queues[Index] = &queue;
    if constexpr (Index + 1 < Size) {
      fill_queue<Index + 1>();
    }
  }
 
  Queues queues_storage;
  ItemQueueBase *queues[Size];
};
 
} // namespace rotor_light