From f1d3a15498019c46202aa8e23df06b25487bf9ea Mon Sep 17 00:00:00 2001
From: terror <terror85@gmail.com>
Date: Sat, 25 Sep 2021 15:44:09 +0300
Subject: [PATCH] added reply to message subscribe and unsubscribe

---
 src/TinyMqtt.cpp | 26 +++++++++++++++++++++++++-
 1 file changed, 25 insertions(+), 1 deletion(-)

diff --git a/src/TinyMqtt.cpp b/src/TinyMqtt.cpp
index a9be1f9..d6e95cf 100644
--- a/src/TinyMqtt.cpp
+++ b/src/TinyMqtt.cpp
@@ -510,17 +510,25 @@ if (mesg->type() != MqttMessage::Type::PingReq && mesg->type() != MqttMessage::T
 				payload = header+2;
 				
 				debug("un/subscribe loop");
+				std::vector<char> qoss;
 				while(payload < mesg->end())
 				{
 					mesg->getString(payload, len);	// Topic
 					debug( "  topic (" << std::string(payload, len) << ')');
 					// subscribe(Topic(payload, len));
 					Topic topic(payload, len);
+
 					payload += len;
 					if ((mesg->type() & 0XF0) == MqttMessage::Type::Subscribe)
 					{
 						uint8_t qos = *payload++;
-						if (qos != 0) debug("Unsupported QOS" << qos << endl);
+						if (qos != 0)
+						{
+							debug("Unsupported QOS" << qos << endl);
+							qoss.push_back(0x80);
+						}
+						else
+							qoss.push_back(qos);
 						subscriptions.insert(topic);
 					}
 					else
@@ -533,6 +541,22 @@ if (mesg->type() != MqttMessage::Type::PingReq && mesg->type() != MqttMessage::T
 				debug("end loop");
 				bclose = false;
 				// TODO SUBACK
+				if ((mesg->type() & 0XF0) == MqttMessage::Type::Subscribe)
+				{
+					sMQTTMessage msg(sMQTTMessage::Type::SubAck);
+					msg.add(header[0]);
+					msg.add(header[1]);
+					for (int i = 0; i<qoss.size(); i++)
+						msg.add(qoss[i]);
+					msg.sendTo(this);
+				}
+				else
+				{
+					sMQTTMessage msg(sMQTTMessage::Type::UnSuback);
+					msg.add(header[0]);
+					msg.add(header[1]);
+					msg.sendTo(this);
+				}
 			}
 			break;