diff --git a/MM-control-01/main.cpp b/MM-control-01/main.cpp
index 628a19e1..bf5bee58 100644
--- a/MM-control-01/main.cpp
+++ b/MM-control-01/main.cpp
@@ -30,10 +30,48 @@ FILE* uart_com = uart0io;
 FILE* uart_com = uart1io;
 #endif //(UART_COM == 0)
 
-static bool enterSetup = false;
-static bool signalFilament = false;
+namespace
+{
+//! @brief State
+enum class S
+{
+    Idle,
+    Setup,
+    Printing,
+    SignalFilament,
+    Wait,
+    WaitOk,
+};
+}
 
-void process_commands(FILE* inout);
+//! @brief Main MMU state
+//!
+//! @startuml
+//!
+//! title MMU Main State Diagram
+//!
+//! state Any {
+//!   state Idle : Manual extruder selector
+//!   state Setup
+//!   state Printing
+//!   state SignalFilament
+//!
+//!   [*] --> Idle : !MiddleButton
+//!   [*] --> Setup : MiddleButton
+//!   Any --> Printing : T<nr> || Eject
+//!   Any --> Idle : Unload || RecoverEject
+//!   Any --> SignalFilament : Load && filamentLoaded
+//!   Any --> Wait : W0
+//!   Setup --> Idle
+//!   Wait --> Idle : RightButton
+//!   WaitOk --> Idle : RightButton
+//!   Wait --> WaitOk : MiddleButton && mmctl_IsOk
+//!   WaitOk --> Wait : MiddleButton && !mmctl_IsOk
+//! }
+//! @enduml
+static S state;
+
+static void process_commands(FILE* inout);
 
 static void led_blink(int _no)
 {
@@ -70,22 +108,40 @@ static void signal_filament_present()
     delay(300);
 }
 
+void signal_load_failure()
+{
+    shr16_set_led(0x000);
+    delay(800);
+    shr16_set_led(2 << 2 * (4 - active_extruder));
+    delay(800);
+}
+
+void signal_ok_after_load_failure()
+{
+    shr16_set_led(0x000);
+    delay(800);
+    shr16_set_led(1 << 2 * (4 - active_extruder));
+    delay(100);
+    shr16_set_led(2 << 2 * (4 - active_extruder));
+    delay(100);
+    delay(800);
+}
+
 //! @brief Signal filament presence
 //!
-//! Does nothing, when not enabled by signalFilament == true.
-void filament_presence_signaler()
+//! @retval true still present
+//! @retval false not present any more
+bool filament_presence_signaler()
 {
-    if (signalFilament)
+    if (digitalRead(A1) == 1)
     {
-        if (digitalRead(A1) == 1)
-        {
-            signal_filament_present();
-        }
-        else
-        {
-            isFilamentLoaded = false;
-            signalFilament = false;
-        }
+        signal_filament_present();
+        return true;
+    }
+    else
+    {
+        isFilamentLoaded = false;
+        return false;
     }
 }
 
@@ -228,7 +284,7 @@ void setup()
     // check if to goto the settings menu
     if (buttonClicked() == Btn::middle)
     {
-        enterSetup = true;
+        state = S::Setup;
     }
 
     tmc2130_init(HOMING_MODE);
@@ -307,7 +363,7 @@ void manual_extruder_selector()
 
 //! @brief main loop
 //!
-//! It is possible to manually select filament and feed it when not printing.
+//! It is possible to manually select filament and feed it when S::Idle.
 //!
 //! button | action
 //! ------ | ------
