From ade2da7fd2a0dd4d3f41d134b9aeff7392b2bf8d Mon Sep 17 00:00:00 2001
From: CommanderRedYT <ccommanderred@gmail.com>
Date: Mon, 3 Jan 2022 00:07:46 +0100
Subject: [PATCH] Readded queue

---
 main/espnowfunctions.cpp | 76 +++++++++++++++++++++++-----------------
 main/espnowfunctions.h   |  2 ++
 2 files changed, 45 insertions(+), 33 deletions(-)

diff --git a/main/espnowfunctions.cpp b/main/espnowfunctions.cpp
index 97893b3..2dc7111 100644
--- a/main/espnowfunctions.cpp
+++ b/main/espnowfunctions.cpp
@@ -18,6 +18,7 @@ constexpr const char * const TAG = "BOBBY_ESP_NOW";
 
 uint16_t lastYear; // Used for esp-now timesync
 
+std::deque<esp_now_message_t> message_queue{};
 std::vector<esp_now_peer_info_t> peers{};
 uint8_t initialized{0};
 
@@ -40,39 +41,7 @@ extern "C" void onReceive(const uint8_t *mac_addr, const uint8_t *data, int data
 
         ESP_LOGD(TAG, "Type: %s - Message: %s", msg.type.c_str(), msg.content.c_str());
 
-        if (msg.type == "T")
-        {
-            if (!receiveTimeStamp || !settings.espnow.syncTime)
-                return;
-
-            if (const auto result = cpputils::fromString<uint64_t>(msg.content); result)
-            {
-                onRecvTs(*result);
-            }
-            else
-            {
-                ESP_LOGW(TAG, "could not parse number: %.*s", result.error().size(), result.error().data());
-            }
-        }
-        else if (msg.type == "BOBBYT")
-        {
-            if (!receiveTsFromOtherBobbycars || !settings.espnow.syncTimeWithOthers)
-                return;
-
-            if (const auto result = cpputils::fromString<uint64_t>(msg.content); result)
-            {
-                ESP_LOGI(TAG, "setting current time to %" PRIu64, *result);
-                onRecvTs(*result, true);
-            }
-            else
-            {
-                ESP_LOGW(TAG, "could not parse number: %.*s", result.error().size(), result.error().data());
-            }
-        }
-        else
-        {
-            ESP_LOGI(TAG, "Unkown Type: %s - Message: %s", msg.type.c_str(), msg.content.c_str());
-        }
+        message_queue.push_back(msg);
     }
     else
     {
@@ -212,6 +181,47 @@ void handle()
         }
         return;
     }
+
+    if (message_queue.size())
+    {
+        for (const esp_now_message_t &msg : message_queue)
+        {
+            if (msg.type == "T")
+            {
+                if (!receiveTimeStamp || !settings.espnow.syncTime)
+                    return;
+
+                if (const auto result = cpputils::fromString<uint64_t>(msg.content); result)
+                {
+                    onRecvTs(*result);
+                }
+                else
+                {
+                    ESP_LOGW(TAG, "could not parse number: %.*s", result.error().size(), result.error().data());
+                }
+            }
+            else if (msg.type == "BOBBYT")
+            {
+                if (!receiveTsFromOtherBobbycars || !settings.espnow.syncTimeWithOthers)
+                    return;
+
+                if (const auto result = cpputils::fromString<uint64_t>(msg.content); result)
+                {
+                    ESP_LOGI(TAG, "setting current time to %" PRIu64, *result);
+                    onRecvTs(*result, true);
+                }
+                else
+                {
+                    ESP_LOGW(TAG, "could not parse number: %.*s", result.error().size(), result.error().data());
+                }
+            }
+            else
+            {
+                ESP_LOGI(TAG, "Unkown Type: %s - Message: %s", msg.type.c_str(), msg.content.c_str());
+            }
+        }
+        message_queue.clear();
+    }
 }
 
 void onRecvTs(uint64_t millis, bool isFromBobbycar)
diff --git a/main/espnowfunctions.h b/main/espnowfunctions.h
index 8c328d6..99f7726 100644
--- a/main/espnowfunctions.h
+++ b/main/espnowfunctions.h
@@ -1,6 +1,7 @@
 #pragma once
 #ifdef FEATURE_ESPNOW
 #include <cstdint>
+#include <deque>
 #include <string>
 #include <string_view>
 #include <vector>
@@ -18,6 +19,7 @@ struct esp_now_message_t
 extern bool receiveTimeStamp;
 extern bool receiveTsFromOtherBobbycars;
 
+extern std::deque<esp_now_message_t> message_queue;
 extern std::vector<esp_now_peer_info_t> peers;
 
 void initESPNow();