From d75bce6691cf396bd975eef6fdca5c4285bccba9 Mon Sep 17 00:00:00 2001
From: CarlWachter <carlwachter@web.de>
Date: Mon, 17 Jun 2024 08:54:36 +0200
Subject: [PATCH] feat(state-via-can): state transitions via can are forced to
 resync state across devices

---
 include/sta/tacos.hpp              |  3 ++-
 include/sta/tacos/statemachine.hpp |  3 ++-
 src/can_bus.cpp                    |  4 ++--
 src/statemachine.cpp               | 27 +++++++++++++++++++++++----
 src/tacos.cpp                      |  4 ++--
 5 files changed, 31 insertions(+), 10 deletions(-)

diff --git a/include/sta/tacos.hpp b/include/sta/tacos.hpp
index b7b6ccf..8d6fa36 100644
--- a/include/sta/tacos.hpp
+++ b/include/sta/tacos.hpp
@@ -42,10 +42,11 @@ namespace sta
 		 * @param from The start we want to transition from.
 		 * @param to The state we want to transition to.
 		 * @param lockout An optional timer blocking state transition for a given time.
+		 * @param force If true, the state transition will be executed regardless of the current state.
 		 * 
 		 * @ingroup tacos_api
 		 */
-		void setState(uint32_t from, uint32_t to, uint32_t lockout = 0);
+		void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false);
 
 		/**
 		 * @brief Request a state transition after a given time has passed. Invalid state transitions will be dismissed.
diff --git a/include/sta/tacos/statemachine.hpp b/include/sta/tacos/statemachine.hpp
index 9305316..afa095b 100644
--- a/include/sta/tacos/statemachine.hpp
+++ b/include/sta/tacos/statemachine.hpp
@@ -162,8 +162,9 @@ namespace sta
 			 * @param from The state which we want to leave. This is used to filter out obsolete transitions.
 			 * @param to The state to transition to.
 			 * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
+			 * @param force If true, the state transition will be executed regardless of the current state.
 			 */
-			void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0);
+			void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false);
 
 			/**
 			 * @brief Request a state transition after a given time has passed.
diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index 39f2387..2eca8af 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -155,8 +155,8 @@ namespace sta {
 
 			STA_ASSERT(header.payloadLength == 2);
 
-			// First byte of payload is the origin state, second byte is the destination state
-			tacos::setState(payload[0], payload[1]);
+			// First byte of payload is the origin state, second byte is the destination state. Transition is forced
+			tacos::setState(payload[0], payload[1], 0, true);
 		}
 	} // namespace tacos
 } // namespace sta
diff --git a/src/statemachine.cpp b/src/statemachine.cpp
index ea07e01..5d92a0e 100644
--- a/src/statemachine.cpp
+++ b/src/statemachine.cpp
@@ -64,7 +64,7 @@ namespace sta
 			return currentState_;
 		}
 
-		void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */)
+		void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */)
 		{
 			StateTransition transition;
 			transition.from = from;
@@ -72,9 +72,28 @@ namespace sta
 			transition.event = EventFlags::NORMAL;
 			transition.lockout = lockout;
 
-			// Try to add a state transition request to the queue. Don't wait if another
-			// thread is already requesting a state change.
-			queue_.put(transition, 0);
+			if (force){
+				// Perform the transition and notify the threads. The event flags are set
+				// here in order to allow threads to react immediately.
+				currentState_ = transition.to;
+				Statemachine::stateChangeEvent.set(transition.event);
+				Statemachine::stateChangeEvent.clear(EventFlags::ALL);
+
+				if (failsafeTimer_.isRunning())
+				{
+					failsafeTimer_.stop();
+				}
+
+				// Start the lockout timer if requested.
+				if (transition.lockout != 0)
+				{
+					setLockoutTimer(transition.lockout);
+				}
+			}else{
+				// Try to add a state transition request to the queue. Don't wait if another
+				// thread is already requesting a state change.
+				queue_.put(transition, 0);
+			}
 		}
 
 		void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
diff --git a/src/tacos.cpp b/src/tacos.cpp
index ca50225..99a9380 100644
--- a/src/tacos.cpp
+++ b/src/tacos.cpp
@@ -16,9 +16,9 @@ namespace sta
             return Statemachine::instance()->getCurrentState();
         }
 
-		void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */)
+		void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */)
         {
-            Statemachine::instance()->requestStateTransition(from, to, lockout);
+            Statemachine::instance()->requestStateTransition(from, to, lockout, force);
         }
 
 		void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)