From 25ad9e46cb913c3ce6b964bf79b3a7be96f7def1 Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Thu, 1 Aug 2019 21:29:14 +0300
Subject: [PATCH] make a QueueManager

---
 Firmware/EggbotWireless/include/Globals.h     |  3 --
 .../EggbotWireless/include/QueueManager.h     | 35 +++++++++++++
 Firmware/EggbotWireless/src/Globals.cpp       |  3 --
 Firmware/EggbotWireless/src/QueueManager.cpp  | 47 ++++++++++++++++++
 Firmware/EggbotWireless/src/main.cpp          | 49 ++-----------------
 5 files changed, 85 insertions(+), 52 deletions(-)
 create mode 100644 Firmware/EggbotWireless/include/QueueManager.h
 create mode 100644 Firmware/EggbotWireless/src/QueueManager.cpp

diff --git a/Firmware/EggbotWireless/include/Globals.h b/Firmware/EggbotWireless/include/Globals.h
index 2466460..f54bd82 100644
--- a/Firmware/EggbotWireless/include/Globals.h
+++ b/Firmware/EggbotWireless/include/Globals.h
@@ -7,7 +7,4 @@
 #include "LocalCommand.h"
 #include "common/Commands.h"
 
-extern std::queue<Command> commandQueue;
-extern std::queue<LCommand> lCommandQueue;
-
 #endif  // GLOBALS_H
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/include/QueueManager.h b/Firmware/EggbotWireless/include/QueueManager.h
new file mode 100644
index 0000000..b104844
--- /dev/null
+++ b/Firmware/EggbotWireless/include/QueueManager.h
@@ -0,0 +1,35 @@
+#ifndef QUEUE_MANAGER_H
+#define QUEUE_MANAGER_h
+
+#include <Arduino.h>
+#include <string>
+#include <queue>
+
+#include "common/Commands.h"
+#include "LocalCommand.h"
+#include "Executor.h"
+#include "LocalExecutor.h"
+#include "common/Status.h"
+#include "Power.h"
+#include "GCodeParser.h"
+
+class QueueManager {
+   private:
+    std::queue<Command> commandQueue;
+    std::queue<LCommand> lCommandQueue;
+
+    bool shouldPrintSts;
+    void printSts(Status status);
+
+   public:
+    QueueManager();
+    void init();
+    void loopRoutine();
+    uint8_t execQueueNum();
+    void putCommand(std::string cmd);
+    void putCommand(char *cmd);
+};
+
+extern QueueManager queueManager;
+
+#endif
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/Globals.cpp b/Firmware/EggbotWireless/src/Globals.cpp
index ab642fd..3fb64a5 100644
--- a/Firmware/EggbotWireless/src/Globals.cpp
+++ b/Firmware/EggbotWireless/src/Globals.cpp
@@ -1,4 +1 @@
 #include "Globals.h"
-
-std::queue<Command> commandQueue;
-std::queue<LCommand> lCommandQueue;
diff --git a/Firmware/EggbotWireless/src/QueueManager.cpp b/Firmware/EggbotWireless/src/QueueManager.cpp
new file mode 100644
index 0000000..e919a52
--- /dev/null
+++ b/Firmware/EggbotWireless/src/QueueManager.cpp
@@ -0,0 +1,47 @@
+#include "QueueManager.h"
+
+QueueManager::QueueManager() {}
+
+void QueueManager::init() {}
+
+void QueueManager::printSts(Status status) {
+        if (status.type == StatusType::WAIT) {
+            shouldPrintSts = true;
+        } else if (status.type == StatusType::NEXT) {
+            Serial.println("OK");
+        } else if (status.type == StatusType::TIMEOUT) {
+            Serial.println("Timeout");
+        } else {
+            Serial.print("Error: ");
+            Serial.println(static_cast<int>(status.type));
+        }
+}
+
+void QueueManager::loopRoutine() {
+    Status status = executor.status();
+    if (shouldPrintSts) {
+        shouldPrintSts = false;
+        printSts(status);
+    }
+    if (status.type == StatusType::NEXT && !commandQueue.empty()) {
+        power.commandHook();
+        executor.execCommand(commandQueue.front());
+        commandQueue.pop();
+        shouldPrintSts = true;
+    }
+    if (!lCommandQueue.empty()) {
+        localExecutor.execCommand(lCommandQueue.front());
+        lCommandQueue.pop();
+        shouldPrintSts = true;
+    }
+}
+
+void QueueManager::putCommand(std::string cmd) {
+    if (toupper(cmd[0]) == 'L') {
+        lCommandQueue.emplace(cmd);
+    } else {
+        commandQueue.push(parseGCode(cmd));
+    }
+}
+
+QueueManager queueManager;
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index 1813427..790f808 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -15,10 +15,9 @@
 #include "Power.h"
 #include "WiFiManager.h"
 #include "WebAPI.h"
+#include "QueueManager.h"
 #include "common/Commands.h"
 
-bool shouldPrintSts;
-
 void setup() {
     Serial.begin(115200);
     Wire.begin(12, 13);
@@ -30,22 +29,8 @@ void setup() {
     webApi.init();
 }
 
-void printSts(Status status) {
-    if (status.type == StatusType::WAIT) {
-        shouldPrintSts = true;
-    } else if (status.type == StatusType::NEXT) {
-        Serial.println("OK");
-    } else if (status.type == StatusType::TIMEOUT) {
-        Serial.println("Timeout");
-    } else {
-        Serial.print("Error: ");
-        Serial.println(static_cast<int>(status.type));
-    }
-}
-
 void serialLoop() {
     static std::string inString;
-    static bool localCmd = false;
     while (Serial.available() > 0) {
         char inChar = Serial.read();
 
@@ -54,17 +39,8 @@ void serialLoop() {
             break;
         }
 
-        if (inString.length() == 0 && toupper(inChar) == 'L') {
-            localCmd = true;
-        }
-
         if (inChar == '\n') {
-            if (localCmd) {
-                lCommandQueue.emplace(inString);
-                localCmd = false;
-            } else {
-                commandQueue.push(parseGCode(inString));
-            }
+            queueManager.putCommand(inString);
             inString = "";
         } else {
             inString += inChar;
@@ -72,28 +48,9 @@ void serialLoop() {
     }
 }
 
-void commandsLoop() {
-    Status status = executor.status();
-    if (shouldPrintSts) {
-        shouldPrintSts = false;
-        printSts(status);
-    }
-    if (status.type == StatusType::NEXT && !commandQueue.empty()) {
-        power.commandHook();
-        executor.execCommand(commandQueue.front());
-        commandQueue.pop();
-        shouldPrintSts = true;
-    }
-    if (!lCommandQueue.empty()) {
-        localExecutor.execCommand(lCommandQueue.front());
-        lCommandQueue.pop();
-        shouldPrintSts = true;
-    }
-}
-
 void loop() {
     serialLoop();
-    commandsLoop();
+    queueManager.loopRoutine();
 
     MDNS.update();
     webApi.loopRoutine();