From 83b9cea3fd2cb58554211916147c480355833cac Mon Sep 17 00:00:00 2001
From: LandryNorris <landry.norris@live.com>
Date: Wed, 11 Sep 2024 10:07:06 -0500
Subject: [PATCH] Allow multiple messages to be scheduled with the same ID if
 the interval is set to 0.

---
 .../include/rev/Drivers/DriverDeviceThread.h  | 27 ++++++++++++++-----
 1 file changed, 21 insertions(+), 6 deletions(-)

diff --git a/src/main/native/include/rev/Drivers/DriverDeviceThread.h b/src/main/native/include/rev/Drivers/DriverDeviceThread.h
index b01d7ff..de2e072 100644
--- a/src/main/native/include/rev/Drivers/DriverDeviceThread.h
+++ b/src/main/native/include/rev/Drivers/DriverDeviceThread.h
@@ -84,6 +84,15 @@ class DriverDeviceThread {
         return nullptr; // If no matching element found
     }
 
+    detail::CANThreadSendQueueElement* findFirstMatchingIdWithNonZeroInterval(int targetId) {
+        for (auto& element : m_sendQueue) {
+            if (element.m_msg.GetMessageId() == targetId && element.m_intervalMs > 0) {
+                return &element;
+            }
+        }
+        return nullptr; // If no matching element found
+    }
+
     void removeElementsWithId(int targetId) {
         m_sendQueue.erase(std::remove_if(m_sendQueue.begin(), m_sendQueue.end(), [targetId](detail::CANThreadSendQueueElement element) { return element.m_msg.GetMessageId() == targetId; }), m_sendQueue.end());
     }
@@ -91,13 +100,19 @@ class DriverDeviceThread {
     bool EnqueueMessage(const CANMessage& msg, int32_t timeIntervalMs) {
         std::lock_guard<std::mutex> lock(m_writeMutex);
 
-        detail::CANThreadSendQueueElement* existing = findFirstMatchingId(msg.GetMessageId());
-
-        if(existing) {
-            existing->m_intervalMs = timeIntervalMs;
-            existing->m_msg = msg;
-        } else {
+        if(timeIntervalMs == 0) {
+            // If the time interval is 0, we want to allow duplicates.
             m_sendQueue.push_back(detail::CANThreadSendQueueElement(msg, timeIntervalMs));
+        } else {
+            // We don't want to replace elements with zero as the interval. Those should be guaranteed to be sent
+            detail::CANThreadSendQueueElement* existing = findFirstMatchingIdWithNonZeroInterval(msg.GetMessageId());
+
+            if(existing) {
+                existing->m_intervalMs = timeIntervalMs;
+                existing->m_msg = msg;
+            } else {
+                m_sendQueue.push_back(detail::CANThreadSendQueueElement(msg, timeIntervalMs));
+            }
         }
 
         // TODO: Limit the max queue size