Fix priority queue bug

Author: endurodave

README.md

@@ -94,7 +94,7 @@ When the timed wait expires with an empty queue, the loop simply continues back
 
 **3. Dequeue and back pressure release**
 
-Messages are dequeued with `m_queue.top()` / `m_queue.pop()`, which returns the highest-priority waiting message (`HIGH` before `NORMAL` before `LOW`). After removing a message, `m_cvNotFull` is signalled to wake any producer that blocked in `PostMsg()` because the queue was at its `MAX_QUEUE_SIZE` limit.
+Messages are dequeued from the first non-empty priority queue, checked in order `HIGH`, `NORMAL`, `LOW`. Each queue is a `std::deque` drained front-to-back, so messages posted at the same priority are always processed in the order they were posted. After removing a message, `m_cvNotFull` is signalled to wake any producer that blocked in `PostMsg()` because the queue was at its `MAX_QUEUE_SIZE` limit.
 
 ```cpp
 void Thread::Process()
@@ -111,34 +111,52 @@ void Thread::Process()
         {
             unique_lock<mutex> lk(m_mutex);
 
+            auto empty = [this]() {
+                return m_highQueue.empty() && m_normalQueue.empty() && m_lowQueue.empty();
+            };
+
             if (m_watchdogTimeout.load() > steady_clock::duration::zero())
             {
                 // Timed wait: wake periodically to keep m_lastAliveTime
                 // current even when the queue is empty.
                 auto heartbeat = m_watchdogTimeout.load() / 4;
-                m_cv.wait_for(lk, heartbeat, [this]() {
-                    return !m_queue.empty() || m_exit.load();
+                m_cv.wait_for(lk, heartbeat, [this, &empty]() {
+                    return !empty() || m_exit.load();
                 });
             }
             else
             {
                 // No watchdog: block until a message arrives or exit is set.
-                m_cv.wait(lk, [this]() {
-                    return !m_queue.empty() || m_exit.load();
+                m_cv.wait(lk, [this, &empty]() {
+                    return !empty() || m_exit.load();
                 });
             }
 
-            if (m_queue.empty())
+            if (empty())
             {
                 // Either the heartbeat fired (loop back, refresh timestamp)
                 // or ExitThread() was called with nothing left (exit).
                 if (m_exit.load()) return;
                 continue;
             }
 
-            // Dequeue highest-priority message (HIGH > NORMAL > LOW).
-            msg = m_queue.top();
-            m_queue.pop();
+            // Dequeue from the highest-priority non-empty queue
+            // (HIGH > NORMAL > LOW). Each queue is FIFO.
+            if (!m_highQueue.empty())
+            {
+                msg = m_highQueue.front();
+                m_highQueue.pop_front();
+            }
+            else if (!m_normalQueue.empty())
+            {
+                msg = m_normalQueue.front();
+                m_normalQueue.pop_front();
+            }
+            else
+            {
+                msg = m_lowQueue.front();
+                m_lowQueue.pop_front();
+            }
 
             // Wake a producer blocked on a full queue.
             if (MAX_QUEUE_SIZE > 0)
@@ -173,21 +191,30 @@ void Thread::PostMsg(std::shared_ptr<UserData> data, Priority priority)
 
     unique_lock<mutex> lk(m_mutex);
 
-    if (MAX_QUEUE_SIZE > 0 && m_queue.size() >= MAX_QUEUE_SIZE)
+    auto totalSize = [this]() {
+        return m_highQueue.size() + m_normalQueue.size() + m_lowQueue.size();
+    };
+
+    if (MAX_QUEUE_SIZE > 0 && totalSize() >= MAX_QUEUE_SIZE)
     {
         if (FULL_POLICY == FullPolicy::DROP)
             return;  // silently discard — caller is not stalled
 
-        m_cvNotFull.wait(lk, [this]() {
-            return m_queue.size() < MAX_QUEUE_SIZE || m_exit.load();
+        m_cvNotFull.wait(lk, [this, &totalSize]() {
+            return totalSize() < MAX_QUEUE_SIZE || m_exit.load();
         });
     }
 
     if (m_exit.load())
         return;
 
     auto threadMsg = make_shared<ThreadMsg>(MSG_POST_USER_DATA, data, priority);
-    m_queue.push(threadMsg);
+    switch (priority)
+    {
+        case Priority::HIGH:   m_highQueue.push_back(threadMsg);   break;
+        case Priority::NORMAL: m_normalQueue.push_back(threadMsg); break;
+        case Priority::LOW:    m_lowQueue.push_back(threadMsg);    break;
+    }
     m_cv.notify_one();
 }
 ```
@@ -196,25 +223,19 @@ void Thread::PostMsg(std::shared_ptr<UserData> data, Priority priority)
 
 # Priority Queue
 
