update espMqttclient

lib/espMqttClient/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:
@@ -110,6 +111,7 @@ class MqttClient {
     disconnectingTcp2 =  8
   };
   std::atomic<State> _state;
+  inline void _setState(State newState);
 
  private:
   char _generatedClientId[EMC_CLIENTID_LENGTH];
@@ -130,7 +132,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 +150,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();