cpp-rotor-light/include/rotor-light/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();

  ItemGuard &operator=(ItemGuard &&);
  inline operator bool() { return message; }
  inline Message &operator*() { return *message; }

  void reset();

private:
  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");
    if (free_index == occupied_index) {
      return nullptr;
    }
    assert(free_index != undef);
    auto ptr = buff_ptr + item_size * free_index;
    if (occupied_index == undef) {
      occupied_index = free_index;
    }
    free_index = (free_index + 1) % items_count;
    return new (ptr) MessageType(std::forward<Args>(args)...);
  }

  void release(Message *);

protected:
  static constexpr auto undef = std::numeric_limits<uint8_t>::max();

  friend struct Message;
  void post_constructor(char *buff);

  char *buff_ptr;
  index_t item_size;
  index_t items_count;
  index_t free_index;
  volatile index_t 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");
    assert((sizeof(MessageType) <= item_size) &&
           "no storage for the message, increase ItemSize");
    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