a simple implementation of Priority Queue using std::vector and std::queue

bench.cpp

#include <iostream>
#include <vector>

#include <benchmark/benchmark.h>

#include "squeue.hpp"

#define COUNT_I 1000
#define COUNT_J 10

auto repeated_queue() {
  PriorityQueue<size_t, size_t> q{};

  for (size_t i = 0; i < COUNT_I; i++)
    for (size_t j = 0; j < COUNT_J; j++)
      q.push(i, j);

  return q;
}

auto unique_queue() {
  PriorityQueue<size_t, size_t> q{};

  for (size_t i = 1; i < COUNT_I + 1; i++)
    for (size_t j = 1; j < COUNT_J + 1; j++)
      q.push(i * j, i * j);

  return q;
}

auto rq1 = repeated_queue();
auto rq2 = rq1;

auto uq1 = unique_queue();
auto uq2 = uq1;

void repeated_inserts(benchmark::State& state) {
  PriorityQueue<size_t, size_t> q;
  benchmark::DoNotOptimize(&q);

  for (auto _ : state) {
    q = repeated_queue();
  }
}
BENCHMARK(repeated_inserts);

void unique_inserts(benchmark::State& state) {
  PriorityQueue<size_t, size_t> q;
  benchmark::DoNotOptimize(&q);

  for (auto _ : state) {
    q = unique_queue();
  }
}
BENCHMARK(unique_inserts);

void top_priority_unique_pulls(benchmark::State& state) {
  PriorityQueue<size_t, size_t> q = std::move(uq1);
  benchmark::DoNotOptimize(&q);

  for (auto _ : state) {
    size_t s = 0;
    benchmark::DoNotOptimize(&s);

    while (!q.empty())
      s += q.pop_top().value();
  }
}
BENCHMARK(top_priority_unique_pulls);

void low_priority_unique_pulls(benchmark::State& state) {
  PriorityQueue<size_t, size_t> q = std::move(uq2);
  benchmark::DoNotOptimize(&q);

  for (auto _ : state) {
    size_t s = 0;
    benchmark::DoNotOptimize(&s);

    while (!q.empty())
      s += q.pop_bottom().value();
  }
}
BENCHMARK(low_priority_unique_pulls);

void top_priority_repeated_pulls(benchmark::State& state) {
  PriorityQueue<size_t, size_t> q = std::move(rq1);
  benchmark::DoNotOptimize(&q);

  for (auto _ : state) {
    size_t s = 0;
    benchmark::DoNotOptimize(&s);

    while (!q.empty())
      s += q.pop_top().value();
  }
}
BENCHMARK(top_priority_repeated_pulls);

void low_priority_repeated_pulls(benchmark::State& state) {
  PriorityQueue<size_t, size_t> q = std::move(rq2);
  benchmark::DoNotOptimize(&q);

  size_t s = 0;
  benchmark::DoNotOptimize(&s);

  for (auto _ : state) {
    while (!q.empty())
      if (q.pop_bottom())
        s++;
  }
}
BENCHMARK(low_priority_repeated_pulls);

void copy_constructor(benchmark::State& state) {
  PriorityQueue<size_t, size_t> q = unique_queue();
  benchmark::DoNotOptimize(&q);

  for (auto _ : state) {
    PriorityQueue q2 = q;
    benchmark::DoNotOptimize(&q2);
  }
}
BENCHMARK(copy_constructor);

BENCHMARK_MAIN();

pqueue.hpp

#ifndef SQUEUE_H_
#define SQUEUE_H_

#include <iostream>

#include <queue>
#include <vector>
#include <optional>
#include <algorithm>
#include <initializer_list>

template<class T, class PT = int>
struct PriorityNode final {
  PT priority;
  std::queue<T> q;

  PriorityNode(PT priority, T item)
    : priority(priority), q(std::queue<T>{})
  {
    q.push(item);
  }
};

template<class T, class PT = int, size_t SZ = 64>
class PriorityQueue final {
  std::vector<PriorityNode<T, PT>> lst_;
  size_t first_;
  size_t last_;

 public:
  PriorityQueue() : lst_({}), first_(0), last_(0) {}

  PriorityQueue(std::initializer_list<std::pair<PT, T>> lst)
    : lst_({}), first_(0), last_(0)
  {
    // TODO sort in descending order

    for (auto [priority, item] : lst)
      _push(priority, item);
  }

  bool empty() const { return first_ <= last_; }
  void reset() { first_ = last_ = 0; lst_ = {}; }

  std::optional<T> top() const
  {
    return empty()
      ? std::nullopt
      : std::optional(lst_[first_].q.front());
  }

  std::optional<T> bottom() const
  {
    return empty()
      ? std::nullopt
      : std::optional(lst_[last_-1].q.front());
  }

  std::optional<T> pop_top();
  std::optional<T> pop_bottom();

  void push(PT priority, T item) { _push(priority, item); }

 private:
  void _push(PT priority, T item);
  void _move_shift_backward()
  {
    std::move(
      std::begin(lst_) + first_,
      std::end(lst_) + last_,
      std::begin(lst_)
    );

    last_ = last_ - first_;
    first_ = 0;
  }
};

template<class T, class PT, size_t SZ>
std::optional<T> PriorityQueue<T, PT, SZ>::pop_top()
{
  if (empty()) {
    if (first_ != 0) reset();

    return std::nullopt;
  }

  auto& q = lst_[first_].q;

  T item = q.front();
  q.pop();

  if (q.empty()) {
    size_t idx = ++first_;
    for (
      /* void */;
      idx < last_ && !lst_[idx].q.empty();
      ++idx
    );

    first_ = idx;

    if (first_ > SZ)
      _move_shift_backward();
  }

  return item;
}

template<class T, class PT, size_t SZ>
std::optional<T> PriorityQueue<T, PT, SZ>::pop_bottom()
{
  if (empty()) {
    if (first_ != 0) reset();

    return std::nullopt;
  }

  auto& q = lst_[last_-1].q;

  T item = q.front();
  q.pop();

  if (q.empty()) {
    lst_.pop_back();
    --last_;
  }

  return item;
}

// TODO use idx as a parameter with 0 as default value
//      and return used idx as a helper for mass internal pushes
template<class T, class PT, size_t SZ>
void PriorityQueue<T, PT, SZ>::_push(PT priority, T item)
{
  size_t idx;

  auto it = std::begin(lst_);
  bool is_empty = (idx < last_) ? (*it).q.empty() : false;
  for (
    idx = 0;
    idx < last_ && (is_empty || priority < (*it).priority);
    ++idx, ++it, is_empty = (*it).q.empty()
  );

  // reuse empty queue node by reassigning priority
  if (is_empty)
    (*it).priority = priority;

  if (idx >= last_) {
    lst_.push_back(PriorityNode<T, PT>(priority, item));

    ++last_;
  }
  else if (priority == (*it).priority) {
    lst_[idx].q.push(item);
  }
  else { // priority > lst_[idx].priority
    lst_.insert(it, PriorityNode<T, PT>(priority, item));

    if (idx < first_)
      first_ = idx;
    else
      ++last_;
  }

  // TODO add reserve SZ more blocks when (last_ % SZ) == 0 
  //      and (lst_.size() - last_) < (SZ / 4)

  // TODO add forehead reservation to avoid
  //      sequential inserts at first_ index
}

#endif  // SQUEUE_H_