From 69d4132415a1f151a1b4afe2cf5563361fc82e45 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Daniel=20Cant=C3=B3=20Catal=C3=A1n?=
 <144663567+Cantonplas@users.noreply.github.com>
Date: Wed, 11 Feb 2026 18:56:45 +0100
Subject: [PATCH 1/4] Add nested state machine start functionality

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index 519c4578c..bac4e5bad 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -415,6 +415,13 @@ class StateMachine : public IStateMachine {
     void start()
     {
       enter();
+      for(auto& nested : nested_state_machine)
+        {
+          if(nested.state == current_state){
+            nested.machine->start();
+            break;
+          }
+        }
     }
 
 

From 04995515542ee7bcb15b41ddba51e7c7f33ab05f Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Wed, 11 Feb 2026 19:21:47 +0100
Subject: [PATCH 2/4] Now nested state machine works

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 37 ++++++++++++++++++--
 1 file changed, 35 insertions(+), 2 deletions(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index bac4e5bad..5fbd2e6be 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -259,6 +259,11 @@ class IStateMachine {
     virtual void force_change_state(size_t state) = 0;
     virtual size_t get_current_state_id() const = 0;
     constexpr bool operator==(const IStateMachine&) const = default;
+    protected:
+    virtual void enter() = 0;
+    virtual void exit() = 0;
+    template <class E, size_t N, size_t T>
+    friend class StateMachine;
 };
 
 template <class StateEnum, size_t NStates, size_t NTransitions>
@@ -327,14 +332,14 @@ class StateMachine : public IStateMachine {
             state.get_transitions().size()};
   }
 
-  inline void enter() 
+  inline void enter() override 
   { 
       auto& state = states[static_cast<size_t>(current_state)];
       state.enter();
       
   }
 
-  inline void exit() 
+  inline void exit() override
   {
       auto& state = states[static_cast<size_t>(current_state)];
       state.exit();
@@ -391,11 +396,25 @@ class StateMachine : public IStateMachine {
             if (t.predicate()) 
             {
                 exit();
+                for(auto& nested : nested_state_machine)
+                {
+                  if(nested.state == current_state){
+                    nested.machine->exit();
+                    break;
+                  }
+                }
                 #ifdef STLIB_ETH
                 remove_state_orders();
                 #endif
                 current_state = t.target;
                 enter();
+                for(auto& nested : nested_state_machine)
+                {
+                  if(nested.state == current_state){
+                    nested.machine->enter();
+                    break;
+                  }
+                }
                 #ifdef STLIB_ETH
                 refresh_state_orders();
                 #endif
@@ -435,11 +454,25 @@ class StateMachine : public IStateMachine {
       }
         
         exit();
+          for(auto& nested : nested_state_machine)
+          {
+            if(nested.state == current_state){
+              nested.machine->exit();
+              break;
+            }
+          }
         #ifdef STLIB_ETH
         remove_state_orders();
         #endif
         current_state = new_state;
         enter();
+          for(auto& nested : nested_state_machine)
+          {
+            if(nested.state == current_state){
+              nested.machine->enter();
+              break;
+            }
+          }
         #ifdef STLIB_ETH
         refresh_state_orders();
         #endif

From 19b5ae3e67287f1c9f3c598122ab4bb4cd270f89 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Thu, 12 Feb 2026 18:49:25 +0100
Subject: [PATCH 3/4] Make IStateMachine friends with StateMachine :3

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index 5fbd2e6be..77e68ccd5 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -262,6 +262,7 @@ class IStateMachine {
     protected:
     virtual void enter() = 0;
     virtual void exit() = 0;
+    virtual void start() = 0;
     template <class E, size_t N, size_t T>
     friend class StateMachine;
 };
@@ -431,7 +432,7 @@ class StateMachine : public IStateMachine {
         }
     }
 
-    void start()
+    void start() override;
     {
       enter();
       for(auto& nested : nested_state_machine)

From 731fa69666a180d992063bfda0141b11805ebfe7 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Thu, 12 Feb 2026 18:53:19 +0100
Subject: [PATCH 4/4] Make IStateMachine friends with StateMachine :3

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index 77e68ccd5..7ae89590b 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -432,7 +432,7 @@ class StateMachine : public IStateMachine {
         }
     }
 
-    void start() override;
+    void start() override
     {
       enter();
       for(auto& nested : nested_state_machine)