From 593921bcc6a114b31d9f94fced762afa37389f85 Mon Sep 17 00:00:00 2001
From: Bert Melis <bertmelis@users.noreply.github.com>
Date: Thu, 31 Aug 2023 10:35:09 +0200
Subject: [PATCH] Fix memory issues on low free memory

* fix memory issues on low free memory
* add queue size
---
 docs/index.md          |  6 ++++++
 src/MqttClient.cpp     |  8 ++++++++
 src/MqttClient.h       | 19 ++++++++++++++-----
 src/Outbox.h           | 10 ++++++++++
 src/Packets/Packet.cpp |  6 +++---
 src/Packets/Packet.h   |  2 +-
 6 files changed, 42 insertions(+), 9 deletions(-)

diff --git a/docs/index.md b/docs/index.md
index ba1072c..0970c82 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -341,6 +341,12 @@ const char* getClientId() const
 
 Retuns the client ID.
 
+```cpp
+size_t queueSize();
+```
+
+Returns the amount of elements, regardless of type, in the queue.
+
 # Compile time configuration
 
 A number of constants which influence the behaviour of the client can be set at compile time. You can set these options in the `Config.h` file or pass the values as compiler flags. Because these options are compile-time constants, they are used for all instances of `espMqttClient` you create in your program.
diff --git a/src/MqttClient.cpp b/src/MqttClient.cpp
index 361f503..1e59b7f 100644
--- a/src/MqttClient.cpp
+++ b/src/MqttClient.cpp
@@ -194,6 +194,14 @@ const char* MqttClient::getClientId() const {
   return _clientId;
 }
 
+size_t MqttClient::queueSize() {
+  size_t ret = 0;
+  EMC_SEMAPHORE_TAKE();
+  ret = _outbox.size();
+  EMC_SEMAPHORE_GIVE();
+  return ret;
+}
+
 void MqttClient::loop() {
   switch (_state) {
     case State::disconnected:
diff --git a/src/MqttClient.h b/src/MqttClient.h
index 8d89aba..b2452e5 100644
--- a/src/MqttClient.h
+++ b/src/MqttClient.h
@@ -65,6 +65,7 @@ class MqttClient {
   uint16_t publish(const char* topic, uint8_t qos, bool retain, espMqttClientTypes::PayloadCallback callback, size_t length);
   void clearQueue(bool deleteSessionData = false);  // Not MQTT compliant and may cause unpredictable results when `deleteSessionData` = true!
   const char* getClientId() const;
+  size_t queueSize();  // No const because of mutex
   void loop();
 
  protected:
@@ -130,7 +131,7 @@ class MqttClient {
     uint32_t timeSent;
     espMqttClientInternals::Packet packet;
     template <typename... Args>
-    OutgoingPacket(uint32_t t, espMqttClientTypes::Error error, Args&&... args) :
+    OutgoingPacket(uint32_t t, espMqttClientTypes::Error& error, Args&&... args) :  // NOLINT(runtime/references)
       timeSent(t),
       packet(error, std::forward<Args>(args) ...) {}
   };
@@ -148,16 +149,24 @@ class MqttClient {
   bool _addPacket(Args&&... args) {
     espMqttClientTypes::Error error(espMqttClientTypes::Error::SUCCESS);
     espMqttClientInternals::Outbox<OutgoingPacket>::Iterator it = _outbox.emplace(0, error, std::forward<Args>(args) ...);
-    if (it && error == espMqttClientTypes::Error::SUCCESS) return true;
-    return false;
+    if (it && error == espMqttClientTypes::Error::SUCCESS) {
+      return true;
+    } else {
+      if (it) _outbox.remove(it);
+      return false;
+    }
   }
 
   template <typename... Args>
   bool _addPacketFront(Args&&... args) {
     espMqttClientTypes::Error error(espMqttClientTypes::Error::SUCCESS);
     espMqttClientInternals::Outbox<OutgoingPacket>::Iterator it = _outbox.emplaceFront(0, error, std::forward<Args>(args) ...);
-    if (it && error == espMqttClientTypes::Error::SUCCESS) return true;
-    return false;
+    if (it && error == espMqttClientTypes::Error::SUCCESS) {
+      return true;
+    } else {
+      if (it) _outbox.remove(it);
+      return false;
+    }
   }
 
   void _checkOutbox();
diff --git a/src/Outbox.h b/src/Outbox.h
index b123b38..486b6ac 100644
--- a/src/Outbox.h
+++ b/src/Outbox.h
@@ -163,6 +163,16 @@ class Outbox {
     return false;
   }
 
+  size_t size() const {
+    Node* n = _first;
+    size_t count = 0;
+    while (n) {
+      n = n->next;
+      ++count;
+    }
+    return count;
+  }
+
  private:
   Node* _first;
   Node* _last;
diff --git a/src/Packets/Packet.cpp b/src/Packets/Packet.cpp
index df463ef..2f84b50 100644
--- a/src/Packets/Packet.cpp
+++ b/src/Packets/Packet.cpp
@@ -100,7 +100,7 @@ Packet::Packet(espMqttClientTypes::Error& error,
   (password ? 2 + strlen(password) : 0);
 
   // allocate memory
-  if (!_allocate(remainingLength)) {
+  if (!_allocate(remainingLength, false)) {
     error = espMqttClientTypes::Error::OUT_OF_MEMORY;
     return;
   }
@@ -300,8 +300,8 @@ Packet::Packet(espMqttClientTypes::Error& error, MQTTPacketType type)
 }
 
 
-bool Packet::_allocate(size_t remainingLength) {
-  if (EMC_GET_FREE_MEMORY() < EMC_MIN_FREE_MEMORY) {
+bool Packet::_allocate(size_t remainingLength, bool check) {
+  if (check && EMC_GET_FREE_MEMORY() < EMC_MIN_FREE_MEMORY) {
     emc_log_w("Packet buffer not allocated: low memory");
     return false;
   }
diff --git a/src/Packets/Packet.h b/src/Packets/Packet.h
index 1af2f06..f2b2902 100644
--- a/src/Packets/Packet.h
+++ b/src/Packets/Packet.h
@@ -133,7 +133,7 @@ class Packet {
 
  private:
   // pass remainingLength = total size - header - remainingLengthLength!
-  bool _allocate(size_t remainingLength);
+  bool _allocate(size_t remainingLength, bool check = true);
 
   // fills header and returns index of next available byte in buffer
   size_t _fillPublishHeader(uint16_t packetId,