[wpiutil] Add custom priority queue implementation (#2770)

This returns the removed object from pop() to avoid copies.

wpiutil/src/main/native/include/wpi/PriorityQueue.h

@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef WPIUTIL_WPI_PRIORITYQUEUE_H_
-#define WPIUTIL_WPI_PRIORITYQUEUE_H_
-
-#include <algorithm>
-#include <functional>
-#include <queue>
-#include <vector>
-
-namespace wpi {
-
-/**
- * This class adds a method for removing all elements from the priority queue
- * matching the given value.
- */
-template <class T, class Container = std::vector<T>,
-          class Compare = std::less<typename Container::value_type>>
-class PriorityQueue : public std::priority_queue<T, Container, Compare> {
- public:
-  bool remove(const T& value) {
-    auto it = std::find(this->c.begin(), this->c.end(), value);
-    if (it != this->c.end()) {
-      this->c.erase(it);
-      std::make_heap(this->c.begin(), this->c.end(), this->comp);
-      return true;
-    } else {
-      return false;
-    }
-  }
-};
-
-}  // namespace wpi
-
-#endif  // WPIUTIL_WPI_PRIORITYQUEUE_H_

wpiutil/src/main/native/include/wpi/priority_queue.h

@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#ifndef WPIUTIL_WPI_PRIORITY_QUEUE_H_
+#define WPIUTIL_WPI_PRIORITY_QUEUE_H_
+
+#include <algorithm>
+#include <functional>
+#include <utility>
+#include <vector>
+
+namespace wpi {
+
+/**
+ * This class is the same as std::priority_queue with two changes:
+ *
+ * 1. Adds a remove() function for removing all elements from the priority queue
+ *    that match the given value.
+ * 2. Replaces "void pop()" with "T pop()" so the element can be moved from the
+ *    queue directly instead of copied from top().
+ */
+template <typename T, typename Sequence = std::vector<T>,
+          typename Compare = std::less<typename Sequence::value_type>>
+class priority_queue {
+ public:
+  static_assert(std::is_same_v<T, typename Sequence::value_type>,
+                "value_type must be the same as the underlying container");
+
+  using value_type = typename Sequence::value_type;
+  using reference = typename Sequence::reference;
+  using const_reference = typename Sequence::const_reference;
+  using size_type = typename Sequence::size_type;
+  using container_type = Sequence;
+  using value_compare = Compare;
+
+  template <typename Seq = Sequence,
+            typename Requires = typename std::enable_if_t<
+                std::is_default_constructible<Compare>{} &&
+                std::is_default_constructible<Seq>{}>>
+  priority_queue() {}
+
+  priority_queue(const Compare& comp, const Sequence& c) : c(c), comp(comp) {
+    std::make_heap(c.begin(), c.end(), comp);
+  }
+
+  explicit priority_queue(const Compare& comp, Sequence&& c = Sequence{})
+      : c(std::move(c)), comp(comp) {
+    std::make_heap(c.begin(), c.end(), comp);
+  }
+
+  template <typename InputIterator>
+  priority_queue(InputIterator first, InputIterator last, const Compare& comp,
+                 const Sequence& c)
+      : c(c), comp(comp) {
+    c.insert(c.end(), first, last);
+    std::make_heap(c.begin(), c.end(), comp);
+  }
+
+  template <typename InputIterator>
+  priority_queue(InputIterator first, InputIterator last,
+                 const Compare& comp = Compare{}, Sequence&& c = Sequence{})
+      : c(std::move(c)), comp(comp) {
+    c.insert(c.end(), first, last);
+    std::make_heap(c.begin(), c.end(), comp);
+  }
+
+  [[nodiscard]] bool empty() const { return c.empty(); }
+
+  size_type size() const { return c.size(); }
+
+  const_reference top() const { return c.front(); }
+
+  void push(const value_type& value) {
+    c.push_back(value);
+    std::push_heap(c.begin(), c.end(), comp);
+  }
+
+  void push(value_type&& value) {
+    c.push_back(std::move(value));
+    std::push_heap(c.begin(), c.end(), comp);
+  }
+
+  template <typename... Args>
+  void emplace(Args&&... args) {
+    c.emplace_back(std::forward<Args>(args)...);
+    std::push_heap(c.begin(), c.end(), comp);
+  }
+
+  T pop() {
+    std::pop_heap(c.begin(), c.end(), comp);
+    auto ret = std::move(c.back());
+    c.pop_back();
+    return ret;
+  }
+
+  bool remove(const T& value) {
+    auto it = std::find(c.begin(), c.end(), value);
+    if (it != this->c.end()) {
+      c.erase(it);
+      std::make_heap(c.begin(), c.end(), comp);
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+ protected:
+  Sequence c;
+  Compare comp;
+};
+
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_PRIORITY_QUEUE_H_