-Messages are stored in a `std::priority_queue` rather than a plain `std::queue`. A custom comparator ensures messages with a higher `Priority` value are dequeued first.
+Messages are stored in one `std::deque` per priority level rather than a single combined queue:
 
 ```cpp
 enum class Priority { LOW = 0, NORMAL = 1, HIGH = 2 };
 
-struct ThreadMsgComparator {
-    bool operator()(const std::shared_ptr<ThreadMsg>& a,
-                    const std::shared_ptr<ThreadMsg>& b) const {
-        return static_cast<int>(a->GetPriority()) < static_cast<int>(b->GetPriority());
-    }
-};
-
-std::priority_queue<
-    std::shared_ptr<ThreadMsg>,
-    std::vector<std::shared_ptr<ThreadMsg>>,
-    ThreadMsgComparator> m_queue;
+std::deque<std::shared_ptr<ThreadMsg>> m_highQueue;
+std::deque<std::shared_ptr<ThreadMsg>> m_normalQueue;
+std::deque<std::shared_ptr<ThreadMsg>> m_lowQueue;
 ```
 
-When several messages are in the queue simultaneously, the worker thread always processes the `HIGH` priority message first, then `NORMAL`, then `LOW`, regardless of the order they were posted. This is useful for giving urgent work — such as a shutdown or error signal — preferential access to the thread without a separate fast-path queue.
+`PostMsg()` appends to the deque matching the message's priority; `Process()` dequeues from the first non-empty deque, checked in order `HIGH`, `NORMAL`, `LOW`. When several messages are queued simultaneously, the worker thread always processes `HIGH` priority messages first, then `NORMAL`, then `LOW`, regardless of post order across levels — useful for giving urgent work, such as a shutdown or error signal, preferential access to the thread without a separate fast-path queue. Because each level is its own FIFO deque, messages posted at the *same* priority are always processed in the order they were posted.
+
+> An earlier version of this class used a single `std::priority_queue` with a comparator over `Priority`. That correctly ordered different priorities, but `std::priority_queue`'s underlying binary heap does not preserve insertion order among equal elements — messages posted at the same priority could be dequeued out of order. The per-priority deque design fixes this while keeping the same `HIGH` > `NORMAL` > `LOW` ordering guarantee across levels.
 
 # Back Pressure
 

Thread.cpp

@@ -98,7 +98,7 @@ void Thread::ExitThread()
     {
         lock_guard<mutex> lock(m_mutex);
         m_exit.store(true);
-        m_queue.push(exitMsg);
+        m_highQueue.push_back(exitMsg);
         m_cv.notify_one();
         m_cvNotFull.notify_all(); // unblock any blocked producers
     }
@@ -114,8 +114,9 @@ void Thread::ExitThread()
     {
         lock_guard<mutex> lock(m_mutex);
         m_thread.reset();
-        while (!m_queue.empty())
-            m_queue.pop();
+        m_highQueue.clear();
+        m_normalQueue.clear();
+        m_lowQueue.clear();
         m_cvNotFull.notify_all();
     }
 }
@@ -153,7 +154,7 @@ bool Thread::IsCurrentThread()
 size_t Thread::GetQueueSize()
 {
     lock_guard<mutex> lock(m_mutex);
-    return m_queue.size();
+    return m_highQueue.size() + m_normalQueue.size() + m_lowQueue.size();
 }
 
 //----------------------------------------------------------------------------
@@ -180,23 +181,32 @@ void Thread::PostMsg(std::shared_ptr<UserData> data, Priority priority)
 
     unique_lock<mutex> lk(m_mutex);
 
+    auto totalSize = [this]() {
+        return m_highQueue.size() + m_normalQueue.size() + m_lowQueue.size();
+    };
+
     // [BACK PRESSURE / DROP LOGIC]
-    if (MAX_QUEUE_SIZE > 0 && m_queue.size() >= MAX_QUEUE_SIZE)
+    if (MAX_QUEUE_SIZE > 0 && totalSize() >= MAX_QUEUE_SIZE)
     {
         if (FULL_POLICY == FullPolicy::DROP)
             return;  // silently discard — caller is not stalled
 
         // BLOCK: wait until the consumer drains a slot or the thread exits
-        m_cvNotFull.wait(lk, [this]() {
-            return m_queue.size() < MAX_QUEUE_SIZE || m_exit.load();
+        m_cvNotFull.wait(lk, [this, &totalSize]() {
+            return totalSize() < MAX_QUEUE_SIZE || m_exit.load();
         });
     }
 
     if (m_exit.load())
         return;
 
     auto threadMsg = make_shared<ThreadMsg>(MSG_POST_USER_DATA, data, priority);
-    m_queue.push(threadMsg);
+    switch (priority)
+    {
+        case Priority::HIGH:   m_highQueue.push_back(threadMsg);   break;
+        case Priority::NORMAL: m_normalQueue.push_back(threadMsg); break;
+        case Priority::LOW:    m_lowQueue.push_back(threadMsg);    break;
+    }
     m_cv.notify_one();
 }
 
@@ -281,19 +291,19 @@ void Thread::Process()
                 // watchdog would incorrectly report the thread as unresponsive.
                 auto heartbeat = m_watchdogTimeout.load() / 4;
                 m_cv.wait_for(lk, heartbeat, [this]() {
-                    return !m_queue.empty() || m_exit.load();
+                    return !(m_highQueue.empty() && m_normalQueue.empty() && m_lowQueue.empty()) || m_exit.load();
                 });
             }
             else
             {
                 // No watchdog: block indefinitely until a message arrives or
                 // ExitThread() sets m_exit and notifies.
                 m_cv.wait(lk, [this]() {
-                    return !m_queue.empty() || m_exit.load();
+                    return !(m_highQueue.empty() && m_normalQueue.empty() && m_lowQueue.empty()) || m_exit.load();
                 });
             }
 
