From 7555bbb906338631b3ef708c89fb864b39160191 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Wed, 9 Jun 2021 09:31:24 +0200
Subject: [PATCH] Add Selector module

---
 src/modules/selector.cpp | 46 ++++++++++++++++++++++++++++++++++
 src/modules/selector.h   | 53 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 99 insertions(+)
 create mode 100644 src/modules/selector.cpp
 create mode 100644 src/modules/selector.h

diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp
new file mode 100644
index 0000000..f9ce348
--- /dev/null
+++ b/src/modules/selector.cpp
@@ -0,0 +1,46 @@
+#include "selector.h"
+#include "buttons.h"
+#include "leds.h"
+#include "motion.h"
+#include "permanent_storage.h"
+
+namespace modules {
+namespace selector {
+
+namespace mm = modules::motion;
+
+bool Selector::MoveToSlot(uint8_t slot) {
+    if (state == Moving)
+        return false;
+    plannedSlot = slot;
+    // mm::motion.PlanMove(1, slotPositions[slot], 0, 1000, 0, 0); // @@TODO
+    state = Moving;
+    return true;
+}
+
+bool Selector::Home() {
+    if (state == Moving)
+        return false;
+    mm::motion.Home(mm::Selector, false);
+    return true;
+}
+
+bool Selector::Step() {
+    switch (state) {
+    case Moving:
+        if (mm::motion.QueueEmpty()) {
+            // move finished
+            state = Ready;
+        }
+        return false;
+    case Ready:
+        currentSlot = plannedSlot;
+        return true;
+    case Failed:
+    default:
+        return true;
+    }
+}
+
+} // namespace selector
+} // namespace modules
diff --git a/src/modules/selector.h b/src/modules/selector.h
new file mode 100644
index 0000000..9edf74e
--- /dev/null
+++ b/src/modules/selector.h
@@ -0,0 +1,53 @@
+#pragma once
+#include <stdint.h>
+
+/// Selector model
+/// Handles asynchronnous move operations between filament individual slots
+/// Keeps track of selector's current state
+
+namespace modules {
+namespace selector {
+
+class Selector {
+public:
+    enum {
+        Ready = 0,
+        Moving,
+        Failed
+    };
+
+    // public operations on the selector
+
+    /// @retuns false in case an operation is already underway
+    bool MoveToSlot(uint8_t slot);
+    /// @retuns false in case an operation is already underway
+    bool Home();
+
+    /// @returns true if the selector is ready to accept new commands (i.e. it has finished the last operation)
+    bool Step();
+
+    /// @returns the current slot of selector
+    /// this state is updated only when a planned move is successfully finished, so it is safe for higher-level
+    /// state machines to use this call as a waiting condition for the desired state of the selector
+    inline uint8_t Slot() const { return currentSlot; }
+
+private:
+    constexpr static const uint16_t slotPositions[5] = { 1, 2, 3, 4, 5 }; // @@TODO
+
+    /// internal state of the automaton
+    uint8_t state;
+    uint8_t plannedSlot;
+
+    /// current state
+    uint8_t currentSlot;
+
+    inline Selector()
+        : state(Ready)
+        , currentSlot(0)
+        , plannedSlot(0) {}
+};
+
+extern Selector selector;
+
+} // namespace selector
+} // namespace modules