From b082daeb79c1b61c0fee5be6b84ebfe3dca5be30 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 29 May 2025 22:32:09 -0400
Subject: [PATCH] refactor state machine

---
 cereal/custom.capnp                           |  9 ++-
 .../lib/speed_limit_controller/state.py       | 71 +++++++++++++++++++
 sunnypilot/selfdrive/selfdrived/events.py     | 12 ++++
 3 files changed, 89 insertions(+), 3 deletions(-)
 create mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py

diff --git a/cereal/custom.capnp b/cereal/custom.capnp
index e52961da5e..c3644f4c23 100644
--- a/cereal/custom.capnp
+++ b/cereal/custom.capnp
@@ -128,9 +128,9 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   enum SpeedLimitControlState {
     inactive @0; # No speed limit set or not enabled by parameter.
     tempInactive @1; # User wants to ignore speed limit until it changes.
-    adapting @2; # Reducing speed to match new speed limit.
-    active @3; # Cruising at speed limit.
-    preActive @4;
+    preActive @2;
+    adapting @3; # Reducing speed to match new speed limit.
+    active @4; # Cruising at speed limit.
   }
 }
 
@@ -175,6 +175,9 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitActive @18;
     speedLimitConfirmed @19;
     speedLimitValueChange @20;
+    speedLimitInactive @21;
+    speedLimitTempInactive @22;
+    speedLimitAdapting @23;
   }
 }
 
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py
new file mode 100644
index 0000000000..a435fc1ea1
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py
@@ -0,0 +1,71 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from cereal import custom
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+EventNameSP = custom.OnroadEventSP.EventName
+State = custom.LongitudinalPlanSP.SpeedLimitControlState
+
+ACTIVE_STATES = (State.active, State.adapting)
+ENABLED_STATES = (State.preActive, State.tempInactive, *ACTIVE_STATES)
+
+
+class StateMachine:
+  def __init__(self):
+    self.state = State.off
+
+  def update(self, events_sp: EventsSP) -> tuple[bool, bool]:
+    # TEMP INACTIVE, ADAPTING, ACTIVE, PRE ACTIVE
+    if self.state != State.inactive:
+      if events_sp.has(EventNameSP.speedLimitInactive):
+        self.state = State.inactive
+
+      elif events_sp.has(EventNameSP.speedLimitTempInactive):
+        self.state = State.tempInactive
+
+      else:
+        # ACTIVE
+        if self.state == State.active:
+          if events_sp.has(EventNameSP.speedLimitTempInactive):
+            self.state = State.tempInactive
+          elif events_sp.has(EventNameSP.speedLimitPreActive):
+            self.state = State.preActive
+          elif events_sp.has(EventNameSP.speedLimitAdapting):
+            self.state = State.adapting
+
+        # PRE ACTIVE
+        elif self.state == State.preActive:
+          if events_sp.has(EventNameSP.speedLimitInactive):
+            self.state = State.inactive
+          elif events_sp.has(EventNameSP.speedLimitActive):
+            self.state = State.active
+
+        # ADAPTING
+        elif self.state == State.adapting:
+          if events_sp.has(EventNameSP.speedLimitActive):
+            self.state = State.active
+
+        # TEMP INACTIVE
+        elif self.state == State.tempInactive:
+          if events_sp.has(EventNameSP.speedLimitPreActive):
+            self.state = State.preActive
+          elif events_sp.has(EventNameSP.speedLimitInactive):
+            self.state = State.inactive
+
+    # INACTIVE
+    elif self.state == State.inactive:
+      if events_sp.has(EventNameSP.speedLimitPreActive):
+        self.state = State.preActive
+      elif events_sp.has(EventNameSP.speedLimitAdapting):
+        self.state = State.adapting
+      elif events_sp.has(EventNameSP.speedLimitActive):
+        self.state = State.active
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py
index aa43d7186e..3f61843fbf 100644
--- a/sunnypilot/selfdrive/selfdrived/events.py
+++ b/sunnypilot/selfdrive/selfdrived/events.py
@@ -175,4 +175,16 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.WARNING: speed_limit_adjust_alert,
   },
 
+  EventNameSP.speedLimitInactive: {
+    ET.WARNING: EngagementAlert(AudibleAlert.none),
+  },
+
+  EventNameSP.speedLimitTempInactive: {
+    ET.WARNING: EngagementAlert(AudibleAlert.none),
+  },
+
+  EventNameSP.speedLimitAdapting: {
+    ET.WARNING: EngagementAlert(AudibleAlert.none),
+  },
+
 }