@@ -317,28 +373,60 @@ void manual_extruder_selector()
 void loop()
 {
     process_commands(uart_com);
-    filament_presence_signaler();
 
-    if (!isPrinting)
+    switch (state)
     {
-        if (enterSetup)
-        {
-            enterSetup = setupMenu();
-        }
-        else
+    case S::Setup:
+        if (!setupMenu()) state = S::Idle;
+        break;
+    case S::Printing:
+        break;
+    case S::SignalFilament:
+        if (!filament_presence_signaler()) state = S::Idle;
+        break;
+    case S::Idle:
+        manual_extruder_selector();
+        if(Btn::middle == buttonClicked() && active_extruder < 5)
         {
-            manual_extruder_selector();
-            if(Btn::middle == buttonClicked() && active_extruder < 5)
+            shr16_set_led(2 << 2 * (4 - active_extruder));
+            delay(500);
+            if (Btn::middle == buttonClicked())
             {
-                shr16_set_led(2 << 2 * (4 - active_extruder));
-                delay(500);
-                if (Btn::middle == buttonClicked())
-                {
-                    motion_set_idler_selector(active_extruder);
-                    feed_filament();
-                }
+                motion_set_idler_selector(active_extruder);
+                feed_filament();
             }
         }
+        break;
+    case S::Wait:
+        signal_load_failure();
+        switch(buttonClicked())
+        {
+        case Btn::middle:
+            if (mmctl_IsOk()) state = S::WaitOk;
+            break;
+        case Btn::right:
+            state = S::Idle;
+            fprintf_P(uart_com, PSTR("ok\n"));
+            break;
+        default:
+            break;
+        }
+        break;
+    case S::WaitOk:
+        signal_ok_after_load_failure();
+        switch(buttonClicked())
+        {
+        case Btn::middle:
+            if (!mmctl_IsOk()) state = S::Wait;
+            break;
+        case Btn::right:
+            state = S::Idle;
+            fprintf_P(uart_com, PSTR("ok\n"));
+            break;
+        default:
+            break;
+        }
+        break;
     }
 }
 
