From 7cab9dc9156a0495112e012ea1ff05aef161a03b Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Thu, 10 Jun 2021 08:16:56 +0200
Subject: [PATCH] Add Tool change operation

---
 src/logic/eject_filament.cpp |  4 +--
 src/logic/progress_codes.h   |  1 +
 src/logic/tool_change.cpp    | 53 +++++++++++++++++++++++++++++++++---
 src/logic/tool_change.h      |  6 +++-
 4 files changed, 57 insertions(+), 7 deletions(-)

diff --git a/src/logic/eject_filament.cpp b/src/logic/eject_filament.cpp
index 78896c3..09aa47d 100644
--- a/src/logic/eject_filament.cpp
+++ b/src/logic/eject_filament.cpp
@@ -1,11 +1,11 @@
 #include "eject_filament.h"
 #include "../modules/buttons.h"
 #include "../modules/finda.h"
+#include "../modules/idler.h"
 #include "../modules/leds.h"
 #include "../modules/motion.h"
-#include "../modules/selector.h"
-#include "../modules/idler.h"
 #include "../modules/permanent_storage.h"
+#include "../modules/selector.h"
 
 namespace logic {
 
diff --git a/src/logic/progress_codes.h b/src/logic/progress_codes.h
index d801fa5..2b4cd03 100644
--- a/src/logic/progress_codes.h
+++ b/src/logic/progress_codes.h
@@ -18,6 +18,7 @@ enum class ProgressCode : uint_fast8_t {
     ERR1WaitingForUser,
 
     UnloadingFilament,
+    LoadingFilament,
     SelectingFilamentSlot,
     FeedingToFINDA,
     PreparingBlade,
diff --git a/src/logic/tool_change.cpp b/src/logic/tool_change.cpp
index 778df0d..ee46ea6 100644
--- a/src/logic/tool_change.cpp
+++ b/src/logic/tool_change.cpp
@@ -1,23 +1,68 @@
 #include "tool_change.h"
 #include "../modules/buttons.h"
 #include "../modules/finda.h"
+#include "../modules/idler.h"
 #include "../modules/leds.h"
 #include "../modules/motion.h"
 #include "../modules/permanent_storage.h"
+#include "../modules/selector.h"
 
 namespace logic {
 
 ToolChange toolChange;
 
+namespace mm = modules::motion;
+namespace mi = modules::idler;
+namespace ms = modules::selector;
+
 void ToolChange::Reset(uint8_t param) {
-    namespace mm = modules::motion;
-    state = ProgressCode::EngagingIdler;
-    error = ErrorCode::OK;
+    //    if( param == active_extruder ) // @@TODO
+    //        return true;
+
+    plannedSlot = param;
+
+    bool isFilamentLoaded = true; //@@TODO
+
+    if (isFilamentLoaded) {
+        state = ProgressCode::UnloadingFilament;
+        unl.Reset(0); //@@TODO act on active extruder only
+    } else {
+        state = ProgressCode::LoadingFilament;
+        load.Reset(plannedSlot);
+    }
 }
 
 bool ToolChange::Step() {
-    namespace mm = modules::motion;
     switch (state) {
+    case ProgressCode::UnloadingFilament:
+        if (unl.Step()) {
+            // unloading sequence finished
+            switch (unl.Error()) {
+            case ErrorCode::OK: // finished successfully
+                state = ProgressCode::LoadingFilament;
+                load.Reset(plannedSlot);
+                break;
+            case ErrorCode::UNLOAD_ERROR2: // @@TODO what shall we do in case of this error?
+            case ErrorCode::UNLOAD_FINDA_DIDNT_TRIGGER:
+                break;
+            }
+        }
+        break;
+    case ProgressCode::LoadingFilament:
+        if (load.Step()) {
+            // unloading sequence finished
+            switch (load.Error()) {
+            case ErrorCode::OK: // finished successfully
+                state = ProgressCode::OK;
+                break;
+                //            case ErrorCode::LOAD_ERROR2: // @@TODO load errors?
+                //            case ErrorCode::LOAD_FINDA_DIDNT_TRIGGER:
+                break;
+            }
+        }
+        break;
+    case ProgressCode::OK:
+        return true;
     }
     return false;
 }
diff --git a/src/logic/tool_change.h b/src/logic/tool_change.h
index e721d26..2453a73 100644
--- a/src/logic/tool_change.h
+++ b/src/logic/tool_change.h
@@ -1,7 +1,8 @@
 #pragma once
 #include <stdint.h>
 #include "command_base.h"
-#include "unload_to_finda.h"
+#include "unload_filament.h"
+#include "load_filament.h"
 
 namespace logic {
 
@@ -19,6 +20,9 @@ public:
     bool Step() override;
 
 private:
+    UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well
+    LoadFilament load;
+    uint8_t plannedSlot;
 };
 
 extern ToolChange toolChange;