From ddb56cd1226d3b468634f6ae452fd54634d576b7 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 13:30:51 +0100
Subject: [PATCH 01/14] No more fixed_vector(idk why I did that)

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 18 ++++++++----------
 1 file changed, 8 insertions(+), 10 deletions(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index 0e038f57..aaff84e0 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -19,8 +19,6 @@ using ms = std::chrono::milliseconds;
 using us = std::chrono::microseconds;
 using s = std::chrono::seconds;
 
-template <typename T, size_t Capacity> using FixedVector = StaticVector<T, Capacity>;
-
 template <class StateEnum>
 concept IsEnum = std::is_enum_v<StateEnum>;
 
@@ -58,14 +56,14 @@ concept are_transitions = (std::same_as<T, Transition<StateEnum>> && ...);
 
 template <IsEnum StateEnum, size_t NTransitions, size_t Number_of_state_orders = 0> class State {
 private:
-    FixedVector<TimedAction, NUMBER_OF_ACTIONS> cyclic_actions = {};
-    FixedVector<Callback, NUMBER_OF_ACTIONS> on_enter_actions = {};
-    FixedVector<Callback, NUMBER_OF_ACTIONS> on_exit_actions = {};
+    StaticVector<TimedAction, NUMBER_OF_ACTIONS> cyclic_actions = {};
+    StaticVector<Callback, NUMBER_OF_ACTIONS> on_enter_actions = {};
+    StaticVector<Callback, NUMBER_OF_ACTIONS> on_exit_actions = {};
     StateEnum state = {};
-    FixedVector<Transition<StateEnum>, NTransitions> transitions = {};
+    StaticVector<Transition<StateEnum>, NTransitions> transitions = {};
 
 public:
-    [[no_unique_address]] FixedVector<uint16_t, Number_of_state_orders> state_orders_ids = {};
+    [[no_unique_address]] StaticVector<uint16_t, Number_of_state_orders> state_orders_ids = {};
     static constexpr size_t transition_count = NTransitions;
 
     template <typename... T>
@@ -285,10 +283,10 @@ class StateMachine : public IStateMachine {
     void set_on(bool is_on) override { this->is_on = is_on; }
 
 private:
-    FixedVector<State<StateEnum, NTransitions>, NStates> states;
-    FixedVector<Transition<StateEnum>, NTransitions> transitions = {};
+    StaticVector<State<StateEnum, NTransitions>, NStates> states;
+    StaticVector<Transition<StateEnum>, NTransitions> transitions = {};
     std::array<std::pair<size_t, size_t>, NStates> transitions_assoc = {};
-    FixedVector<NestedPair, NStates> nested_state_machine = {};
+    StaticVector<NestedPair, NStates> nested_state_machine = {};
 
     constexpr bool operator==(const StateMachine&) const = default;
 

From a29f9e44329567f7ea3bf856cb6c14f3b6747c6c Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 14:34:28 +0100
Subject: [PATCH 02/14] First version implementing std::tupple

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 212 +++++++++++--------
 1 file changed, 118 insertions(+), 94 deletions(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index aaff84e0..f4b756d3 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -10,6 +10,7 @@
 #include <utility>
 #include <unordered_map>
 #include <algorithm>
+#include <tuple>
 
 #ifdef STLIB_ETH
 #include "StateMachine/StateOrder.hpp"
@@ -244,39 +245,65 @@ class IStateMachine {
     virtual void enter() = 0;
     virtual void exit() = 0;
     virtual void start() = 0;
-    template <class E, size_t N, size_t T> friend class StateMachine;
+    template <class E, size_t N, size_t T, class... NestedSMType> friend class StateMachine;
 };
 
-template <class StateEnum, size_t NStates, size_t NTransitions>
+template <class StateEnum, class NestedSMType>
+struct NestedMachineBinding {
+    StateEnum state;
+    NestedSMType* machine;
+};
+
+template <class StateEnum, class NestedSMType, size_t N, size_t O>
+constexpr auto bind_nested_machine(NestedSMType& machine, const State<StateEnum, N, O>& state) {
+    return NestedMachineBinding<StateEnum, NestedSMType>{state.get_state(), &machine};
+}
+
+template <class StateEnum, size_t NStates, size_t NTransitions, class... NestedMachines>
 class StateMachine : public IStateMachine {
 private:
-    struct NestedPair {
-        StateEnum state;
-        IStateMachine* machine;
-        constexpr bool operator==(const NestedPair&) const = default;
-    };
-
     StateEnum current_state;
+    std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...> nested_machines;
 
-public:
-    constexpr ~StateMachine() override = default;
-
-    void force_change_state(size_t state) override {
-        StateEnum new_state = static_cast<StateEnum>(state);
+    void perform_state_change(StateEnum new_state) {
         if (current_state == new_state) {
             return;
         }
+
+        exit();
+        std::apply([this](auto&... nested) {
+            (..., [&] {
+                if (nested.state == this->current_state && nested.machine != nullptr) {
+                    nested.machine->exit();
+                }
+            }());
+        }, nested_machines);
+
 #ifdef STLIB_ETH
         remove_state_orders();
 #endif
-        exit();
         current_state = new_state;
         enter();
+        std::apply([this](auto&... nested) {
+            (..., [&] {
+                if (nested.state == this->current_state && nested.machine != nullptr) {
+                    nested.machine->enter();
+                }
+            }());
+        }, nested_machines);
+
 #ifdef STLIB_ETH
         refresh_state_orders();
 #endif
     }
 
+public:
+    constexpr ~StateMachine() override = default;
+
+    void force_change_state(size_t state) override {
+        perform_state_change(static_cast<StateEnum>(state));
+    }
+
     size_t get_current_state_id() const override { return static_cast<size_t>(current_state); }
 
     bool is_on = true;
@@ -286,7 +313,6 @@ class StateMachine : public IStateMachine {
     StaticVector<State<StateEnum, NTransitions>, NStates> states;
     StaticVector<Transition<StateEnum>, NTransitions> transitions = {};
     std::array<std::pair<size_t, size_t>, NStates> transitions_assoc = {};
-    StaticVector<NestedPair, NStates> nested_state_machine = {};
 
     constexpr bool operator==(const StateMachine&) const = default;
 
@@ -313,13 +339,16 @@ class StateMachine : public IStateMachine {
     }
 
 public:
+    // Constructor principal que toma la tupla de nested machines
     template <IsState<StateEnum>... S>
-    consteval StateMachine(StateEnum initial_state, S... states) : current_state(initial_state) {
-        // Sort states by their enum value
+    consteval StateMachine(StateEnum initial_state, const std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...>& nested_machines_tuple, S... states_input) : 
+        current_state(initial_state), 
+        nested_machines(nested_machines_tuple) {
+        
         using StateType = State<StateEnum, NTransitions>;
         std::array<StateType, sizeof...(S)> sorted_states;
         size_t index = 0;
-        ((sorted_states[index++] = StateType(states)), ...);
+        ((sorted_states[index++] = StateType(states_input)), ...);
 
         for (size_t i = 0; i < sorted_states.size(); i++) {
             for (size_t j = 0; j < sorted_states.size() - 1; j++) {
@@ -349,81 +378,69 @@ class StateMachine : public IStateMachine {
         }
     }
 
+    // Constructor de copia necesario para devolver por valor en add_state_machine
+    template <class... OtherNestedMachines>
+    constexpr StateMachine(
+        StateEnum current_state_arg,
+        const StaticVector<State<StateEnum, NTransitions>, NStates>& states_arg,
+        const StaticVector<Transition<StateEnum>, NTransitions>& transitions_arg,
+        const std::array<std::pair<size_t, size_t>, NStates>& transitions_assoc_arg,
+        const std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...>& nested_machines_arg
+    ) :
+        current_state(current_state_arg),
+        nested_machines(nested_machines_arg),
+        states(states_arg),
+        transitions(transitions_arg),
+        transitions_assoc(transitions_assoc_arg)
+    {}
+
+    template <size_t N, size_t O, class NewNestedMachine>
+    constexpr auto add_state_machine(NewNestedMachine& machine, const State<StateEnum, N, O>& state) const {
+        auto new_binding = NestedMachineBinding<StateEnum, NewNestedMachine>{state.get_state(), &machine};
+        auto new_nested_tuple = std::tuple_cat(nested_machines, std::make_tuple(new_binding));
+        
+        return StateMachine<StateEnum, NStates, NTransitions, NestedMachines..., NewNestedMachine>(
+            current_state,
+            states,
+            transitions,
+            transitions_assoc,
+            new_nested_tuple
+        );
+    }
+
     void check_transitions() override {
         auto& [i, n] = transitions_assoc[static_cast<size_t>(current_state)];
+
         for (auto index = i; index < i + n; ++index) {
             const auto& t = transitions[index];
             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
-                break;
+                perform_state_change(t.target);
+                return;
             }
         }
 
-        for (auto& nested : nested_state_machine) {
-            if (nested.state == current_state) {
-                nested.machine->check_transitions();
-                break;
-            }
-        }
+        std::apply([this](auto&... nested) {
+            (..., [&] {
+                if (nested.state == this->current_state && nested.machine != nullptr) {
+                    nested.machine->check_transitions();
+                }
+            }());
+        }, nested_machines);
     }
 
     void start() override {
         enter();
-        for (auto& nested : nested_state_machine) {
-            if (nested.state == current_state) {
-                nested.machine->start();
-                break;
-            }
-        }
+        std::apply([this](auto&... nested) {
+            (..., [&] {
+                if (nested.state == this->current_state && nested.machine != nullptr) {
+                    nested.machine->start();
+                }
+            }());
+        }, nested_machines);
     }
 
     template <size_t N, size_t O> void force_change_state(const State<StateEnum, N, O>& state) {
-        StateEnum new_state = state.get_state();
-        if (current_state == new_state) {
-            return;
-        }
-
-        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
+        perform_state_change(state.get_state());
     }
 
     template <ValidTime TimeUnit, size_t N, size_t O>
@@ -471,22 +488,6 @@ class StateMachine : public IStateMachine {
         ErrorHandler("Error: The state is not added to the state machine");
     }
 
-    template <size_t N, size_t O>
-    constexpr void
-    add_state_machine(IStateMachine& state_machine, const State<StateEnum, N, O>& state) {
-        for (auto& nested : nested_state_machine) {
-            if (nested.state == state.get_state()) {
-                ErrorHandler(
-                    "Only one Nested State Machine can be added per state, tried to add to state: "
-                    "%d",
-                    static_cast<int>(state.get_state())
-                );
-                return;
-            }
-        }
-        nested_state_machine.push_back({state.get_state(), &state_machine});
-    }
-
     StateEnum get_current_state() const { return current_state; }
 
     inline void refresh_state_orders() {
@@ -550,6 +551,29 @@ consteval auto make_state_machine(StateEnum initial_state, States... states) {
 
     return StateMachine<StateEnum, number_of_states, number_of_transitions>(
         initial_state,
+        std::tuple<>{},
+        states...
+    );
+}
+
+template <typename StateEnum, typename... NestedMachines, typename... States>
+    requires are_states<StateEnum, States...>
+consteval auto make_state_machine(
+    StateEnum initial_state,
+    std::tuple<NestedMachines...> nested_machines,
+    States... states
+) {
+    constexpr size_t number_of_states = sizeof...(states);
+    constexpr size_t number_of_transitions =
+        (std::remove_reference_t<States>::transition_count + ... + 0);
+
+    return StateMachine<
+        StateEnum,
+        number_of_states,
+        number_of_transitions,
+        typename std::remove_pointer<decltype(std::declval<NestedMachines>().machine)>::type...>(
+        initial_state,
+        nested_machines,
         states...
     );
 }

From 9b3561fbfd07b13d56061da57462dee73fd0c8fd Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 14:46:11 +0100
Subject: [PATCH 03/14] No more copy constructor bullshit

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 30 --------------------
 1 file changed, 30 deletions(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index f4b756d3..f7112f60 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -378,36 +378,6 @@ class StateMachine : public IStateMachine {
         }
     }
 
-    // Constructor de copia necesario para devolver por valor en add_state_machine
-    template <class... OtherNestedMachines>
-    constexpr StateMachine(
-        StateEnum current_state_arg,
-        const StaticVector<State<StateEnum, NTransitions>, NStates>& states_arg,
-        const StaticVector<Transition<StateEnum>, NTransitions>& transitions_arg,
-        const std::array<std::pair<size_t, size_t>, NStates>& transitions_assoc_arg,
-        const std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...>& nested_machines_arg
-    ) :
-        current_state(current_state_arg),
-        nested_machines(nested_machines_arg),
-        states(states_arg),
-        transitions(transitions_arg),
-        transitions_assoc(transitions_assoc_arg)
-    {}
-
-    template <size_t N, size_t O, class NewNestedMachine>
-    constexpr auto add_state_machine(NewNestedMachine& machine, const State<StateEnum, N, O>& state) const {
-        auto new_binding = NestedMachineBinding<StateEnum, NewNestedMachine>{state.get_state(), &machine};
-        auto new_nested_tuple = std::tuple_cat(nested_machines, std::make_tuple(new_binding));
-        
-        return StateMachine<StateEnum, NStates, NTransitions, NestedMachines..., NewNestedMachine>(
-            current_state,
-            states,
-            transitions,
-            transitions_assoc,
-            new_nested_tuple
-        );
-    }
-
     void check_transitions() override {
         auto& [i, n] = transitions_assoc[static_cast<size_t>(current_state)];
 

From 8c0e35a055979b57da06cd1bfae261fd525b64ec Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 16:54:51 +0100
Subject: [PATCH 04/14] Equal operator fixed, now it compiles

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

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index f7112f60..78e4dd6a 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -252,6 +252,8 @@ template <class StateEnum, class NestedSMType>
 struct NestedMachineBinding {
     StateEnum state;
     NestedSMType* machine;
+    
+    constexpr bool operator==(const NestedMachineBinding&) const = default;
 };
 
 template <class StateEnum, class NestedSMType, size_t N, size_t O>
@@ -261,6 +263,7 @@ constexpr auto bind_nested_machine(NestedSMType& machine, const State<StateEnum,
 
 template <class StateEnum, size_t NStates, size_t NTransitions, class... NestedMachines>
 class StateMachine : public IStateMachine {
+    template <class E, size_t N, size_t T, class... Nested> friend class StateMachine;
 private:
     StateEnum current_state;
     std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...> nested_machines;

From 01bd9699f8a9e8f94af38a18b71c4cc298bf6fb0 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 17:48:09 +0100
Subject: [PATCH 05/14] Compiling version, tests to be done

---
 .../StateMachine/HeapStateOrder.hpp           |  4 +-
 .../StateMachine/StackStateOrder.hpp          |  4 +-
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp  | 88 ++++++++++++++-----
 3 files changed, 69 insertions(+), 27 deletions(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp
index 6d71b8d7..a1d5dd84 100644
--- a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp
@@ -24,13 +24,13 @@ template <class StateMachineType> class HeapStateOrder : public HeapOrder {
     }
 
     void process() override {
-        if (callback != nullptr && state_machine.is_on &&
+        if (callback != nullptr &&
             state_machine.get_current_state_id() == state)
             callback();
     }
 
     void parse(OrderProtocol* socket, uint8_t* data) override {
-        if (state_machine.is_on && state_machine.get_current_state_id() == state)
+        if (state_machine.get_current_state_id() == state)
             HeapOrder::parse(data);
     }
 };
diff --git a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp
index 83cffb9c..a10de34b 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp
@@ -25,13 +25,13 @@ class StackStateOrder : public StackOrder<BufferLength, Types...> {
     }
 
     void process() override {
-        if (this->callback != nullptr && state_machine.is_on &&
+        if (this->callback != nullptr &&
             state_machine.get_current_state_id() == state)
             this->callback();
     }
 
     void parse(OrderProtocol* socket, uint8_t* data) override {
-        if (state_machine.is_on && state_machine.get_current_state_id() == state)
+        if (state_machine.get_current_state_id() == state)
             StackOrder<BufferLength, Types...>::parse(data);
     }
 };
diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index 78e4dd6a..9739bec6 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -69,8 +69,10 @@ template <IsEnum StateEnum, size_t NTransitions, size_t Number_of_state_orders =
 
     template <typename... T>
         requires are_transitions<StateEnum, T...>
-    consteval State(StateEnum state, T... transitions) : state(state) {
-        (this->transitions.push_back(transitions), ...);
+    consteval State(StateEnum state, T... transitions) : state(state) {        
+        if (((transitions.target == state) || ...)) {
+            ErrorHandler("Current state cannot be the target of a transition");
+        }        (this->transitions.push_back(transitions), ...);
     }
 
     consteval State() = default;
@@ -193,8 +195,10 @@ template <IsEnum StateEnum, size_t NTransitions, size_t Number_of_state_orders =
         Scheduler::unregister_task(timed_action->id);
         timed_action->is_on = false;
     }
-
+    
+#ifdef STLIB_ETH
     constexpr void add_state_order(uint16_t id) { state_orders_ids.push_back(id); }
+#endif
 
     template <ValidTime TimeUnit>
     consteval TimedAction* add_cyclic_action(Callback action, TimeUnit period) {
@@ -230,13 +234,24 @@ concept IsState = is_state<T, StateEnum>::value;
 template <class StateEnum, typename... Ts>
 concept are_states = (IsState<Ts, StateEnum> && ...);
 
+template <class StateEnum, size_t NStates, size_t NTransitions, class... NestedMachines>
+class StateMachine;
+
+template <typename T>
+struct is_state_machine : std::false_type {};
+
+template <class StateEnum, size_t NStates, size_t NTransitions, class... NestedMachines>
+struct is_state_machine<StateMachine<StateEnum, NStates, NTransitions, NestedMachines...>> : std::true_type {};
+
+template <typename T>
+concept IsStateMachineClass = is_state_machine<std::remove_cvref_t<T>>::value;
+
 /// Interface for State Machines to allow other classes to interact with the state machine without
 /// knowing its implementation
 class IStateMachine {
 public:
     virtual constexpr ~IStateMachine() = default;
     virtual void check_transitions() = 0;
-    virtual void set_on(bool is_on) = 0;
     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;
@@ -248,7 +263,7 @@ class IStateMachine {
     template <class E, size_t N, size_t T, class... NestedSMType> friend class StateMachine;
 };
 
-template <class StateEnum, class NestedSMType>
+template <class StateEnum, IsStateMachineClass NestedSMType>
 struct NestedMachineBinding {
     StateEnum state;
     NestedSMType* machine;
@@ -256,15 +271,38 @@ struct NestedMachineBinding {
     constexpr bool operator==(const NestedMachineBinding&) const = default;
 };
 
-template <class StateEnum, class NestedSMType, size_t N, size_t O>
-constexpr auto bind_nested_machine(NestedSMType& machine, const State<StateEnum, N, O>& state) {
+template <typename T>
+struct is_nested_machine_binding : std::false_type {};
+
+template <class StateEnum, class NestedSMType>
+struct is_nested_machine_binding<NestedMachineBinding<StateEnum, NestedSMType>> : std::true_type {};
+
+template <typename T>
+concept IsNestedMachineBinding = is_nested_machine_binding<T>::value;
+
+namespace StateMachineHelper {
+
+template <class StateEnum, IsStateMachineClass NestedSMType, size_t N, size_t O>
+static constexpr auto add_nesting(const State<StateEnum, N, O>& state, NestedSMType& machine) {
     return NestedMachineBinding<StateEnum, NestedSMType>{state.get_state(), &machine};
 }
 
+template <typename... Bindings>
+   requires (IsNestedMachineBinding<Bindings> && ...)
+static constexpr auto add_nested_machines(Bindings... bindings) {
+    return std::make_tuple(bindings...);
+}
+
+}
+
 template <class StateEnum, size_t NStates, size_t NTransitions, class... NestedMachines>
 class StateMachine : public IStateMachine {
+    static_assert((IsEnum<StateEnum>), "StateEnum must be an enum type");
+    static_assert((IsStateMachineClass<NestedMachines> && ...), "All nested machines must be of type StateMachine");
+
+
     template <class E, size_t N, size_t T, class... Nested> friend class StateMachine;
-private:
+
     StateEnum current_state;
     std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...> nested_machines;
 
@@ -300,18 +338,6 @@ class StateMachine : public IStateMachine {
 #endif
     }
 
-public:
-    constexpr ~StateMachine() override = default;
-
-    void force_change_state(size_t state) override {
-        perform_state_change(static_cast<StateEnum>(state));
-    }
-
-    size_t get_current_state_id() const override { return static_cast<size_t>(current_state); }
-
-    bool is_on = true;
-    void set_on(bool is_on) override { this->is_on = is_on; }
-
 private:
     StaticVector<State<StateEnum, NTransitions>, NStates> states;
     StaticVector<Transition<StateEnum>, NTransitions> transitions = {};
@@ -342,7 +368,6 @@ class StateMachine : public IStateMachine {
     }
 
 public:
-    // Constructor principal que toma la tupla de nested machines
     template <IsState<StateEnum>... S>
     consteval StateMachine(StateEnum initial_state, const std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...>& nested_machines_tuple, S... states_input) : 
         current_state(initial_state), 
@@ -366,7 +391,7 @@ class StateMachine : public IStateMachine {
         // Check that states are contiguous and start from 0
         for (size_t i = 0; i < sorted_states.size(); i++) {
             if (static_cast<size_t>(sorted_states[i].get_state()) != i) {
-                ErrorHandler("States Enum must be contiguous and start from 0");
+                ErrorHandler("States Enum must be contiguous and start from 0, with no duplicates");
             }
         }
 
@@ -380,6 +405,8 @@ class StateMachine : public IStateMachine {
             offset += s.get_transitions().size();
         }
     }
+    constexpr ~StateMachine() override = default;
+
 
     void check_transitions() override {
         auto& [i, n] = transitions_assoc[static_cast<size_t>(current_state)];
@@ -412,6 +439,13 @@ class StateMachine : public IStateMachine {
         }, nested_machines);
     }
 
+    void force_change_state(size_t state) override {
+        perform_state_change(static_cast<StateEnum>(state));
+    }
+
+    size_t get_current_state_id() const override { return static_cast<size_t>(current_state); }
+
+
     template <size_t N, size_t O> void force_change_state(const State<StateEnum, N, O>& state) {
         perform_state_change(state.get_state());
     }
@@ -528,8 +562,16 @@ consteval auto make_state_machine(StateEnum initial_state, States... states) {
         states...
     );
 }
+/* @brief Helper function to create a StateMachine instance
+ *
+ * @tparam States Variadic template parameter pack representing the states
+ * @param initial_state The initial state enum value
+ * @tparam nested_machines Tuple of NestedMachineBinding representing the nested state machines to its corresponding state
+ * @param states The states to be included in the state machine
+ * @return A StateMachine instance initialized with the provided initial state and states, as well as the nested state machines
+ */
 
-template <typename StateEnum, typename... NestedMachines, typename... States>
+template <IsEnum StateEnum, typename... NestedMachines, typename... States>
     requires are_states<StateEnum, States...>
 consteval auto make_state_machine(
     StateEnum initial_state,

From 5e336e5bcbfe330b4ddf02172a785c0b5ec8f366 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 18:10:41 +0100
Subject: [PATCH 06/14] Okay now we check for duplicates

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

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index 9739bec6..e666a599 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -388,10 +388,19 @@ class StateMachine : public IStateMachine {
             }
         }
 
+        // Check for duplicate states
+        for (size_t i = 0; i < sorted_states.size() - 1; i++) {
+            for(size_t j = i + 1; j < sorted_states.size(); j++) {
+                if (sorted_states[i].get_state() == sorted_states[j].get_state()) {
+                    ErrorHandler("Duplicate state found in StateMachine constructor");
+                }
+            }
+        }
+
         // Check that states are contiguous and start from 0
         for (size_t i = 0; i < sorted_states.size(); i++) {
             if (static_cast<size_t>(sorted_states[i].get_state()) != i) {
-                ErrorHandler("States Enum must be contiguous and start from 0, with no duplicates");
+                ErrorHandler("States Enum must be contiguous and start from 0");
             }
         }
 

From e8483e83927fdcfd302ba6bad9e4e683a7e05650 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 18:14:45 +0100
Subject: [PATCH 07/14] Some formating changes

---
 .../StateMachine/HeapStateOrder.hpp           |   3 +-
 .../StateMachine/StackStateOrder.hpp          |   3 +-
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp  | 120 ++++++++++--------
 3 files changed, 70 insertions(+), 56 deletions(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp
index a1d5dd84..502ee577 100644
--- a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp
@@ -24,8 +24,7 @@ template <class StateMachineType> class HeapStateOrder : public HeapOrder {
     }
 
     void process() override {
-        if (callback != nullptr &&
-            state_machine.get_current_state_id() == state)
+        if (callback != nullptr && state_machine.get_current_state_id() == state)
             callback();
     }
 
diff --git a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp
index a10de34b..98c6d6d5 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp
@@ -25,8 +25,7 @@ class StackStateOrder : public StackOrder<BufferLength, Types...> {
     }
 
     void process() override {
-        if (this->callback != nullptr &&
-            state_machine.get_current_state_id() == state)
+        if (this->callback != nullptr && state_machine.get_current_state_id() == state)
             this->callback();
     }
 
diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index e666a599..c8a8638d 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -69,10 +69,11 @@ template <IsEnum StateEnum, size_t NTransitions, size_t Number_of_state_orders =
 
     template <typename... T>
         requires are_transitions<StateEnum, T...>
-    consteval State(StateEnum state, T... transitions) : state(state) {        
+    consteval State(StateEnum state, T... transitions) : state(state) {
         if (((transitions.target == state) || ...)) {
             ErrorHandler("Current state cannot be the target of a transition");
-        }        (this->transitions.push_back(transitions), ...);
+        }
+        (this->transitions.push_back(transitions), ...);
     }
 
     consteval State() = default;
@@ -195,7 +196,7 @@ template <IsEnum StateEnum, size_t NTransitions, size_t Number_of_state_orders =
         Scheduler::unregister_task(timed_action->id);
         timed_action->is_on = false;
     }
-    
+
 #ifdef STLIB_ETH
     constexpr void add_state_order(uint16_t id) { state_orders_ids.push_back(id); }
 #endif
@@ -237,11 +238,11 @@ concept are_states = (IsState<Ts, StateEnum> && ...);
 template <class StateEnum, size_t NStates, size_t NTransitions, class... NestedMachines>
 class StateMachine;
 
-template <typename T>
-struct is_state_machine : std::false_type {};
+template <typename T> struct is_state_machine : std::false_type {};
 
 template <class StateEnum, size_t NStates, size_t NTransitions, class... NestedMachines>
-struct is_state_machine<StateMachine<StateEnum, NStates, NTransitions, NestedMachines...>> : std::true_type {};
+struct is_state_machine<StateMachine<StateEnum, NStates, NTransitions, NestedMachines...>>
+    : std::true_type {};
 
 template <typename T>
 concept IsStateMachineClass = is_state_machine<std::remove_cvref_t<T>>::value;
@@ -263,16 +264,14 @@ class IStateMachine {
     template <class E, size_t N, size_t T, class... NestedSMType> friend class StateMachine;
 };
 
-template <class StateEnum, IsStateMachineClass NestedSMType>
-struct NestedMachineBinding {
+template <class StateEnum, IsStateMachineClass NestedSMType> struct NestedMachineBinding {
     StateEnum state;
     NestedSMType* machine;
-    
+
     constexpr bool operator==(const NestedMachineBinding&) const = default;
 };
 
-template <typename T>
-struct is_nested_machine_binding : std::false_type {};
+template <typename T> struct is_nested_machine_binding : std::false_type {};
 
 template <class StateEnum, class NestedSMType>
 struct is_nested_machine_binding<NestedMachineBinding<StateEnum, NestedSMType>> : std::true_type {};
@@ -288,18 +287,20 @@ static constexpr auto add_nesting(const State<StateEnum, N, O>& state, NestedSMT
 }
 
 template <typename... Bindings>
-   requires (IsNestedMachineBinding<Bindings> && ...)
+    requires(IsNestedMachineBinding<Bindings> && ...)
 static constexpr auto add_nested_machines(Bindings... bindings) {
     return std::make_tuple(bindings...);
 }
 
-}
+} // namespace StateMachineHelper
 
 template <class StateEnum, size_t NStates, size_t NTransitions, class... NestedMachines>
 class StateMachine : public IStateMachine {
     static_assert((IsEnum<StateEnum>), "StateEnum must be an enum type");
-    static_assert((IsStateMachineClass<NestedMachines> && ...), "All nested machines must be of type StateMachine");
-
+    static_assert(
+        (IsStateMachineClass<NestedMachines> && ...),
+        "All nested machines must be of type StateMachine"
+    );
 
     template <class E, size_t N, size_t T, class... Nested> friend class StateMachine;
 
@@ -312,26 +313,32 @@ class StateMachine : public IStateMachine {
         }
 
         exit();
-        std::apply([this](auto&... nested) {
-            (..., [&] {
-                if (nested.state == this->current_state && nested.machine != nullptr) {
-                    nested.machine->exit();
-                }
-            }());
-        }, nested_machines);
+        std::apply(
+            [this](auto&... nested) {
+                (..., [&] {
+                    if (nested.state == this->current_state && nested.machine != nullptr) {
+                        nested.machine->exit();
+                    }
+                }());
+            },
+            nested_machines
+        );
 
 #ifdef STLIB_ETH
         remove_state_orders();
 #endif
         current_state = new_state;
         enter();
-        std::apply([this](auto&... nested) {
-            (..., [&] {
-                if (nested.state == this->current_state && nested.machine != nullptr) {
-                    nested.machine->enter();
-                }
-            }());
-        }, nested_machines);
+        std::apply(
+            [this](auto&... nested) {
+                (..., [&] {
+                    if (nested.state == this->current_state && nested.machine != nullptr) {
+                        nested.machine->enter();
+                    }
+                }());
+            },
+            nested_machines
+        );
 
 #ifdef STLIB_ETH
         refresh_state_orders();
@@ -369,10 +376,13 @@ class StateMachine : public IStateMachine {
 
 public:
     template <IsState<StateEnum>... S>
-    consteval StateMachine(StateEnum initial_state, const std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...>& nested_machines_tuple, S... states_input) : 
-        current_state(initial_state), 
-        nested_machines(nested_machines_tuple) {
-        
+    consteval StateMachine(
+        StateEnum initial_state,
+        const std::tuple<NestedMachineBinding<StateEnum, NestedMachines>...>& nested_machines_tuple,
+        S... states_input
+    )
+        : current_state(initial_state), nested_machines(nested_machines_tuple) {
+
         using StateType = State<StateEnum, NTransitions>;
         std::array<StateType, sizeof...(S)> sorted_states;
         size_t index = 0;
@@ -390,7 +400,7 @@ class StateMachine : public IStateMachine {
 
         // Check for duplicate states
         for (size_t i = 0; i < sorted_states.size() - 1; i++) {
-            for(size_t j = i + 1; j < sorted_states.size(); j++) {
+            for (size_t j = i + 1; j < sorted_states.size(); j++) {
                 if (sorted_states[i].get_state() == sorted_states[j].get_state()) {
                     ErrorHandler("Duplicate state found in StateMachine constructor");
                 }
@@ -416,7 +426,6 @@ class StateMachine : public IStateMachine {
     }
     constexpr ~StateMachine() override = default;
 
-
     void check_transitions() override {
         auto& [i, n] = transitions_assoc[static_cast<size_t>(current_state)];
 
@@ -428,24 +437,30 @@ class StateMachine : public IStateMachine {
             }
         }
 
-        std::apply([this](auto&... nested) {
-            (..., [&] {
-                if (nested.state == this->current_state && nested.machine != nullptr) {
-                    nested.machine->check_transitions();
-                }
-            }());
-        }, nested_machines);
+        std::apply(
+            [this](auto&... nested) {
+                (..., [&] {
+                    if (nested.state == this->current_state && nested.machine != nullptr) {
+                        nested.machine->check_transitions();
+                    }
+                }());
+            },
+            nested_machines
+        );
     }
 
     void start() override {
         enter();
-        std::apply([this](auto&... nested) {
-            (..., [&] {
-                if (nested.state == this->current_state && nested.machine != nullptr) {
-                    nested.machine->start();
-                }
-            }());
-        }, nested_machines);
+        std::apply(
+            [this](auto&... nested) {
+                (..., [&] {
+                    if (nested.state == this->current_state && nested.machine != nullptr) {
+                        nested.machine->start();
+                    }
+                }());
+            },
+            nested_machines
+        );
     }
 
     void force_change_state(size_t state) override {
@@ -454,7 +469,6 @@ class StateMachine : public IStateMachine {
 
     size_t get_current_state_id() const override { return static_cast<size_t>(current_state); }
 
-
     template <size_t N, size_t O> void force_change_state(const State<StateEnum, N, O>& state) {
         perform_state_change(state.get_state());
     }
@@ -575,9 +589,11 @@ consteval auto make_state_machine(StateEnum initial_state, States... states) {
  *
  * @tparam States Variadic template parameter pack representing the states
  * @param initial_state The initial state enum value
- * @tparam nested_machines Tuple of NestedMachineBinding representing the nested state machines to its corresponding state
+ * @tparam nested_machines Tuple of NestedMachineBinding representing the nested state machines to
+ * its corresponding state
  * @param states The states to be included in the state machine
- * @return A StateMachine instance initialized with the provided initial state and states, as well as the nested state machines
+ * @return A StateMachine instance initialized with the provided initial state and states, as well
+ * as the nested state machines
  */
 
 template <IsEnum StateEnum, typename... NestedMachines, typename... States>

From e23b126b3f96300ddaf677d4db6279c56e48d8ef Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 18:22:44 +0100
Subject: [PATCH 08/14] Clanker made me do this so the start and exit functions
 stop when the condition is true

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 38 +++++++++-----------
 1 file changed, 17 insertions(+), 21 deletions(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index c8a8638d..bfac2f4a 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -315,11 +315,10 @@ class StateMachine : public IStateMachine {
         exit();
         std::apply(
             [this](auto&... nested) {
-                (..., [&] {
-                    if (nested.state == this->current_state && nested.machine != nullptr) {
-                        nested.machine->exit();
-                    }
-                }());
+                ((nested.state == this->current_state && nested.machine != nullptr
+                      ? (nested.machine->exit(), true)
+                      : false) ||
+                 ...);
             },
             nested_machines
         );
@@ -331,11 +330,10 @@ class StateMachine : public IStateMachine {
         enter();
         std::apply(
             [this](auto&... nested) {
-                (..., [&] {
-                    if (nested.state == this->current_state && nested.machine != nullptr) {
-                        nested.machine->enter();
-                    }
-                }());
+                ((nested.state == this->current_state && nested.machine != nullptr
+                      ? (nested.machine->enter(), true)
+                      : false) ||
+                 ...);
             },
             nested_machines
         );
@@ -439,11 +437,10 @@ class StateMachine : public IStateMachine {
 
         std::apply(
             [this](auto&... nested) {
-                (..., [&] {
-                    if (nested.state == this->current_state && nested.machine != nullptr) {
-                        nested.machine->check_transitions();
-                    }
-                }());
+                ((nested.state == this->current_state && nested.machine != nullptr
+                      ? (nested.machine->check_transitions(), true)
+                      : false) ||
+                 ...);
             },
             nested_machines
         );
@@ -453,11 +450,10 @@ class StateMachine : public IStateMachine {
         enter();
         std::apply(
             [this](auto&... nested) {
-                (..., [&] {
-                    if (nested.state == this->current_state && nested.machine != nullptr) {
-                        nested.machine->start();
-                    }
-                }());
+                ((nested.state == this->current_state && nested.machine != nullptr
+                      ? (nested.machine->start(), true)
+                      : false) ||
+                 ...);
             },
             nested_machines
         );
@@ -589,7 +585,7 @@ consteval auto make_state_machine(StateEnum initial_state, States... states) {
  *
  * @tparam States Variadic template parameter pack representing the states
  * @param initial_state The initial state enum value
- * @tparam nested_machines Tuple of NestedMachineBinding representing the nested state machines to
+ * @param nested_machines Tuple of NestedMachineBinding representing the nested state machines to
  * its corresponding state
  * @param states The states to be included in the state machine
  * @return A StateMachine instance initialized with the provided initial state and states, as well

From 0bdb147da0747019d3ec99671b7ea3d883262e63 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 23:11:55 +0100
Subject: [PATCH 09/14] Made nesting consteval

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

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index bfac2f4a..cab93b79 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -282,13 +282,13 @@ concept IsNestedMachineBinding = is_nested_machine_binding<T>::value;
 namespace StateMachineHelper {
 
 template <class StateEnum, IsStateMachineClass NestedSMType, size_t N, size_t O>
-static constexpr auto add_nesting(const State<StateEnum, N, O>& state, NestedSMType& machine) {
+static consteval auto add_nesting(const State<StateEnum, N, O>& state, NestedSMType& machine) {
     return NestedMachineBinding<StateEnum, NestedSMType>{state.get_state(), &machine};
 }
 
 template <typename... Bindings>
     requires(IsNestedMachineBinding<Bindings> && ...)
-static constexpr auto add_nested_machines(Bindings... bindings) {
+static consteval auto add_nested_machines(Bindings... bindings) {
     return std::make_tuple(bindings...);
 }
 

From 7b117859042531f0edfe0504ce03fad27464d7ee Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 23:18:34 +0100
Subject: [PATCH 10/14] Fix short circuit no use on return warning

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

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index cab93b79..84265bfe 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -315,7 +315,7 @@ class StateMachine : public IStateMachine {
         exit();
         std::apply(
             [this](auto&... nested) {
-                ((nested.state == this->current_state && nested.machine != nullptr
+                (void)((nested.state == this->current_state && nested.machine != nullptr
                       ? (nested.machine->exit(), true)
                       : false) ||
                  ...);
@@ -330,7 +330,7 @@ class StateMachine : public IStateMachine {
         enter();
         std::apply(
             [this](auto&... nested) {
-                ((nested.state == this->current_state && nested.machine != nullptr
+                (void)((nested.state == this->current_state && nested.machine != nullptr
                       ? (nested.machine->enter(), true)
                       : false) ||
                  ...);
@@ -437,7 +437,7 @@ class StateMachine : public IStateMachine {
 
         std::apply(
             [this](auto&... nested) {
-                ((nested.state == this->current_state && nested.machine != nullptr
+                (void)((nested.state == this->current_state && nested.machine != nullptr
                       ? (nested.machine->check_transitions(), true)
                       : false) ||
                  ...);
@@ -450,7 +450,7 @@ class StateMachine : public IStateMachine {
         enter();
         std::apply(
             [this](auto&... nested) {
-                ((nested.state == this->current_state && nested.machine != nullptr
+                (void)((nested.state == this->current_state && nested.machine != nullptr
                       ? (nested.machine->start(), true)
                       : false) ||
                  ...);

From 3b983524f442ce4d2e9227067719ba63699370be Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 23:36:15 +0100
Subject: [PATCH 11/14] Added state machine tests

---
 Tests/CMakeLists.txt                      |   1 +
 Tests/StateMachine/state_machine_test.cpp | 235 ++++++++++++++++++++++
 2 files changed, 236 insertions(+)
 create mode 100644 Tests/StateMachine/state_machine_test.cpp

diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt
index 0c882b8a..5262628e 100644
--- a/Tests/CMakeLists.txt
+++ b/Tests/CMakeLists.txt
@@ -21,6 +21,7 @@ add_executable(${STLIB_TEST_EXECUTABLE}
     ${CMAKE_CURRENT_LIST_DIR}/../Src/HALAL/Models/DMA/DMA2.cpp
     ${CMAKE_CURRENT_LIST_DIR}/Time/scheduler_test.cpp
     ${CMAKE_CURRENT_LIST_DIR}/Time/timer_wrapper_test.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/StateMachine/state_machine_test.cpp
     ${CMAKE_CURRENT_LIST_DIR}/adc_test.cpp
     ${CMAKE_CURRENT_LIST_DIR}/spi2_test.cpp
     ${CMAKE_CURRENT_LIST_DIR}/dma2_test.cpp
diff --git a/Tests/StateMachine/state_machine_test.cpp b/Tests/StateMachine/state_machine_test.cpp
new file mode 100644
index 00000000..cb841533
--- /dev/null
+++ b/Tests/StateMachine/state_machine_test.cpp
@@ -0,0 +1,235 @@
+#include <gtest/gtest.h>
+#include "ST-LIB_LOW/StateMachine/StateMachine.hpp"
+#include "HALAL/Services/Time/Scheduler.hpp"
+
+enum class MasterState { A, B, C };
+
+enum class SubState { S1, S2 };
+
+static int a_enter_count = 0;
+static int a_exit_count = 0;
+static int a_cyclic_count = 0;
+
+static int b_enter_count = 0;
+static int b_exit_count = 0;
+static int b_cyclic_count = 0;
+
+static int c_enter_count = 0;
+static int c_exit_count = 0;
+
+static int s1_enter_count = 0;
+static int s1_exit_count = 0;
+static int s1_cyclic_count = 0;
+
+static int s2_enter_count = 0;
+static int s2_exit_count = 0;
+
+static bool condition_a_to_b = false;
+static bool condition_b_to_c = false;
+static bool condition_c_to_a = false;
+static bool condition_s1_to_s2 = false;
+
+static void reset_test_state() {
+    a_enter_count = 0;
+    a_exit_count = 0;
+    a_cyclic_count = 0;
+
+    b_enter_count = 0;
+    b_exit_count = 0;
+    b_cyclic_count = 0;
+
+    c_enter_count = 0;
+    c_exit_count = 0;
+
+    s1_enter_count = 0;
+    s1_exit_count = 0;
+    s1_cyclic_count = 0;
+
+    s2_enter_count = 0;
+    s2_exit_count = 0;
+
+    condition_a_to_b = false;
+    condition_b_to_c = false;
+    condition_c_to_a = false;
+    condition_s1_to_s2 = false;
+
+    Scheduler::active_task_count_ = 0;
+    Scheduler::free_bitmap_ = 0xFFFF'FFFF;
+    Scheduler::ready_bitmap_ = 0;
+    Scheduler::sorted_task_ids_ = 0;
+    Scheduler::global_tick_us_ = 0;
+    Scheduler::current_interval_us_ = 0;
+
+    TIM2_BASE->CNT = 0;
+    TIM2_BASE->ARR = 0;
+    TIM2_BASE->SR = 0;
+    TIM2_BASE->CR1 = 0;
+    TIM2_BASE->DIER = 0;
+}
+
+static void tick_scheduler(int ticks) {
+    TIM2_BASE->PSC = 2;
+    for (int i = 0; i < ticks; i++) {
+        for (int j = 0; j <= TIM2_BASE->PSC; j++)
+            TIM2_BASE->inc_cnt_and_check(1);
+        Scheduler::update();
+    }
+}
+
+static constexpr auto state_s1 =
+    make_state(SubState::S1, Transition<SubState>{SubState::S2, []() {
+                                                      return condition_s1_to_s2;
+                                                  }});
+static constexpr auto state_s2 = make_state(SubState::S2);
+
+static inline auto test_nested_machine = []() consteval {
+    auto sm = make_state_machine(SubState::S1, state_s1, state_s2);
+    using namespace std::chrono_literals;
+
+    sm.add_enter_action([]() { s1_enter_count++; }, state_s1);
+    sm.add_exit_action([]() { s1_exit_count++; }, state_s1);
+    sm.add_cyclic_action([]() { s1_cyclic_count++; }, 10ms, state_s1);
+
+    sm.add_enter_action([]() { s2_enter_count++; }, state_s2);
+    sm.add_exit_action([]() { s2_exit_count++; }, state_s2);
+
+    return sm;
+}();
+
+static constexpr auto state_a =
+    make_state(MasterState::A, Transition<MasterState>{MasterState::B, []() {
+                                                           return condition_a_to_b;
+                                                       }});
+static constexpr auto state_b =
+    make_state(MasterState::B, Transition<MasterState>{MasterState::C, []() {
+                                                           return condition_b_to_c;
+                                                       }});
+static constexpr auto state_c =
+    make_state(MasterState::C, Transition<MasterState>{MasterState::A, []() {
+                                                           return condition_c_to_a;
+                                                       }});
+
+static inline auto test_machine = []() consteval {
+    auto nested = StateMachineHelper::add_nesting(state_b, test_nested_machine);
+    auto sm = make_state_machine(
+        MasterState::A,
+        StateMachineHelper::add_nested_machines(nested),
+        state_a,
+        state_b,
+        state_c
+    );
+    using namespace std::chrono_literals;
+
+    sm.add_enter_action([]() { a_enter_count++; }, state_a);
+    sm.add_exit_action([]() { a_exit_count++; }, state_a);
+    sm.add_cyclic_action([]() { a_cyclic_count++; }, 10ms, state_a);
+
+    sm.add_enter_action([]() { b_enter_count++; }, state_b);
+    sm.add_exit_action([]() { b_exit_count++; }, state_b);
+    sm.add_cyclic_action([]() { b_cyclic_count++; }, 20ms, state_b);
+
+    sm.add_enter_action([]() { c_enter_count++; }, state_c);
+    sm.add_exit_action([]() { c_exit_count++; }, state_c);
+
+    return sm;
+}();
+
+class StateMachineTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        // Reset everything before tests
+        reset_test_state();
+
+
+        test_machine.force_change_state((size_t)MasterState::A);
+        test_nested_machine.force_change_state((size_t)SubState::S1);
+
+        test_machine.force_change_state((size_t)MasterState::A);
+        test_nested_machine.force_change_state((size_t)SubState::S1);
+        test_machine.get_states()[0].unregister_all_timed_actions();
+        test_machine.get_states()[1].unregister_all_timed_actions();
+        test_machine.get_states()[2].unregister_all_timed_actions();
+        test_nested_machine.get_states()[0].unregister_all_timed_actions();
+        test_nested_machine.get_states()[1].unregister_all_timed_actions();
+
+        reset_test_state(); 
+    }
+};
+
+TEST_F(StateMachineTest, StartTriggersEnterActions) {
+    test_machine.start();
+    EXPECT_EQ(a_enter_count, 1);
+    EXPECT_EQ(a_exit_count, 0);
+    EXPECT_EQ(b_enter_count, 0);
+    EXPECT_EQ(test_machine.get_current_state(), MasterState::A);
+}
+
+TEST_F(StateMachineTest, BasicTransition) {
+    test_machine.start();
+    a_enter_count = 0; 
+
+    condition_a_to_b = true;
+    test_machine.check_transitions();
+
+    EXPECT_EQ(test_machine.get_current_state(), MasterState::B);
+    EXPECT_EQ(a_exit_count, 1);
+    EXPECT_EQ(b_enter_count, 1);
+    EXPECT_EQ(s1_enter_count, 1); // Nested machine should also enter its initial state
+}
+
+TEST_F(StateMachineTest, NestedTransition) {
+    test_machine.start();
+    condition_a_to_b = true;
+    test_machine.check_transitions();
+
+    EXPECT_EQ(test_machine.get_current_state(), MasterState::B);
+    EXPECT_EQ(test_nested_machine.get_current_state(), SubState::S1);
+
+    condition_s1_to_s2 = true;
+    test_machine.check_transitions();
+
+    EXPECT_EQ(test_nested_machine.get_current_state(), SubState::S2);
+    EXPECT_EQ(s1_exit_count, 1);
+    EXPECT_EQ(s2_enter_count, 1);
+}
+
+TEST_F(StateMachineTest, MasterStateChangeExitsNested) {
+    test_machine.start();
+    condition_a_to_b = true;
+    test_machine.check_transitions();
+
+    EXPECT_EQ(test_nested_machine.get_current_state(), SubState::S1);
+    s1_enter_count = 0; 
+    b_exit_count = 0;  
+
+    condition_b_to_c = true;
+    test_machine.check_transitions();
+
+    EXPECT_EQ(test_machine.get_current_state(), MasterState::C);
+    EXPECT_EQ(b_exit_count, 1);
+    EXPECT_EQ(s1_exit_count, 1);
+    EXPECT_EQ(c_enter_count, 1);
+}
+
+TEST_F(StateMachineTest, CyclicActionsRun) {
+    test_machine.start();
+
+    Scheduler::start();
+
+    tick_scheduler(100);
+    tick_scheduler(10000);
+
+    EXPECT_GE(a_cyclic_count, 1);
+    EXPECT_EQ(b_cyclic_count, 0);
+
+    condition_a_to_b = true;
+    test_machine.check_transitions();
+
+    a_cyclic_count = 0;
+
+    tick_scheduler(20000);
+
+    EXPECT_EQ(a_cyclic_count, 0);  // A cyclic shouldn't run
+    EXPECT_GE(b_cyclic_count, 1);  // B cyclic should run
+    EXPECT_GE(s1_cyclic_count, 1); // Nested S1 cyclic should run
+}

From 92d600bd2e6e8991d35f084f1b8481d3c163f068 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Fri, 27 Feb 2026 23:59:58 +0100
Subject: [PATCH 12/14] Formating errors fixed

---
 Tests/StateMachine/state_machine_test.cpp | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/Tests/StateMachine/state_machine_test.cpp b/Tests/StateMachine/state_machine_test.cpp
index cb841533..0f56004e 100644
--- a/Tests/StateMachine/state_machine_test.cpp
+++ b/Tests/StateMachine/state_machine_test.cpp
@@ -140,7 +140,6 @@ class StateMachineTest : public ::testing::Test {
         // Reset everything before tests
         reset_test_state();
 
-
         test_machine.force_change_state((size_t)MasterState::A);
         test_nested_machine.force_change_state((size_t)SubState::S1);
 
@@ -152,7 +151,7 @@ class StateMachineTest : public ::testing::Test {
         test_nested_machine.get_states()[0].unregister_all_timed_actions();
         test_nested_machine.get_states()[1].unregister_all_timed_actions();
 
-        reset_test_state(); 
+        reset_test_state();
     }
 };
 
@@ -166,7 +165,7 @@ TEST_F(StateMachineTest, StartTriggersEnterActions) {
 
 TEST_F(StateMachineTest, BasicTransition) {
     test_machine.start();
-    a_enter_count = 0; 
+    a_enter_count = 0;
 
     condition_a_to_b = true;
     test_machine.check_transitions();
@@ -199,8 +198,8 @@ TEST_F(StateMachineTest, MasterStateChangeExitsNested) {
     test_machine.check_transitions();
 
     EXPECT_EQ(test_nested_machine.get_current_state(), SubState::S1);
-    s1_enter_count = 0; 
-    b_exit_count = 0;  
+    s1_enter_count = 0;
+    b_exit_count = 0;
 
     condition_b_to_c = true;
     test_machine.check_transitions();

From ca6bdbb1ab14a79818e38a9da6a08b3fe5a20c6c Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Sat, 28 Feb 2026 00:04:20 +0100
Subject: [PATCH 13/14] Ok now format errors are fixed

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

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index 84265bfe..db893b0b 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -316,9 +316,9 @@ class StateMachine : public IStateMachine {
         std::apply(
             [this](auto&... nested) {
                 (void)((nested.state == this->current_state && nested.machine != nullptr
-                      ? (nested.machine->exit(), true)
-                      : false) ||
-                 ...);
+                            ? (nested.machine->exit(), true)
+                            : false) ||
+                       ...);
             },
             nested_machines
         );
@@ -331,9 +331,9 @@ class StateMachine : public IStateMachine {
         std::apply(
             [this](auto&... nested) {
                 (void)((nested.state == this->current_state && nested.machine != nullptr
-                      ? (nested.machine->enter(), true)
-                      : false) ||
-                 ...);
+                            ? (nested.machine->enter(), true)
+                            : false) ||
+                       ...);
             },
             nested_machines
         );
@@ -438,9 +438,9 @@ class StateMachine : public IStateMachine {
         std::apply(
             [this](auto&... nested) {
                 (void)((nested.state == this->current_state && nested.machine != nullptr
-                      ? (nested.machine->check_transitions(), true)
-                      : false) ||
-                 ...);
+                            ? (nested.machine->check_transitions(), true)
+                            : false) ||
+                       ...);
             },
             nested_machines
         );
@@ -451,9 +451,9 @@ class StateMachine : public IStateMachine {
         std::apply(
             [this](auto&... nested) {
                 (void)((nested.state == this->current_state && nested.machine != nullptr
-                      ? (nested.machine->start(), true)
-                      : false) ||
-                 ...);
+                            ? (nested.machine->start(), true)
+                            : false) ||
+                       ...);
             },
             nested_machines
         );

From e445d192f5716b927f9c12e2a5d785e5d6cacd15 Mon Sep 17 00:00:00 2001
From: Cantonplas <dcancat@etsinf.upv.es>
Date: Sat, 28 Feb 2026 00:32:34 +0100
Subject: [PATCH 14/14] More tests need to be addes

---
 Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 19 +++++++-
 Tests/StateMachine/state_machine_test.cpp    | 46 ++++++++++++++++++++
 2 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
index db893b0b..2bffc305 100644
--- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
+++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp
@@ -197,9 +197,13 @@ template <IsEnum StateEnum, size_t NTransitions, size_t Number_of_state_orders =
         timed_action->is_on = false;
     }
 
+    constexpr void add_state_order(uint16_t id) {
 #ifdef STLIB_ETH
-    constexpr void add_state_order(uint16_t id) { state_orders_ids.push_back(id); }
+        state_orders_ids.push_back(id);
+#else
+        (void)id;
 #endif
+    }
 
     template <ValidTime TimeUnit>
     consteval TimedAction* add_cyclic_action(Callback action, TimeUnit period) {
@@ -289,6 +293,17 @@ static consteval auto add_nesting(const State<StateEnum, N, O>& state, NestedSMT
 template <typename... Bindings>
     requires(IsNestedMachineBinding<Bindings> && ...)
 static consteval auto add_nested_machines(Bindings... bindings) {
+    constexpr std::size_t count = sizeof...(Bindings);
+    if constexpr (count > 1) {
+        auto states = std::array{bindings.state...};
+        for (std::size_t i = 0; i < count; ++i) {
+            for (std::size_t j = i + 1; j < count; ++j) {
+                if (states[i] == states[j]) {
+                    ErrorHandler("Duplicate state found in add_nested_machines");
+                }
+            }
+        }
+    }
     return std::make_tuple(bindings...);
 }
 
@@ -431,7 +446,7 @@ class StateMachine : public IStateMachine {
             const auto& t = transitions[index];
             if (t.predicate()) {
                 perform_state_change(t.target);
-                return;
+                break;
             }
         }
 
diff --git a/Tests/StateMachine/state_machine_test.cpp b/Tests/StateMachine/state_machine_test.cpp
index 0f56004e..c05ff95b 100644
--- a/Tests/StateMachine/state_machine_test.cpp
+++ b/Tests/StateMachine/state_machine_test.cpp
@@ -232,3 +232,49 @@ TEST_F(StateMachineTest, CyclicActionsRun) {
     EXPECT_GE(b_cyclic_count, 1);  // B cyclic should run
     EXPECT_GE(s1_cyclic_count, 1); // Nested S1 cyclic should run
 }
+
+template <auto V> struct constant_eval {};
+
+template <typename F>
+concept CanCompile = requires { typename constant_eval<F::invoke()>; };
+
+struct DuplicateNestedCheck {
+    static consteval bool invoke() {
+        auto sm_nested_1 = make_state_machine(SubState::S1, state_s1, state_s2);
+        auto sm_nested_2 = make_state_machine(SubState::S1, state_s1, state_s2);
+
+        auto nested1 = StateMachineHelper::add_nesting(state_a, sm_nested_1);
+        auto nested2 = StateMachineHelper::add_nesting(state_a, sm_nested_2);
+
+        auto sm = make_state_machine(
+            MasterState::A,
+            StateMachineHelper::add_nested_machines(nested1, nested2),
+            state_a,
+            state_b
+        );
+        return true;
+    }
+};
+
+struct ValidNestedCheck {
+    static consteval bool invoke() {
+        auto sm_nested_1 = make_state_machine(SubState::S1, state_s1, state_s2);
+        auto sm_nested_2 = make_state_machine(SubState::S1, state_s1, state_s2);
+
+        auto nested1 = StateMachineHelper::add_nesting(state_a, sm_nested_1);
+        auto nested2 = StateMachineHelper::add_nesting(state_b, sm_nested_2);
+
+        auto sm = make_state_machine(
+            MasterState::A,
+            StateMachineHelper::add_nested_machines(nested1, nested2),
+            state_a,
+            state_b
+        );
+        return true;
+    }
+};
+
+TEST(StateMachineCompileCheck, ValidatesSFINAEOntoS_M) {
+    static_assert(CanCompile<ValidNestedCheck>, "Valid nested mapping should compile.");
+    static_assert(!CanCompile<DuplicateNestedCheck>, "Duplicate state mappings must not compile.");
+}