From c50787ba8435068562a70faacf8b2aa96c9c694e Mon Sep 17 00:00:00 2001
From: CommanderRedYT <ccommanderred@gmail.com>
Date: Mon, 13 Dec 2021 02:14:03 +0100
Subject: [PATCH] Fixed esp_now_init(), added message queue to not block the
 callback function

---
 main/espnowfunctions.cpp | 57 +++++++++++++++++++++++++++++++++++++---
 main/espnowfunctions.h   | 10 +++++++
 main/main.cpp            |  6 ++++-
 3 files changed, 68 insertions(+), 5 deletions(-)

diff --git a/main/espnowfunctions.cpp b/main/espnowfunctions.cpp
index befadfb..d5d75af 100644
--- a/main/espnowfunctions.cpp
+++ b/main/espnowfunctions.cpp
@@ -6,18 +6,29 @@ constexpr const char * const TAG = "BOBBY_ESP_NOW";
 #include <espnowwrapper.h>
 #include <esp_log.h>
 
+#include "globals.h"
+
 namespace espnow {
 
+std::deque<esp_now_message_t> message_queue{};
+
 void onReceive(const uint8_t *mac_addr, const uint8_t *data, int data_len)
 {
+    ESP_LOGI(TAG, "Received data");
     const std::string data_str(data, data+data_len);
 
     size_t sep_pos = data_str.find(":");
     if (std::string::npos != sep_pos)
     {
         std::string msg_type = data_str.substr(0, sep_pos);
-        std::string msg = data_str.substr(sep_pos, data_str.length()-1);
+        std::string msg = data_str.substr(sep_pos+1, data_str.length()-3); // - 3 may needs to be converted to sep_pos+1
         ESP_LOGI(TAG, "Type: %s - Message: %s", msg_type.c_str(), msg.c_str());
+
+        const esp_now_message_t message{
+            .content = msg,
+            .type = msg_type
+        };
+        message_queue.push_back(message);
     }
     else
     {
@@ -27,9 +38,47 @@ void onReceive(const uint8_t *mac_addr, const uint8_t *data, int data_len)
 
 void initESPNow()
 {
-    ESP_LOGI(TAG, "Initializing esp now...");
-    ESPNow.reg_recv_cb(onReceive);
-    ESPNow.init();
+    ESP_LOGI(TAG, "Initializing esp-now...");
+
+    if (!settings.wifiSettings.wifiApEnabled && !settings.wifiSettings.wifiStaEnabled)
+    {
+        ESP_LOGW(TAG, "cannot execute esp_now_init(): tcp stack is down.");
+        return;
+    }
+
+    if (const auto error = esp_now_init(); error != ESP_OK)
+    {
+        ESP_LOGE(TAG, "esp_now_init() failed with %s", esp_err_to_name(error));
+        return;
+    }
+
+    if (const auto error = esp_now_register_recv_cb(onReceive); error != ESP_OK)
+    {
+        ESP_LOGE(TAG, "esp_now_register_recv_cb() failed with %s", esp_err_to_name(error));
+        return;
+    }
+}
+
+void handle()
+{
+    if(message_queue.size())
+    {
+        for (const esp_now_message_t &msg : message_queue)
+        {
+            ESP_LOGW(TAG, "queue has processed message of type '%s' with content '%s'", msg.type.c_str(), msg.content.c_str());
+            message_queue.pop_front();
+
+            if (msg.type == "T")
+            {
+                // convert msg.content into uint64_t
+                // onRecvTs(converted);
+            }
+        }
+        for (const esp_now_message_t &msg : message_queue)
+        {
+            ESP_LOGI(TAG, "%s", msg.content.c_str());
+        }
+    }
 }
 
 void onRecvTs(uint64_t millis)
diff --git a/main/espnowfunctions.h b/main/espnowfunctions.h
index f3b25be..0e18189 100644
--- a/main/espnowfunctions.h
+++ b/main/espnowfunctions.h
@@ -1,11 +1,21 @@
 #pragma once
 #ifdef FEATURE_ESPNOW
 #include <cstdint>
+#include <deque>
+#include <string>
 
 namespace espnow {
 constexpr const uint8_t broadcast_address[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+struct esp_now_message_t {
+    const std::string content;
+    const std::string type;
+};
+
+extern std::deque<esp_now_message_t> message_queue;
+
 void onReceive(const uint8_t *mac_addr, const uint8_t *data, int data_len);
 void initESPNow();
+void handle();
 void onRecvTs(uint64_t millis);
 } // namespace espnow
 #endif
diff --git a/main/main.cpp b/main/main.cpp
index c2fce5e..4678423 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -294,7 +294,7 @@ extern "C" void app_main()
         espgui::switchScreen<StatusDisplay>();
 #endif
 #ifdef FEATURE_ESPNOW
-        espnow::initESPNow();
+    espnow::initESPNow();
 #endif
 
     while (true)
@@ -412,6 +412,10 @@ extern "C" void app_main()
         }
 #endif
 
+#ifdef FEATURE_ESPNOW
+        espnow::handle();
+#endif
+
 #ifdef FEATURE_CLOUD
         if (!lastCloudCollect || now - *lastCloudCollect >= std::chrono::milliseconds{settings.boardcomputerHardware.timersSettings.cloudCollectRate})
         {