#pragma once

#include <Dispatcher.h>

template <uint8_t N>
class FixedPacketQueue {
public:
  FixedPacketQueue() : _num(0) {
    memset(_table, 0, sizeof(_table));
    memset(_pri_table, 0, sizeof(_pri_table));
    memset(_schedule_table, 0, sizeof(_schedule_table));
  }

  mesh::Packet* get(uint32_t now) {
    uint8_t min_pri = 0xFF;
    int best_idx = -1;
    for (uint8_t j = 0; j < _num; ++j) {
      if ((int32_t)(_schedule_table[j] - now) > 0) {
        continue;
      }
      if (_pri_table[j] < min_pri) {
        min_pri = _pri_table[j];
        best_idx = j;
      }
    }
    return removeByIdx(best_idx);
  }

  bool add(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) {
    if (!packet || _num >= N) {
      return false;
    }
    _table[_num] = packet;
    _pri_table[_num] = priority;
    _schedule_table[_num] = scheduled_for;
    ++_num;
    return true;
  }

  int count() const {
    return _num;
  }

  int countBefore(uint32_t now) const {
    if (now == 0xFFFFFFFFUL) {
      return _num;
    }

    int n = 0;
    for (uint8_t j = 0; j < _num; ++j) {
      if ((int32_t)(_schedule_table[j] - now) <= 0) {
        ++n;
      }
    }
    return n;
  }

  mesh::Packet* itemAt(int i) const {
    return (i >= 0 && i < _num) ? _table[i] : NULL;
  }

  mesh::Packet* removeByIdx(int i) {
    if (i < 0 || i >= _num) {
      return NULL;
    }

    mesh::Packet* item = _table[i];
    --_num;
    while (i < _num) {
      _table[i] = _table[i + 1];
      _pri_table[i] = _pri_table[i + 1];
      _schedule_table[i] = _schedule_table[i + 1];
      ++i;
    }
    return item;
  }

private:
  mesh::Packet* _table[N];
  uint8_t _pri_table[N];
  uint32_t _schedule_table[N];
  uint8_t _num;
};

template <uint8_t N>
class FixedPoolPacketManager : public mesh::PacketManager {
public:
  FixedPoolPacketManager() {
    for (uint8_t i = 0; i < N; ++i) {
      _unused.add(&_packets[i], 0, 0);
    }
  }

  mesh::Packet* allocNew() override {
    return _unused.removeByIdx(0);
  }

  void free(mesh::Packet* packet) override {
    _unused.add(packet, 0, 0);
  }

  void queueOutbound(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) override {
    if (!_send_queue.add(packet, priority, scheduled_for)) {
      MESH_DEBUG_PRINTLN("queueOutbound: send queue full, dropping packet");
      free(packet);
    }
  }

  mesh::Packet* getNextOutbound(uint32_t now) override {
    return _send_queue.get(now);
  }

  int getOutboundCount(uint32_t now) const override {
    return _send_queue.countBefore(now);
  }

  int getOutboundTotal() const override {
    return _send_queue.count();
  }

  int getFreeCount() const override {
    return _unused.count();
  }

  mesh::Packet* getOutboundByIdx(int i) override {
    return _send_queue.itemAt(i);
  }

  mesh::Packet* removeOutboundByIdx(int i) override {
    return _send_queue.removeByIdx(i);
  }

  void queueInbound(mesh::Packet* packet, uint32_t scheduled_for) override {
    if (!_rx_queue.add(packet, 0, scheduled_for)) {
      MESH_DEBUG_PRINTLN("queueInbound: rx queue full, dropping packet");
      free(packet);
    }
  }

  mesh::Packet* getNextInbound(uint32_t now) override {
    return _rx_queue.get(now);
  }

private:
  mesh::Packet _packets[N];
  FixedPacketQueue<N> _unused;
  FixedPacketQueue<N> _send_queue;
  FixedPacketQueue<N> _rx_queue;
};