@@ -378,6 +466,7 @@ void process_commands(FILE* inout)
 		{
 			if ((value >= 0) && (value < EXTRUDERS))
 			{
+			    state = S::Printing;
 				switch_extruder_withSensor(value);
 				fprintf_P(inout, PSTR("ok\n"));
 			}
@@ -387,7 +476,7 @@ void process_commands(FILE* inout)
 		{
 			if ((value >= 0) && (value < EXTRUDERS))
 			{
-			    if (isFilamentLoaded) signalFilament = true;
+			    if (isFilamentLoaded) state = S::SignalFilament;
 			    else
 			    {
                     select_extruder(value);
@@ -417,7 +506,7 @@ void process_commands(FILE* inout)
 			unload_filament_withSensor();
 			fprintf_P(inout, PSTR("ok\n"));
 
-			isPrinting = false;
+			state = S::Idle;
 		}
 		else if (sscanf_P(line, PSTR("X%d"), &value) > 0)
 		{
@@ -464,6 +553,7 @@ void process_commands(FILE* inout)
 			{
 				eject_filament(value);
 				fprintf_P(inout, PSTR("ok\n"));
+				state = S::Printing;
 			}
 		}
 		else if (sscanf_P(line, PSTR("R%d"), &value) > 0)
@@ -472,8 +562,16 @@ void process_commands(FILE* inout)
 			{
 				recover_after_eject();
 				fprintf_P(inout, PSTR("ok\n"));
+				state = S::Idle;
 			}
 		}
+        else if (sscanf_P(line, PSTR("W%d"), &value) > 0)
+        {
+            if (value == 0) //! W0 Wait for user click
+            {
+                state = S::Wait;
+            }
+        }
 	}
 	else
 	{ //nothing received
diff --git a/MM-control-01/main.h b/MM-control-01/main.h
index d1ed6007..c916da45 100644
--- a/MM-control-01/main.h
+++ b/MM-control-01/main.h
@@ -9,7 +9,8 @@ void manual_extruder_selector();
 void unrecoverable_error();
 void drive_error();
 void check_filament_not_present();
-void filament_presence_signaler();
+void signal_load_failure();
+void signal_ok_after_load_failure();
 
 extern uint8_t tmc2130_mode;
 extern FILE* uart_com;
diff --git a/MM-control-01/mmctl.cpp b/MM-control-01/mmctl.cpp
index 9c9f96fd..b491c2fb 100644
--- a/MM-control-01/mmctl.cpp
+++ b/MM-control-01/mmctl.cpp
@@ -17,7 +17,6 @@
 int active_extruder = 0;
 int previous_extruder = -1;
 bool isFilamentLoaded = false;
-bool isPrinting = false;
 
 static const int eject_steps = 2500;
 
@@ -78,8 +77,6 @@ bool feed_filament()
 //! @par new_extruder Filament to be selected
 void switch_extruder_withSensor(int new_extruder)
 {
-	isPrinting = true;
-
 	shr16_set_led(2 << 2 * (4 - active_extruder));
 
 	active_extruder = new_extruder;
@@ -170,7 +167,7 @@ void recover_after_eject()
     tmc2130_disable_axis(AX_PUL, tmc2130_mode);
 }
 
-bool checkOk()
+static bool checkOk()
 {
     bool _ret = false;
     int _steps = 0;
@@ -235,6 +232,21 @@ bool checkOk()
     return _ret;
 }
 
+//! @brief Can FINDA detect filament tip
+//!
+//! Move filament back and forth to align it by FINDA.
+//! @retval true success
+//! @retval false failure
+bool mmctl_IsOk()
+{
+    tmc2130_init_axis(AX_PUL, tmc2130_mode);
+    motion_engage_idler();
+    const bool retval = checkOk();
+    motion_disengage_idler();
+    tmc2130_disable_axis(AX_PUL, tmc2130_mode);
+    return retval;
+}
+
 void load_filament_withSensor()
 {
     FilamentLoaded::set(active_extruder);
@@ -296,20 +308,14 @@ void load_filament_withSensor()
         motion_disengage_idler();
         do
         {
-            shr16_set_led(0x000);
-            delay(800);
             if (!_isOk)
             {
-                shr16_set_led(2 << 2 * (4 - active_extruder));
+                signal_load_failure();
             }
             else
             {
-                shr16_set_led(1 << 2 * (4 - active_extruder));
-                delay(100);
-                shr16_set_led(2 << 2 * (4 - active_extruder));
-                delay(100);
+                signal_ok_after_load_failure();
             }
-            delay(800);
 
             switch (buttonClicked())
             {
@@ -513,7 +519,6 @@ void unload_filament_withSensor()
     motion_disengage_idler();
     tmc2130_disable_axis(AX_PUL, tmc2130_mode);
     isFilamentLoaded = false; // filament unloaded
-    filament_presence_signaler();
 }
 
 //! @brief Do 38.20 mm pulley push
diff --git a/MM-control-01/mmctl.h b/MM-control-01/mmctl.h
index b1b12f14..d8d053e3 100644
--- a/MM-control-01/mmctl.h
+++ b/MM-control-01/mmctl.h
@@ -6,7 +6,6 @@
 
 #include <inttypes.h>
 
-extern bool isPrinting;
 extern int active_extruder;
 extern int previous_extruder;
 extern bool isFilamentLoaded;
@@ -19,5 +18,6 @@ void load_filament_inPrinter();
 void unload_filament_withSensor();
 void eject_filament(uint8_t filament);
 void recover_after_eject();
+bool mmctl_IsOk();
 
 #endif //_MMCTL_H
diff --git a/MM-control-01/version.h.in b/MM-control-01/version.h.in
index fb81a1ff..122849ce 100644
--- a/MM-control-01/version.h.in
+++ b/MM-control-01/version.h.in
@@ -3,7 +3,7 @@
 #ifndef VERSION_H_
 #define VERSION_H_
 
-static const uint16_t fw_version = 104; //!< example: 103 means version 1.0.3
+static const uint16_t fw_version = 105; //!< example: 103 means version 1.0.3
 static const uint16_t fw_buildnr = ${GIT_PARENT_COMMITS}; //!< number of commits preceeding current HEAD
 #define FW_HASH "${GIT_COMMIT_HASH}"