From dd042a0ef22803d08151b0023c6610936becc8de Mon Sep 17 00:00:00 2001
From: Will Tatam <will@netmindz.net>
Date: Tue, 18 Jul 2023 14:02:49 +0100
Subject: [PATCH] Remove the command from the queue as soon as we send, but
 also send 3 idle/empty commands after each real one

---
 sensor/src/sensor.ino | 45 ++++++++++++++++++-------------------------
 1 file changed, 19 insertions(+), 26 deletions(-)

diff --git a/sensor/src/sensor.ino b/sensor/src/sensor.ino
index dbf76d4..fa51b83 100644
--- a/sensor/src/sensor.ino
+++ b/sensor/src/sensor.ino
@@ -130,11 +130,14 @@ String state = "unknown";
 
 ArduinoQueue<String> sendBuffer(10);  // TODO: might be better bigger for large temp changes. Would need testing
 
-void sendCommand(String command, int count) {
+void sendCommand(String command, int count = 1) {
     Serial.printf("Sending %s - %u times\n", command.c_str(), count);
     for (int i = 0; i < count; i++) {
         sendBuffer.enqueue(command.c_str());
     }
+    for (int i = 0; i < 3; i++) {
+        sendBuffer.enqueue(COMMAND_EMPTY);
+    }
 }
 
 void setOption(int currentIndex, int targetIndex, int options, String command = COMMAND_DOWN) {
@@ -150,7 +153,7 @@ void onSwitchStateChanged(bool state, HASwitch* sender) {
     Serial.printf("Switch %s changed - ", sender->getName());
     if (state != lightState) {
         Serial.println("Toggle");
-        sendBuffer.enqueue(COMMAND_LIGHT);
+        sendCommand(COMMAND_LIGHT);
     } else {
         Serial.println("No change needed");
     }
@@ -172,20 +175,20 @@ void onModeSwitchStateChanged(int8_t index, HASelect* sender) {
     Serial.printf("Mode Switch changed - %u\n", index);
     int currentIndex = sender->getCurrentState();
     int options = 3;
-    sendBuffer.enqueue(COMMAND_CHANGE_MODE);
+    sendCommand(COMMAND_CHANGE_MODE);
     setOption(currentIndex, index, options);
-    sendBuffer.enqueue(COMMAND_CHANGE_MODE);
+    sendCommand(COMMAND_CHANGE_MODE);
 }
 
 void onButtonPress(HAButton* sender) {
     String name = sender->getName();
     Serial.printf("Button press - %s\n", name);
     if (name == "Up") {
-        sendBuffer.enqueue(COMMAND_UP);
+        sendCommand(COMMAND_UP);
     } else if (name == "Down") {
-        sendBuffer.enqueue(COMMAND_DOWN);
+        sendCommand(COMMAND_DOWN);
     } else if (name == "Mode") {
-        sendBuffer.enqueue(COMMAND_CHANGE_MODE);
+        sendCommand(COMMAND_CHANGE_MODE);
     } else {
         Serial.printf("Unknown button %s\n", name);
     }
@@ -199,28 +202,23 @@ void onTargetTemperatureCommand(HANumeric temperature, HAHVAC* sender) {
 
     if (tubTargetTemp < 0) {
         Serial.print("ERROR: can't adjust target as current value not known");
-        sendBuffer.enqueue(
+        sendCommand(
             COMMAND_UP);  // Enter set temp mode - won't change, but should allow us to capture the set target value
         return;
     }
 
     int target = temperatureFloat * 2;  // 0.5 inc so double
     int current = tubTargetTemp * 2;
-    sendBuffer.enqueue(COMMAND_UP);  // Enter set temp mode
-    sendBuffer.enqueue(COMMAND_EMPTY);
+    sendCommand(COMMAND_UP);  // Enter set temp mode
 
     if (temperatureFloat > tubTargetTemp) {
-        for (int i = 0; i < (target - current); i++) {
-            Serial.println("Raise the temp");
-            sendBuffer.enqueue(COMMAND_UP);
-            // sendBuffer.enqueue(COMMAND_EMPTY);
-        }
+        int count = (target - current);
+        Serial.printf("Raise the temp x %u\n", count);
+        sendCommand(COMMAND_UP, count);
     } else {
-        for (int i = 0; i < (current - target); i++) {
-            Serial.println("Lower the temp");
-            sendBuffer.enqueue(COMMAND_DOWN);
-            // sendBuffer.enqueue(COMMAND_EMPTY);
-        }
+        int count = (current - target);
+        Serial.printf("Lower the temp x %u\n", count);
+        sendCommand(COMMAND_DOWN, count);
     }
 
     // sender->setTargetTemperature(temperature); // report target temperature back to the HA panel - better to see what
@@ -628,11 +626,6 @@ void handleMessage() {
                 } else {
                     telnetSend("CMD: " + cmd);
                 }
-                if (!lastRaw3.equals(cmd)) {
-                    // Controller responded to command
-                    sendBuffer.dequeue();
-                    Serial.printf("YAY: command response : %u\n", delayTime);
-                }
 
                 if (!lastRaw3.equals(cmd) && cmd != "0000000000") {  // ignore idle command
                     lastRaw3 = cmd;
@@ -743,7 +736,7 @@ void sendCommand() {
         }
         // tub.flush(true);
         if (digitalRead(PIN_5_PIN) == LOW) {
-            // sendBuffer.dequeue(); // TODO: trying to resend now till we see response
+            sendBuffer.dequeue();
             Serial.printf("message sent : %u\n", delayTime);
             // delayTime += 10;
         }