From e064a6c2f59ed3590bbb48bac57cf50563c76f2d Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Fri, 19 Nov 2021 11:52:05 +0100
Subject: [PATCH] Introduce H0 command to invoke Idler+Selector homing seq. if
 safe

---
 src/logic/CMakeLists.txt   |  1 +
 src/logic/home.cpp         | 39 ++++++++++++++++++++++++++++++++++++++
 src/logic/home.h           | 37 ++++++++++++++++++++++++++++++++++++
 src/logic/progress_codes.h |  2 ++
 src/main.cpp               |  4 ++++
 src/modules/protocol.h     |  3 ++-
 6 files changed, 85 insertions(+), 1 deletion(-)
 create mode 100644 src/logic/home.cpp
 create mode 100644 src/logic/home.h

diff --git a/src/logic/CMakeLists.txt b/src/logic/CMakeLists.txt
index 36bfad8..f879f93 100644
--- a/src/logic/CMakeLists.txt
+++ b/src/logic/CMakeLists.txt
@@ -5,6 +5,7 @@ target_sources(
           eject_filament.cpp
           feed_to_bondtech.cpp
           feed_to_finda.cpp
+          home.cpp
           load_filament.cpp
           no_command.cpp
           retract_from_finda.cpp
diff --git a/src/logic/home.cpp b/src/logic/home.cpp
new file mode 100644
index 0000000..6aec28b
--- /dev/null
+++ b/src/logic/home.cpp
@@ -0,0 +1,39 @@
+/// @file home.cpp
+#include "home.h"
+#include "../modules/finda.h"
+#include "../modules/globals.h"
+#include "../modules/idler.h"
+#include "../modules/motion.h"
+#include "../modules/permanent_storage.h"
+#include "../modules/selector.h"
+#include "../debug.h"
+
+namespace logic {
+
+Home home;
+
+void Home::Reset(uint8_t /*param*/) {
+    error = ErrorCode::RUNNING;
+    state = ProgressCode::Homing;
+    InvalidateHomingAndFilamentState();
+}
+
+bool Home::StepInner() {
+    switch (state) {
+    case ProgressCode::Homing:
+        if (mi::idler.State() == mi::Idler::Ready && ms::selector.State() == ms::selector.Ready) {
+            state = ProgressCode::OK;
+            error = ErrorCode::OK;
+        }
+        break;
+    case ProgressCode::OK:
+        return true;
+    default: // we got into an unhandled state, better report it
+        state = ProgressCode::ERRInternal;
+        error = ErrorCode::INTERNAL;
+        return true;
+    }
+    return false;
+}
+
+} // namespace logic
diff --git a/src/logic/home.h b/src/logic/home.h
new file mode 100644
index 0000000..eb07d1e
--- /dev/null
+++ b/src/logic/home.h
@@ -0,0 +1,37 @@
+/// @file home.h
+#pragma once
+#include <stdint.h>
+#include "command_base.h"
+
+namespace logic {
+
+/// @brief  A high-level command state machine - wrapps the rehoming procedure to be used from a printer
+///
+/// The home operation consists of:
+/// - invalidates Idler's and Selector's homing flags
+/// - waits until Idler and Selector finish their homing sequences
+///
+/// Beware - Idler and Selector will NOT perform the homing moves if filament sensor state is not in the right state
+/// - Idler: filament must not be in the fsensor or nozzle
+/// - Selector: filament must not be in the selector, fsensor or nozzle
+/// It is up to the printer to let the MMU unload filament first (to make sure everything is safe) and then issue the homing command
+///
+/// Moreover - Idler and Selector try to home automagically a runtime when they know it is safe.
+/// This high-level command is just a way to invoke re-homing from the printer while all safety measures are kept.
+class Home : public CommandBase {
+public:
+    inline Home()
+        : CommandBase() {}
+
+    /// Restart the automaton
+    /// @param param unused
+    void Reset(uint8_t /*param*/) override;
+
+    /// @returns true if the state machine finished its job, false otherwise
+    bool StepInner() override;
+};
+
+/// The one and only instance of Home state machine in the FW
+extern Home home;
+
+} // namespace logic
diff --git a/src/logic/progress_codes.h b/src/logic/progress_codes.h
index e4f8070..eb97c56 100644
--- a/src/logic/progress_codes.h
+++ b/src/logic/progress_codes.h
@@ -35,4 +35,6 @@ enum class ProgressCode : uint_fast8_t {
     ParkingSelector, // P23
     EjectingFilament, // P24
     RetractingFromFinda, // P25
+
+    Homing,
 };
diff --git a/src/main.cpp b/src/main.cpp
index 7132fdd..3c8fb5d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -27,6 +27,7 @@
 #include "logic/command_base.h"
 #include "logic/cut_filament.h"
 #include "logic/eject_filament.h"
+#include "logic/home.h"
 #include "logic/load_filament.h"
 #include "logic/no_command.h"
 #include "logic/set_mode.h"
@@ -223,6 +224,9 @@ void PlanCommand(const mp::RequestMsg &rq) {
         case mp::RequestMsgCodes::Eject:
             currentCommand = &logic::ejectFilament;
             break;
+        case mp::RequestMsgCodes::Home:
+            currentCommand = &logic::home;
+            break;
         case mp::RequestMsgCodes::Load:
             currentCommand = &logic::loadFilament;
             break;
diff --git a/src/modules/protocol.h b/src/modules/protocol.h
index a4b59cf..68fde9a 100644
--- a/src/modules/protocol.h
+++ b/src/modules/protocol.h
@@ -26,7 +26,8 @@ enum class RequestMsgCodes : uint8_t {
     Wait = 'W',
     Cut = 'K',
     FilamentType = 'F',
-    FilamentSensor = 'f'
+    FilamentSensor = 'f',
+    Home = 'H'
 };
 
 /// Definition of response message parameter codes