-            if (m_queue.empty())
+            if (m_highQueue.empty() && m_normalQueue.empty() && m_lowQueue.empty())
             {
                 // Woken with no message — either the watchdog heartbeat fired
                 // (loop back to refresh m_lastAliveTime) or ExitThread() was
@@ -302,11 +312,24 @@ void Thread::Process()
                 continue;
             }
 
-            // Dequeue the highest-priority waiting message.
-            // std::priority_queue::top() returns the greatest element per the
-            // ThreadMsgComparator, i.e. HIGH > NORMAL > LOW.
-            msg = m_queue.top();
-            m_queue.pop();
+            // Dequeue the oldest waiting message from the highest-priority
+            // non-empty queue: HIGH before NORMAL before LOW. Each queue is
+            // FIFO, so same-priority messages are processed in post order.
+            if (!m_highQueue.empty())
+            {
+                msg = m_highQueue.front();
+                m_highQueue.pop_front();
+            }
+            else if (!m_normalQueue.empty())
+            {
+                msg = m_normalQueue.front();
+                m_normalQueue.pop_front();
+            }
+            else
+            {
+                msg = m_lowQueue.front();
+                m_lowQueue.pop_front();
+            }
 
             // --- Back pressure: notify a blocked producer -----------------
             // A producer in PostMsg() may be sleeping on m_cvNotFull because

Thread.h

@@ -10,8 +10,12 @@
 ///
 /// @details
 /// Key features:
-/// * **Priority Queue:** Uses std::priority_queue so high-priority messages
-///   are processed before lower-priority ones.
+/// * **Priority Queue:** Uses one FIFO deque per priority level (HIGH,
+///   NORMAL, LOW) so higher-priority messages are always processed before
+///   lower-priority ones, while messages posted at the same priority are
+///   processed in the order they were posted. (An earlier implementation
+///   used a single std::priority_queue, whose underlying binary heap does
+///   not preserve insertion order among equal-priority elements.)
 /// * **Back Pressure:** Configurable maxQueueSize. When the queue is full,
 ///   PostMsg() blocks the caller until space is available.
 /// * **Watchdog:** Optional timeout detects a stalled thread (deadlock or
@@ -22,7 +26,7 @@
 
 #include "ThreadMsg.h"
 #include <thread>
-#include <queue>
+#include <deque>
 #include <mutex>
 #include <atomic>
 #include <condition_variable>
@@ -37,14 +41,6 @@ struct UserData
     int year;
 };
 
-// Comparator: highest Priority value is processed first
-struct ThreadMsgComparator {
-    bool operator()(const std::shared_ptr<ThreadMsg>& a,
-                    const std::shared_ptr<ThreadMsg>& b) const {
-        return static_cast<int>(a->GetPriority()) < static_cast<int>(b->GetPriority());
-    }
-};
-
 /// @brief Policy applied when the thread message queue is full.
 /// @details Only meaningful when maxQueueSize > 0.
 ///   - BLOCK: PostMsg() blocks the caller until space is available (back pressure).
@@ -115,10 +111,11 @@ class Thread
     std::optional<std::thread> m_thread;
     std::atomic<bool> m_exit;
 
-    std::priority_queue<
-        std::shared_ptr<ThreadMsg>,
-        std::vector<std::shared_ptr<ThreadMsg>>,
-        ThreadMsgComparator> m_queue;
+    // One FIFO queue per priority level. Draining checks HIGH, then NORMAL,
+    // then LOW, so within a level messages are processed in post order.
+    std::deque<std::shared_ptr<ThreadMsg>> m_highQueue;
+    std::deque<std::shared_ptr<ThreadMsg>> m_normalQueue;
+    std::deque<std::shared_ptr<ThreadMsg>> m_lowQueue;
 
     std::mutex m_mutex;
     std::condition_variable m_cv;       // notifies consumer of new messages