Add HybridExperimental mode

common/params_keys.h

@@ -238,6 +238,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"CommunityFavorites", {PERSISTENT, STRING, "", "", 1}},
     {"ConditionalChill", {PERSISTENT, BOOL, "0", "0", 1}},
     {"ConditionalExperimental", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
+    {"HybridExpBias", {PERSISTENT, FLOAT, "0", "0", 1}},
+    {"HybridExperimental", {PERSISTENT, BOOL, "0", "0", 1}},
+    {"HybridVisionBrakeSensitivity", {PERSISTENT, FLOAT, "1", "1", 1}},
     {"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
     {"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
     {"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},

selfdrive/controls/lib/longitudinal_planner.py

@@ -9,6 +9,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.starpilot.common.model_versions import is_tinygrad_model_version
+from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
@@ -596,6 +597,7 @@ class LongitudinalPlanner:
     self.duplicate_vision_comfort_lead_source = None
     self.prev_experimental_mode = None
     self.experimental_release_accel_until = 0.0
+    self.hybrid_controller = HybridExperimentalMode()
 
     if self.is_preap:
       try:
@@ -1936,6 +1938,7 @@ class LongitudinalPlanner:
       self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
       self.model_allow_throttle = True
       self.model_allow_throttle_transition_t = 0.0
+      self.hybrid_controller.reset(float(self.a_desired))
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -2284,26 +2287,42 @@ class LongitudinalPlanner:
       model_launch_accel = self.get_model_launch_accel(model_launch_v, model_launch_a, action_t, scene_v_ego)
 
     if classic_model:
-      output_a_target, output_should_stop = get_accel_from_plan_classic(
+      output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan_classic(
         self.CP, self.v_desired_trajectory, self.a_desired_trajectory, starpilot_toggles.vEgoStopping)
     elif tinygrad_model:
       output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(
         self.v_desired_trajectory, self.a_desired_trajectory,
         action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping)
-      output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
-      output_should_stop_e2e = sm['modelV2'].action.shouldStop
-
-      if self.mode == 'acc' or self.generation == 'v9':
-        output_a_target = output_a_target_mpc
-        output_should_stop = output_should_stop_mpc
-      else:
-        output_a_target = min(output_a_target_mpc, output_a_target_e2e)
-        output_should_stop = output_should_stop_e2e or output_should_stop_mpc
     else:
-      output_a_target, output_should_stop = get_accel_from_plan(
+      output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(
         self.v_desired_trajectory, self.a_desired_trajectory,
         action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping)
 
+    if bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)):
+      self.hybrid_controller.set_tuning(
+        getattr(starpilot_toggles, "hybrid_exp_bias", 0.0),
+        getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0),
+      )
+      output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
+      output_should_stop_e2e = sm['modelV2'].action.shouldStop
+      output_a_target = self.hybrid_controller.update(
+        v_ego=scene_v_ego,
+        v_cruise=v_cruise,
+        lead_one=self.lead_one,
+        model_v2=sm['modelV2'],
+        a_chill=output_a_target_mpc,
+        a_exp=output_a_target_e2e,
+      )
+      output_should_stop = output_should_stop_mpc or output_should_stop_e2e
+    elif tinygrad_model and self.mode != 'acc' and self.generation != 'v9':
+      output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
+      output_should_stop_e2e = sm['modelV2'].action.shouldStop
+      output_a_target = min(output_a_target_mpc, output_a_target_e2e)
+      output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+    else:
+      output_a_target = output_a_target_mpc
+      output_should_stop = output_should_stop_mpc
+
     comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]
     vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego))
     output_accel_min = comfort_output_accel_min

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -514,6 +514,46 @@ def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version):
   assert planner.output_a_target < comfort_min_accel
 
 
+def test_hybrid_mode_shields_unconfirmed_vision_braking():
+  v_ego = 20.0
+  desired_accel = -2.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+
+  # Hybrid mode clamps unconfirmed vision braking to a gentle coast (no lead,
+  # model not predicting a stop), instead of passing the raw E2E target.
+  hybrid_toggles = SimpleNamespace(**vars(make_toggles("v11")), hybrid_experimental_mode=True)
+  planner_hybrid = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(v_ego, desired_accel, -2.0, experimental_mode=False)
+  planner_hybrid.update(sm, hybrid_toggles)
+  assert planner_hybrid.mode == "acc"
+  assert planner_hybrid.output_a_target >= -0.6
+
+  # Without the hybrid shield, experimental mode lets the raw E2E target through.
+  planner_exp = LongitudinalPlanner(CP, init_v=v_ego)
+  sm_exp = make_sm(v_ego, desired_accel, -2.0, experimental_mode=True)
+  planner_exp.update(sm_exp, make_toggles("v11"))
+  assert planner_exp.output_a_target <= -1.5
+
+
+def test_hybrid_mode_arbitrates_vision_braking_with_lead():
+  v_ego = 20.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  hybrid_toggles = SimpleNamespace(**vars(make_toggles("v11")), hybrid_experimental_mode=True)
+
+  # With a tracked lead, the hybrid grants full E2E braking authority.
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(
+    v_ego,
+    desired_accel=-2.0,
+    min_accel=-2.0,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=30.0, v_lead=10.0),
+  )
+  planner.update(sm, hybrid_toggles)
+  assert planner.output_a_target < 0.0
+
+
 def test_gm_pedal_vehicle_min_accel_uses_brand_when_car_name_is_missing():
   CP = SimpleNamespace(
     carName=None,

selfdrive/controls/tests/test_starpilot_planner.py

@@ -175,6 +175,23 @@ def test_lateral_resume_delay_ignores_signal_cycles_that_never_slow_enough(monke
     planner.shutdown()
 
 
+def test_hybrid_mode_runs_continuous_controller(monkeypatch):
+  planner = make_planner(monkeypatch)
+  monkeypatch.setattr(planner.starpilot_cem, "update", lambda *args, **kwargs: None)
+
+  try:
+    toggles = make_toggles(hybrid_experimental_mode=True)
+
+    planner.update(0.0, False, make_sm(planner, frame=1, v_ego=20.0, left_blinker=False), toggles)
+
+    assert planner.hybrid_controller is not None
+    assert planner.hybrid_acceleration != 0.0
+    assert planner.starpilot_ccm.experimental_mode is False
+    assert planner.starpilot_cem.experimental_mode is False
+  finally:
+    planner.shutdown()
+
+
 def test_radarless_follow_hold_applies_to_tracked_vision_lead(monkeypatch):
   planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager())
 

selfdrive/ui/lib/starpilot_status.py

@@ -11,6 +11,7 @@ AOL_COLOR = rl.Color(10, 186, 181, 255)
 ENGAGED_COLOR = rl.Color(22, 127, 64, 255)
 OVERRIDE_COLOR = rl.Color(137, 146, 141, 255)
 EXPERIMENTAL_COLOR = rl.Color(218, 111, 37, 255)
+HYBRID_EXPERIMENTAL_COLOR = rl.Color(0, 153, 255, 255)
 CEM_OVERRIDE_COLOR = rl.Color(255, 214, 0, 255)
 SWITCHBACK_COLOR = rl.Color(139, 108, 197, 255)
 TRAFFIC_COLOR = rl.Color(201, 34, 49, 255)
@@ -27,6 +28,11 @@ def is_longitudinal_only_active(state: UIState) -> bool:
   return bool(state.sm["selfdriveState"].enabled and not car_control.latActive)
 
 
+def _is_hybrid_experimental_mode(state: UIState) -> bool:
+  """True when HEM (hybrid experimental) is the active longitudinal mode."""
+  return bool(state.starpilot_toggles.get("hybrid_experimental_mode", False))
+
+
 def _override_color_applies(state: UIState) -> bool:
   """Only gray the status when the active control mode is being overridden."""
   if state.status != UIStatus.OVERRIDE:
@@ -57,6 +63,8 @@ def get_border_color(state: UIState):
   if state.always_on_lateral_active:
     return AOL_COLOR
   # Only color the border for CEM/experimental while actually enabled.
+  if enabled and _is_hybrid_experimental_mode(state):
+    return HYBRID_EXPERIMENTAL_COLOR
   if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES:
     return CEM_OVERRIDE_COLOR
   if enabled and state.sm["selfdriveState"].experimentalMode:
@@ -67,6 +75,8 @@ def get_border_color(state: UIState):
 
 
 def get_path_edge_color(state: UIState):
+  if state.sm["selfdriveState"].enabled and _is_hybrid_experimental_mode(state):
+    return HYBRID_EXPERIMENTAL_COLOR
   if state.conditional_status in CEM_ACTIVE_STATUSES:
     return EXPERIMENTAL_COLOR
   return get_border_color(state)
@@ -85,6 +95,8 @@ def get_screen_edge_color(state: UIState):
     return AOL_COLOR
   # Keep the screen edge disengaged-blue when experimental mode is only the
   # requested longitudinal mode, not the active driving state.
+  if enabled and _is_hybrid_experimental_mode(state):
+    return HYBRID_EXPERIMENTAL_COLOR
   if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES:
     return CEM_OVERRIDE_COLOR
   if enabled and state.sm["selfdriveState"].experimentalMode:

selfdrive/ui/tests/test_starpilot_status.py

@@ -3,16 +3,18 @@ from types import SimpleNamespace
 from openpilot.selfdrive.ui.lib.starpilot_status import (
   DISENGAGED_COLOR,
   ENGAGED_COLOR,
+  HYBRID_EXPERIMENTAL_COLOR,
   LONGITUDINAL_ONLY_COLOR,
   AOL_COLOR,
   OVERRIDE_COLOR,
   get_border_color,
+  get_path_edge_color,
   get_screen_edge_color,
 )
 from openpilot.selfdrive.ui.ui_state import UIStatus
 
 
-def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=()):
+def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=(), hybrid=False):
   return SimpleNamespace(
     sm={
       "selfdriveState": SimpleNamespace(enabled=enabled, experimentalMode=False),
@@ -24,6 +26,7 @@ def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=()
     switchback_mode_enabled=False,
     traffic_mode_enabled=False,
     conditional_status=0,
+    starpilot_toggles={"hybrid_experimental_mode": hybrid},
   )
 
 
@@ -44,6 +47,19 @@ def test_lateral_active_colors_remain_unchanged():
   assert _rgb(get_border_color(_state())) == _rgb(DISENGAGED_COLOR)
 
 
+def test_hybrid_experimental_mode_uses_purple_border():
+  state = _state(enabled=True, hybrid=True)
+
+  assert _rgb(get_border_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR)
+  assert _rgb(get_screen_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR)
+  assert _rgb(get_path_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR)
+
+
+def test_hybrid_experimental_mode_requires_enabled():
+  assert _rgb(get_border_color(_state(hybrid=True))) == _rgb(DISENGAGED_COLOR)
+  assert _rgb(get_border_color(_state(enabled=True))) == _rgb(ENGAGED_COLOR)
+
+
 def test_override_color_matches_active_control_mode():
   lateral_override = SimpleNamespace(overrideLateral=True, overrideLongitudinal=False)
   longitudinal_override = SimpleNamespace(overrideLateral=False, overrideLongitudinal=True)

starpilot/common/assets/device_settings_layout.json

@@ -2029,6 +2029,44 @@
         "parent_key": "ConditionalChill",
         "settings_tier": "advanced"
       },
+      {
+        "key": "HybridExperimental",
+        "label": "Continuous Hybrid Control",
+        "description": "Fuse the crisp throttle response of classical ACC with the model's early, natural vision braking using a single continuous controller. Classical ACC always provides the hard safety distance, while the model's vision trajectory is blended in for early stops. Unconfirmed vision braking is capped to a gentle coast, and vision is never allowed to under-brake.",
+        "picker_description": "Continuously blends Chill ACC with the model's vision braking.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "is_parent_toggle": true,
+        "settings_tier": "advanced"
+      },
+      {
+        "key": "HybridExpBias",
+        "label": "E2E Authority Bias",
+        "description": "Biases the continuous hybrid between Chill and E2E. Negative favors Chill throttle and braking, positive favors E2E vision behavior. Range -1.0 to 1.0, default 0.0.",
+        "picker_description": "Biases authority between Chill and E2E vision.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": -1.0,
+        "max": 1.0,
+        "step": 0.05,
+        "precision": 2,
+        "parent_key": "HybridExperimental",
+        "settings_tier": "advanced"
+      },
+      {
+        "key": "HybridVisionBrakeSensitivity",
+        "label": "Vision Brake Sensitivity",
+        "description": "Scales how strongly the model's predicted stop horizon contributes to early braking. Below 1.0 reduces unprompted slowing; above 1.0 starts red-light and queue braking earlier. Range 0.0 to 2.0, default 1.0.",
+        "picker_description": "Scales early vision braking authority.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": 0.0,
+        "max": 2.0,
+        "step": 0.05,
+        "precision": 2,
+        "parent_key": "HybridExperimental",
+        "settings_tier": "advanced"
+      },
       {
         "key": "SLCAbbreviatedSources",
         "label": "Show Abbreviated Icon Sources",

starpilot/common/safe_mode.py

@@ -112,6 +112,7 @@ SAFE_MODE_MANAGED_KEYS = (
   "ReduceLateralAccelerationSnow",
   "ConditionalExperimental",
   "ConditionalChill",
+  "HybridExperimental",
   "CECurves",
   "CECurvesLead",
   "CELead",

starpilot/common/starpilot_variables.py

@@ -377,6 +377,19 @@ def speed_limit_controller_available(openpilot_longitudinal: bool, redneck_cruis
   return openpilot_longitudinal or redneck_cruise
 
 
+def get_longitudinal_modes(openpilot_longitudinal: bool, cem: bool, ccm: bool, hybrid: bool) -> tuple[bool, bool, bool]:
+  """Resolve the mutually exclusive longitudinal control modes (CEM/CCM/Hybrid)."""
+  conditional_experimental_mode = bool(openpilot_longitudinal) and bool(cem)
+  conditional_chill_mode = bool(openpilot_longitudinal) and not conditional_experimental_mode and bool(ccm)
+  hybrid_experimental_mode = (
+    bool(openpilot_longitudinal) and
+    not conditional_experimental_mode and
+    not conditional_chill_mode and
+    bool(hybrid)
+  )
+  return conditional_experimental_mode, conditional_chill_mode, hybrid_experimental_mode
+
+
 def migrate_cancel_button_controls(params: Params | None = None) -> bool:
   params = params or Params(return_defaults=True)
   if params.get_bool(CANCEL_BUTTON_MIGRATION_KEY) or not params.get_bool("RemapCancelToDistance"):
@@ -812,8 +825,16 @@ class StarPilotVariables:
     self.migrate_prius_cluster_offset(str(toggle.car_model))
     toggle.cluster_offset = self.get_value("ClusterOffset", cast=float, condition=toggle.car_make == "toyota")
 
-    toggle.conditional_experimental_mode = toggle.openpilot_longitudinal and self.get_value("ConditionalExperimental")
-    toggle.conditional_chill_mode = toggle.openpilot_longitudinal and not toggle.conditional_experimental_mode and self.get_value("ConditionalChill")
+    toggle.conditional_experimental_mode, toggle.conditional_chill_mode, toggle.hybrid_experimental_mode = get_longitudinal_modes(
+      toggle.openpilot_longitudinal,
+      self.get_value("ConditionalExperimental"),
+      self.get_value("ConditionalChill"),
+      self.get_value("HybridExperimental"),
+    )
+    toggle.hybrid_exp_bias = self.get_value(
+      "HybridExpBias", cast=float, condition=toggle.hybrid_experimental_mode, default=0.0, min=-1.0, max=1.0)
+    toggle.hybrid_vision_brake_sensitivity = self.get_value(
+      "HybridVisionBrakeSensitivity", cast=float, condition=toggle.hybrid_experimental_mode, default=1.0, min=0.0, max=2.0)
     toggle.conditional_curves = self.get_value("CECurves", condition=toggle.conditional_experimental_mode)
     toggle.conditional_curves_lead = self.get_value("CECurvesLead", condition=toggle.conditional_curves)
     toggle.conditional_lead = self.get_value("CELead", condition=toggle.conditional_experimental_mode)

starpilot/common/tests/test_starpilot_variables.py

@@ -227,6 +227,18 @@ def test_missing_bounded_value_uses_explicit_default():
   assert value == 1.0
 
 
+def test_get_longitudinal_modes_is_mutually_exclusive():
+  # Hybrid wins when selected and CEM/CCM params are off.
+  assert spv.get_longitudinal_modes(True, False, False, True) == (False, False, True)
+  # CEM takes priority over the other modes when enabled.
+  assert spv.get_longitudinal_modes(True, True, True, True) == (True, False, False)
+  # CCM engages only when CEM is off.
+  assert spv.get_longitudinal_modes(True, False, True, False) == (False, True, False)
+  assert spv.get_longitudinal_modes(True, False, True, True) == (False, True, False)
+  # Nothing engages without openpilot longitudinal.
+  assert spv.get_longitudinal_modes(False, True, True, True) == (False, False, False)
+
+
 def test_disabled_conditional_experimental_toggles_are_off(monkeypatch, tmp_path):
   params_cls = spv.Params
 

starpilot/controls/lib/hybrid_experimental_mode.py

@@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
+
+def lerp(a: float, b: float, t: float) -> float:
+  """Linear interpolation / blend between a and b by weight t (0.0 to 1.0)."""
+  return float((1.0 - t) * a + t * b)
+
+
+def sigmoid(x: float, k: float = 4.0, x0: float = 0.0) -> float:
+  """Smooth 0-to-1 activation curve."""
+  z = np.clip(-k * (x - x0), -30.0, 30.0)
+  return float(1.0 / (1.0 + np.exp(z)))
+
+
+def smooth_min(a: float, b: float, k: float = 6.0) -> float:
+  return lerp(b, a, sigmoid(b - a, k=k))
+
+
+def smooth_max(a: float, b: float, k: float = 6.0) -> float:
+  return lerp(b, a, sigmoid(a - b, k=k))
+
+class HybridExperimentalMode:
+  """
+  Fuses Chill Mode (radar/lead tracking) and Experimental Mode (vision/stop signs/lights):
+  1. Detects vision braking intent from the E2E model trajectory.
+  2. Blends Chill and Exp acceleration smoothly based on intent.
+  3. Holds 0 m/s at standstills to prevent creep.
+  4. Falls back safely to Chill if lead vehicle distance is compromised.
+  5. Slew-rate limits acceleration to respect vehicle jerk limits.
+  """
+
+  # Base physical actuator jerk limits (m/s^3)
+  BASE_MAX_JERK_BRAKE = 3.5
+  BASE_MAX_JERK_ACCEL = 5.5
+
+  # Base safety floor parameters
+  BASE_T_FOLLOW = 1.45
+  BASE_STOP_DISTANCE = STOP_DISTANCE  # 6.0 m
+
+  def __init__(self):
+    self.DT = DT_MDL
+    self.prev_a_target = 0.0
+    self.exp_authority = 0.5
+
+    # User tuning
+    self.HYBRID_EXP_BIAS = 0.0            # [-1.0, 1.0]
+    self.VISION_BRAKE_SENSITIVITY = 1.0   # [0.0, 2.0]
+
+    # Active profile parameters
+    self.t_follow = self.BASE_T_FOLLOW
+    self.jerk_factor = 1.0
+    self._update_profile_limits(self.t_follow, self.jerk_factor)
+
+  def reset(self, a: float = 0.0):
+    self.prev_a_target = float(a)
+    self.exp_authority = 0.5
+
+  def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float, t_follow=None, jerk_factor=None):
+    self.HYBRID_EXP_BIAS = float(np.clip(exp_bias, -1.0, 1.0))
+    self.VISION_BRAKE_SENSITIVITY = float(np.clip(vision_brake_sensitivity, 0.0, 2.0))
+    if t_follow is not None or jerk_factor is not None:
+      self._update_profile_limits(t_follow, jerk_factor)
+
+  def _update_profile_limits(self, t_follow, jerk_factor=1.0):
+    """Updates safety headway floor and jerk limits based on active driving profile."""
+    self.t_follow = float(t_follow) if t_follow is not None else self.BASE_T_FOLLOW
+    self.jerk_factor = float(np.clip(jerk_factor if jerk_factor is not None else 1.0, 0.25, 2.0))
+
+    # Safety buffer floors
+    self.T_FOLLOW_SAFE = float(np.clip(self.t_follow * 0.80, 1.00, 1.60))
+    self.D_STATIC_SAFE = float(np.clip(self.BASE_STOP_DISTANCE * 0.75, 3.5, 6.0))
+
+    # Jerk rate limits
+    self.MAX_JERK_ACCEL = float(np.clip(self.BASE_MAX_JERK_ACCEL * self.jerk_factor, 2.5, 8.0))
+    self.MAX_JERK_BRAKE = float(np.clip(self.BASE_MAX_JERK_BRAKE * self.jerk_factor, 1.8, 6.0))
+
+  @staticmethod
+  def _get_model_trajectory_v(model_v2, v_ego: float) -> np.ndarray:
+    velocity = getattr(model_v2, "velocity", None)
+    traj_v = getattr(velocity, "x", None) if velocity is not None else None
+    if traj_v is None or len(traj_v) == 0:
+      return np.array([v_ego], dtype=float)
+    return np.asarray(traj_v, dtype=float)
+
+  def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp,
+             t_follow=None, jerk_factor=None):
+    # 0. Sync profile parameters if passed per-frame
+    if (t_follow is not None and abs(t_follow - self.t_follow) > 1e-4) or \
+       (jerk_factor is not None and abs(jerk_factor - self.jerk_factor) > 1e-4):
+      self._update_profile_limits(t_follow, jerk_factor)
+
+    lead_status = bool(getattr(lead_one, "status", False))
+    lead_d_rel = float(getattr(lead_one, "dRel", 150.0))
+
+    # 1. VISION INTENT DETECTION (How urgently does the vision model want to slow?)
+    traj_v = self._get_model_trajectory_v(model_v2, v_ego)
+    v_terminal = float(traj_v[-1])
+    v_min = float(np.min(traj_v))
+    v_ref = max(v_ego, 2.0)
+
+    speed_drop_ratio     = max(0.0, (v_ego - v_min) / v_ref)
+    stop_ahead_intent    = max(0.0, (v_ego - v_terminal) / v_ref) * sigmoid(v_ego, k=3.0, x0=1.0)
+    model_decel_strength = max(0.0, -a_exp / 3.0)
+
+    raw_vision_metric = max(speed_drop_ratio, stop_ahead_intent, model_decel_strength)
+    w_vision = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0))
+
+    # Experimental authority weight (bias + vision confidence)
+    base_auth = 0.5 + (0.35 * self.HYBRID_EXP_BIAS)
+    alpha_exp = float(np.clip(base_auth + (0.5 * w_vision), 0.0, 1.0))
+    self.exp_authority = alpha_exp
+
+    # 2. ACCELERATION FUSION (Throttle vs Braking Regimes)
+    # Throttle: Be responsive (smooth_max), but prioritize stopping if vision sees a stop
+    a_throttle_optimal = smooth_max(a_chill, a_exp, k=4.0)
+    a_throttle_conservative = smooth_min(a_chill, a_exp, k=4.0)
+    a_throttle_fused = lerp(a_throttle_optimal, a_throttle_conservative, w_vision)
+
+    # Braking: Smoothly hand control to Exp based on vision confidence
+    a_brake_fused = lerp(a_chill, a_exp, alpha_exp)
+
+    # Pick between Accel and Brake regimes
+    phase_metric = smooth_min(a_chill, a_exp, k=4.0)
+    w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1)
+    a_fused = lerp(a_brake_fused, a_throttle_fused, w_accel)
+
+    # 3. STANDSTILL ANCHOR (Prevent creeping at 0 mph)
+    is_stopped = sigmoid(0.3 - v_ego, k=8.0, x0=0.0)
+    is_terminal_stopped = sigmoid(0.8 - v_terminal, k=4.0, x0=0.0)
+    standstill_weight = is_stopped * is_terminal_stopped
+    a_anchored = lerp(a_fused, smooth_min(a_fused, 0.0, k=8.0), standstill_weight)
+
+    # 4. SAFETY BARRIER (Lead Vehicle Proximity Check)
+    d_static_effective = self.D_STATIC_SAFE + max(0.0, 1.5 * (1.0 - (v_ego / 4.0)))
+    d_safe = (v_ego * self.T_FOLLOW_SAFE) + d_static_effective
+
+    # Compute safety risk if lead is within minimum buffer
+    distance_ratio = (lead_d_rel - d_static_effective) / max(d_safe - d_static_effective, 1.0)
+    lead_safety_risk = sigmoid(1.0 - distance_ratio, k=5.0, x0=0.0) * float(lead_status)
+
+    # Fall back to Chill braking if Chill is more conservative
+    a_emergency_brake = smooth_min(a_anchored, a_chill, k=6.0)
+    a_safe = lerp(a_anchored, a_emergency_brake, lead_safety_risk)
+
+    # 5. ASYMMETRIC SLEW FILTER (Limit Jerk)
+    jerk_limit = self.MAX_JERK_ACCEL if a_safe >= self.prev_a_target else self.MAX_JERK_BRAKE
+    max_delta = jerk_limit * self.DT
+
+    self.prev_a_target = float(np.clip(a_safe, self.prev_a_target - max_delta, self.prev_a_target + max_delta))
+    return self.prev_a_target

starpilot/controls/starpilot_planner.py

@@ -24,6 +24,7 @@ from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width,
 from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD
 from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode
 from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
+from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode
 from openpilot.starpilot.controls.lib.starpilot_acceleration import StarPilotAcceleration
 from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents
 from openpilot.starpilot.controls.lib.starpilot_following import StarPilotFollowing
@@ -68,6 +69,7 @@ class StarPilotPlanner:
     self.starpilot_acceleration = StarPilotAcceleration(self)
     self.starpilot_cem = ConditionalExperimentalMode(self)
     self.starpilot_ccm = ConditionalChillMode(self, self.starpilot_cem)
+    self.hybrid_controller = HybridExperimentalMode()
     self.starpilot_events = StarPilotEvents(self, error_log, ThemeManager)
     self.starpilot_following = StarPilotFollowing(self)
     self.starpilot_vcruise = StarPilotVCruise(self)
@@ -100,6 +102,7 @@ class StarPilotPlanner:
     self.road_curvature = 0
     self.time_to_curve = 0
     self.v_cruise = 0
+    self.hybrid_acceleration = 0.0
 
     self.gps_position = None
 
@@ -220,7 +223,29 @@ class StarPilotPlanner:
     self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles)
 
     conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled
-    if conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)):
+    if conditional_tracking_active and bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)):
+      # Continuous Hybrid Experimental Control: instead of a binary CEM/CCM mode
+      # switch, fuse the classical Chill ACC target with the model's E2E
+      # trajectory through the HybridExperimentalMode controller. CEM's detector
+      # stays warm so red-light/stop-sign scene state (redLight, forcing_stop)
+      # remains accurate. Experimental Mode stays off so the MPC keeps producing
+      # the crisp classical a_chill that the hybrid is built on.
+      self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise)
+      self.starpilot_ccm.experimental_mode = False
+      self.starpilot_cem.experimental_mode = False
+      self.hybrid_controller.set_tuning(
+        getattr(starpilot_toggles, "hybrid_exp_bias", 0.0),
+        getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0),
+      )
+      self.hybrid_acceleration = self.hybrid_controller.update(
+        v_ego=v_ego,
+        v_cruise=v_cruise,
+        lead_one=self.lead_one,
+        model_v2=sm["modelV2"],
+        a_chill=self._get_chill_accel(v_ego, v_cruise),
+        a_exp=self._get_vision_exp_accel(sm),
+      )
+    elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)):
       # Keep CEM's filters warm in AOL so engagement can inherit the current scene.
       self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise)
       self.starpilot_ccm.experimental_mode = True
@@ -296,6 +321,40 @@ class StarPilotPlanner:
     self.tracking_lead_filter.update(following_lead)
     return self.tracking_lead_filter.x >= THRESHOLD
 
+  def _get_vision_exp_accel(self, sm):
+    """Model's raw predicted acceleration for the E2E (Experimental) channel."""
+    try:
+      accel_x = sm["modelV2"].acceleration.x
+      if len(accel_x) > 0:
+        return float(accel_x[0])
+    except (AttributeError, IndexError, TypeError):
+      pass
+    try:
+      return float(sm["modelV2"].action.desiredAcceleration)
+    except (AttributeError, TypeError, ValueError):
+      return 0.0
+
+  def _get_chill_accel(self, v_ego, v_cruise):
+    """Cruise/lead tracking acceleration for the Chill (ACC) channel."""
+    max_accel = float(getattr(self.starpilot_acceleration, "max_accel", 0.0) or 0.0)
+    min_accel = float(getattr(self.starpilot_acceleration, "min_accel", 0.0) or 0.0)
+    if max_accel <= 0.0 and min_accel == 0.0:
+      max_accel, min_accel = 1.5, -2.0
+
+    a_chill = float(np.clip((v_cruise - v_ego) / 2.0, min_accel, max_accel))
+    lead = getattr(self, "lead_one", None)
+    if lead is not None and bool(getattr(lead, "status", False)):
+      d_rel = float(getattr(lead, "dRel", float("inf")))
+      v_lead = float(getattr(lead, "vLead", v_ego))
+      t_follow = float(getattr(self.starpilot_following, "t_follow", 1.45) or 1.45)
+      desired_gap = max(v_ego * t_follow, 4.0)
+      if d_rel < desired_gap:
+        gap_deficit = max(desired_gap - d_rel, 0.0)
+        closing = max(0.0, v_ego - v_lead)
+        lead_decel = -min(closing / 2.0 + gap_deficit / 4.0, 3.0)
+        a_chill = min(a_chill, lead_decel)
+    return float(np.clip(a_chill, min_accel, max_accel))
+
   def publish(self, theme_updated, sm, pm, starpilot_toggles, serialized_toggles=""):
     starpilot_plan_send = messaging.new_message("starpilotPlan")
     starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"])

starpilot/controls/tests/test_hybrid_experimental_mode.py

@@ -0,0 +1,125 @@
+import numpy as np
+import pytest
+
+from openpilot.starpilot.controls.lib.hybrid_experimental_mode import (
+  HybridExperimentalMode,
+  sigmoid,
+  soft_max,
+  soft_min,
+)
+
+
+class FakeLead:
+  def __init__(self, status=False, d_rel=150.0, v_lead=0.0):
+    self.status = status
+    self.dRel = d_rel
+    self.vLead = v_lead
+
+
+class FakeModel:
+  def __init__(self, velocity=None):
+    self.velocity = type("Velocity", (), {"x": velocity})()
+
+
+def make_controller(prev=0.0):
+  controller = HybridExperimentalMode()
+  controller.reset(prev)
+  return controller
+
+
+def run(controller, *, v_ego=20.0, v_cruise=30.0, lead=None, model=None, a_chill=0.5, a_exp=0.8, frames=80):
+  lead = lead if lead is not None else FakeLead()
+  model = model if model is not None else FakeModel(velocity=[v_ego] * 20)
+  result = 0.0
+  for _ in range(frames):
+    result = controller.update(v_ego, v_cruise, lead, model, a_chill, a_exp)
+  return result
+
+
+def test_soft_operators_are_continuous_and_bounded():
+  for value in (-5.0, -0.1, 0.0, 0.1, 5.0):
+    assert 0.0 < sigmoid(value) < 1.0
+  assert soft_max(1.0, 2.0) == pytest.approx(2.0, abs=1e-2)
+  assert soft_min(1.0, 2.0) == pytest.approx(1.0, abs=1e-2)
+
+
+def test_throttle_fusion_leans_toward_snappier_target():
+  controller = make_controller()
+  # No lead, no vision stop: the continuous fusion routes toward the more
+  # confident throttle (exp 0.8) without a mode switch.
+  a = run(controller, a_chill=0.5, a_exp=0.8)
+  assert a > 0.75
+  assert a < 0.85
+
+
+def test_vision_stop_horizon_grants_full_braking():
+  controller = make_controller()
+  model = FakeModel(velocity=np.linspace(20.0, 0.1, 20))
+  # The model trajectory decays to a terminal stop, so w_vision -> 1 and the
+  # early vision braking curve takes over.
+  a = run(controller, lead=FakeLead(status=False), model=model, a_chill=-0.5, a_exp=-2.0)
+  assert a <= -1.5
+
+
+def test_phantom_brake_shield_caps_unconfirmed_vision_braking():
+  controller = make_controller()
+  # No lead and a flat model horizon: unconfirmed vision braking is clamped to
+  # the gentle coast limit instead of passing the raw E2E target through.
+  a = run(controller, lead=FakeLead(status=False), a_chill=0.0, a_exp=-2.0)
+  assert -0.6 <= a <= -0.5
+
+
+def test_cbf_safety_floor_prevents_under_braking_near_close_lead():
+  controller = make_controller()
+  # Vision wants throttle but a close lead means the Control Barrier Function
+  # smoothly forces the target toward the Chill safety floor.
+  lead = FakeLead(status=True, d_rel=5.0, v_lead=0.0)
+  a = run(controller, lead=lead, a_chill=-1.0, a_exp=0.5)
+  assert a == pytest.approx(-1.0, abs=1e-2)
+
+
+def test_cbf_is_inactive_when_lead_is_far():
+  controller = make_controller()
+  lead = FakeLead(status=True, d_rel=80.0, v_lead=25.0)
+  a = run(controller, lead=lead, a_chill=0.5, a_exp=0.8)
+  assert a > 0.7
+
+
+def test_jerk_slew_limits_single_frame_step():
+  controller = make_controller(prev=0.0)
+  lead = FakeLead(status=False)
+  model = FakeModel(velocity=[20.0] * 20)
+  a = controller.update(20.0, 30.0, lead, model, 2.0, 2.0)
+  assert a == pytest.approx(controller.MAX_JERK * controller.DT, abs=1e-3)
+
+
+def test_reset_clears_integrator_state():
+  controller = make_controller(prev=0.0)
+  lead = FakeLead(status=True, d_rel=20.0, v_lead=15.0)
+  model = FakeModel(velocity=[20.0] * 20)
+  controller.update(20.0, 30.0, lead, model, -1.0, -2.0)
+  assert controller.prev_a_target != 0.0
+  controller.reset(0.0)
+  assert controller.prev_a_target == 0.0
+
+
+def test_set_tuning_clamps_to_documented_ranges():
+  controller = make_controller()
+  controller.set_tuning(5.0, 9.0)
+  assert controller.HYBRID_EXP_BIAS == 1.0
+  assert controller.VISION_BRAKE_SENSITIVITY == 2.0
+  controller.set_tuning(-5.0, -1.0)
+  assert controller.HYBRID_EXP_BIAS == -1.0
+  assert controller.VISION_BRAKE_SENSITIVITY == 0.0
+  controller.set_tuning(0.5, 1.5)
+  assert controller.HYBRID_EXP_BIAS == 0.5
+  assert controller.VISION_BRAKE_SENSITIVITY == 1.5
+
+
+def test_missing_model_velocity_falls_back_to_ego_speed():
+  controller = make_controller()
+  model = FakeModel(velocity=None)
+  lead = FakeLead(status=False)
+  # Falls back to a constant-velocity horizon, so no vision braking is injected.
+  a = run(controller, lead=lead, model=model, a_chill=0.5, a_exp=0.2)
+  assert a > 0.4

starpilot/system/the_galaxy/the_galaxy.py

@@ -1523,6 +1523,7 @@ _TROUBLESHOOT_PERSONALITY_KEYS = [
 
 _TROUBLESHOOT_CEM_KEYS = [
   "ConditionalExperimental",
+  "HybridExperimental",
   "CESpeed",
   "CESpeedLead",
   "CECurves",
@@ -5201,15 +5202,16 @@ def setup(app):
           "updated": updated,
         }), 200
 
-      if key in {"ConditionalExperimental", "ConditionalChill"}:
+      if key in {"ConditionalExperimental", "ConditionalChill", "HybridExperimental"}:
         enabled = str_val.strip() in ("1", "true", "True")
         params.put_bool(key, enabled)
 
         updated = {key: enabled}
         if enabled:
-          other_key = "ConditionalChill" if key == "ConditionalExperimental" else "ConditionalExperimental"
-          params.put_bool(other_key, False)
-          updated[other_key] = False
+          for other_key in ("ConditionalExperimental", "ConditionalChill", "HybridExperimental"):
+            if other_key != key:
+              params.put_bool(other_key, False)
+              updated[other_key] = False
 
         update_starpilot_toggles()
         return jsonify({

tools/replay/mode_sim.py

@@ -0,0 +1,837 @@
+#!/usr/bin/env python3
+"""Offline mode-scenario estimator for CEM / Chill (CCM) / HEM / Exp.
+
+Replays a logged route segment through the real StarPilot mode state machines
+(ConditionalExperimentalMode, ConditionalChillMode, HybridExperimentalMode)
+and openpilot's LongitudinalPlanner, then plots and prints what each mode's
+experimental intent and acceleration target would have been at every 20 Hz frame.
+
+Usage:
+  ./dev python tools/replay/mode_sim.py <route> [--segment 0] [--start 30] [--end 120]
+      [--data_dir /path/to/routes] [--out graph.png] [--show]
+      [--set CESpeed=20 --set CEModelStopTime=3.0 --set CCMLead=true] [--csv out.csv]
+
+Examples:
+  ./dev python tools/replay/mode_sim.py afb7ef2ed593d651/00000095--9dcb90357c --segment 9
+"""
+from __future__ import annotations
+
+import argparse
+import bisect
+import sys
+import time
+from pathlib import Path
+from types import SimpleNamespace
+from typing import Any
+
+import numpy as np
+
+ROOT_DIR = Path(__file__).resolve().parents[2]
+if str(ROOT_DIR) not in sys.path:
+  sys.path.insert(0, str(ROOT_DIR))
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_accel_from_plan
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE
+from openpilot.starpilot.common.experimental_state import CCStatus, CEStatus
+from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature
+from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME
+from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode
+from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
+from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode
+from openpilot.tools.lib.logreader import LogReader, ReadMode, parse_direct, parse_indirect
+from openpilot.tools.lib.route import SegmentRange
+
+SERVICES = {
+  "carState", "radarState", "starpilotRadarState", "modelV2",
+  "longitudinalPlan", "selfdriveState", "starpilotPlan", "starpilotCarState",
+  "controlsState", "carParams", "liveParameters", "carControl",
+}
+
+CE_LABELS = {
+  CEStatus["CURVATURE"]: "CURVE",
+  CEStatus["LEAD"]: "LEAD",
+  CEStatus["SIGNAL"]: "SIGNAL",
+  CEStatus["SPEED"]: "SPEED",
+  CEStatus["SPEED_LIMIT"]: "SPEED_LIMIT",
+  CEStatus["STOP_LIGHT"]: "STOP_LIGHT",
+}
+CC_LABELS = {
+  CCStatus["LEAD"]: "LEAD",
+  CCStatus["SPEED"]: "SPEED",
+}
+CC_AUTO_STATUSES = {CCStatus["SPEED"], CCStatus["LEAD"]}
+
+# CEM/CCM toggle defaults (units in m/s where appropriate).
+TOGGLE_DEFAULTS: dict[str, Any] = {
+  # CEM
+  "conditional_curves": True,
+  "conditional_curves_lead": True,
+  "conditional_lead": True,
+  "conditional_slower_lead": True,
+  "conditional_stopped_lead": True,
+  "conditional_open_road": False,
+  "conditional_limit": 0.0,                           # CESpeed, m/s (0 = disabled)
+  "conditional_limit_lead": 0.0,                      # CESpeedLead, m/s (0 = disabled)
+  "conditional_signal": 0.0,                          # CESignalSpeed, m/s (0 = disabled)
+  "conditional_model_stop_time": 0.0,                 # CEModelStopTime, s (0 = disabled)
+  "conditional_signal_lane_detection": False,
+  "lane_detection_width": 0.0,
+  # Chill (CCM)
+  "conditional_chill_speed": 30 * CV.MPH_TO_MS,       # CCMSpeed
+  "conditional_chill_speed_lead": 25 * CV.MPH_TO_MS,  # CCMSpeedLead
+  "conditional_chill_speed_margin": 3 * CV.MPH_TO_MS, # CCMSetSpeedMargin
+  "conditional_chill_lead": True,
+  "conditional_chill_launch_assist": False,
+  # HEM
+  "hybrid_exp_bias": 0.0,
+  "hybrid_vision_brake_sensitivity": 1.0,
+  # StarPilot / Planner toggles
+  "taco_tune": False,
+  "classic_model": False,
+  "tinygrad_model": False,
+  "vEgoStopping": 0.05,
+  "hybrid_experimental_mode": False,
+  "radar_takeoffs": False,
+  "lane_change_close_gap": False,
+  "minimum_lane_change_speed": 0.0,
+  "model_version": None,
+}
+BOOL_KEYS = {
+  "conditional_curves", "conditional_curves_lead", "conditional_lead",
+  "conditional_slower_lead", "conditional_stopped_lead", "conditional_open_road",
+  "conditional_signal_lane_detection", "conditional_chill_lead",
+  "conditional_chill_launch_assist", "taco_tune", "classic_model", "tinygrad_model",
+  "hybrid_experimental_mode", "radar_takeoffs", "lane_change_close_gap",
+}
+
+
+class FakeParams:
+  def __init__(self):
+    self.bools = {}
+    self.ints = {}
+
+  def get_bool(self, key):
+    return bool(self.bools.get(key, False))
+
+  def put_bool(self, key, value):
+    self.bools[key] = bool(value)
+
+  def get_int(self, key, default=0):
+    return int(self.ints.get(key, default))
+
+  def put_int(self, key, value):
+    self.ints[key] = int(value)
+
+
+def default_lead(status=False):
+  return SimpleNamespace(
+    status=status,
+    dRel=float("inf"),
+    yRel=0.0,
+    vRel=0.0,
+    aRel=0.0,
+    vLead=0.0,
+    vLeadK=0.0,
+    dPath=0.0,
+    vLat=0.0,
+    aLeadK=0.0,
+    aLeadTau=1.5,
+    fcw=False,
+    modelProb=0.0,
+    radar=False,
+  )
+
+
+def as_lead(raw):
+  if raw is None:
+    return default_lead()
+  try:
+    status = bool(getattr(raw, "status", False))
+  except Exception:
+    status = False
+  return SimpleNamespace(
+    status=status,
+    dRel=float(getattr(raw, "dRel", float("inf"))),
+    yRel=float(getattr(raw, "yRel", 0.0)),
+    vRel=float(getattr(raw, "vRel", 0.0)),
+    aRel=float(getattr(raw, "aRel", 0.0)),
+    vLead=float(getattr(raw, "vLead", 0.0)),
+    vLeadK=float(getattr(raw, "vLeadK", getattr(raw, "vLead", 0.0))),
+    dPath=float(getattr(raw, "dPath", 0.0)),
+    vLat=float(getattr(raw, "vLat", 0.0)),
+    aLeadK=float(getattr(raw, "aLeadK", 0.0)),
+    aLeadTau=float(getattr(raw, "aLeadTau", 1.5)),
+    fcw=bool(getattr(raw, "fcw", False)),
+    modelProb=float(getattr(raw, "modelProb", 0.0)),
+    radar=bool(getattr(raw, "radar", False)),
+  )
+
+
+def local_segment_files(data_dir, route_name, segment):
+  data_root = Path(data_dir)
+  segment_names = (f"{route_name}--{segment}", f"{route_name.replace('|', '/')}/{segment}")
+  filenames = ("rlog.zst", "rlog.bz2", "qlog.zst", "qlog.bz2")
+  identifiers = []
+  for segment_name in segment_names:
+    for filename in filenames:
+      candidate = data_root / segment_name / filename
+      if candidate.exists():
+        identifiers.append(str(candidate))
+  for filename in filenames:
+    explorer = data_root / f"{route_name}--{segment}--{filename}"
+    if explorer.exists():
+      identifiers.append(str(explorer))
+  return identifiers
+
+
+def resolve_segment_identifier(route, segment, data_dir):
+  direct = parse_direct(route)
+  if direct is not None:
+    return [str(direct)]
+  parsed = parse_indirect(route)
+  sr = SegmentRange(parsed)
+  route_name = sr.route_name.replace("/", "|")
+  if data_dir:
+    identifiers = local_segment_files(data_dir, route_name, segment)
+    if identifiers:
+      return identifiers
+  return [f"{route_name}--{segment}"]
+
+
+def load_buffers(identifiers, services):
+  bufs = {s: [] for s in services}
+  for identifier in identifiers:
+    try:
+      logreader = LogReader(identifier, default_mode=ReadMode.RLOG)
+    except Exception as exc:
+      print(f"Unable to open {identifier}: {exc}", file=sys.stderr)
+      continue
+    for evt in logreader:
+      which = evt.which()
+      if which not in services:
+        continue
+      try:
+        t = evt.logMonoTime * 1e-9
+      except Exception:
+        continue
+      bufs[which].append((t, getattr(evt, which)))
+  for service in services:
+    bufs[service].sort(key=lambda x: x[0])
+  return bufs
+
+
+def parse_toggle_overrides(args_set):
+  overrides = {}
+  for item in args_set or []:
+    key, sep, raw = item.partition("=")
+    key = key.strip()
+    if not sep or key not in TOGGLE_DEFAULTS:
+      print(f"warning: ignoring unknown toggle override '{item}'", file=sys.stderr)
+      continue
+    raw = raw.strip()
+    if key in BOOL_KEYS:
+      overrides[key] = raw.lower() in {"1", "true", "yes", "on"}
+    else:
+      try:
+        overrides[key] = float(raw)
+      except ValueError:
+        print(f"warning: ignoring non-numeric toggle override '{item}'", file=sys.stderr)
+  return overrides
+
+
+def run_simulation(grid, bufs, toggles):
+  ts = {s: [t for t, _ in m] for s, m in bufs.items()}
+  ms = {s: [m for _, m in bufs[s]] for s in bufs}
+
+  def latest(service, t):
+    times = ts.get(service)
+    if not times:
+      return None
+    idx = bisect.bisect_right(times, t) - 1
+    return ms[service][idx] if idx >= 0 else None
+
+  cp_candidate = None
+  for _, cp in bufs.get("carParams", []):
+    if cp is not None:
+      cp_candidate = cp
+      break
+
+  if cp_candidate is None:
+    cp_candidate = SimpleNamespace(
+      brand="toyota",
+      carFingerprint="TOYOTA_RAV4",
+      openpilotLongitudinalControl=True,
+      pcmCruise=False,
+      steerRatio=15.0,
+      wheelbase=2.7,
+      longitudinalActuatorDelay=0.2,
+      flags=0,
+    )
+
+  planner_state = SimpleNamespace(
+    params=FakeParams(),
+    params_memory=FakeParams(),
+    starpilot_following=SimpleNamespace(following_lead=False, slower_lead=False),
+    starpilot_vcruise=SimpleNamespace(
+      slc=SimpleNamespace(experimental_mode=False),
+      stop_sign_confirmed=False,
+      forcing_stop=False,
+    ),
+  )
+
+  cem = ConditionalExperimentalMode(planner_state)
+  cem_ccm_detector = ConditionalExperimentalMode(planner_state)
+  ccm = ConditionalChillMode(planner_state, cem_ccm_detector)
+  hybrid = HybridExperimentalMode()
+  hybrid.set_tuning(toggles.hybrid_exp_bias, toggles.hybrid_vision_brake_sensitivity)
+
+  # Full openpilot Longitudinal Planner for true Chill (ACC) MPC execution
+  chill_long_planner = LongitudinalPlanner(cp_candidate, dt=DT_MDL)
+
+  n = len(grid)
+  out = {
+    "t": grid,
+    "v_ego": np.zeros(n),
+    "v_cruise": np.zeros(n),
+    "model_v0": np.zeros(n),
+    "lead_status": np.zeros(n, dtype=bool),
+    "lead_dRel": np.zeros(n),
+    "lead_vLead": np.zeros(n),
+    "cem_exp": np.zeros(n, dtype=bool),
+    "cem_status": np.zeros(n, dtype=int),
+    "ccm_exp": np.zeros(n, dtype=bool),
+    "ccm_status": np.zeros(n, dtype=int),
+    "chill_active": np.zeros(n, dtype=bool),
+    "logged_exp": np.zeros(n, dtype=bool),
+    "logged_aTarget": np.zeros(n),
+    "a_chill": np.zeros(n),
+    "a_exp": np.zeros(n),
+    "hem_a": np.zeros(n),
+    "hem_authority": np.zeros(n),
+  }
+
+  real_monotonic = time.monotonic
+  fake_clock = [0.0]
+  time.monotonic = lambda: fake_clock[0]
+
+  try:
+    for i, t in enumerate(grid):
+      fake_clock[0] = float(i) * DT_MDL
+
+      car_raw = latest("carState", t)
+      v_ego_raw = max(float(getattr(car_raw, "vEgo", 0.0)), 0.0)
+      car = SimpleNamespace(
+        vEgo=v_ego_raw,
+        vEgoCluster=max(float(getattr(car_raw, "vEgoCluster", v_ego_raw)), 0.0),
+        vCruise=float(getattr(car_raw, "vCruise", 0.0)),
+        standstill=bool(getattr(car_raw, "standstill", False)),
+        leftBlinker=bool(getattr(car_raw, "leftBlinker", False)),
+        rightBlinker=bool(getattr(car_raw, "rightBlinker", False)),
+        gasPressed=bool(getattr(car_raw, "gasPressed", False)),
+        brakePressed=bool(getattr(car_raw, "brakePressed", False)),
+        steeringAngleDeg=float(getattr(car_raw, "steeringAngleDeg", 0.0)),
+        aEgo=float(getattr(car_raw, "aEgo", 0.0)),
+        leftBlindspot=bool(getattr(car_raw, "leftBlindspot", False)),
+        rightBlindspot=bool(getattr(car_raw, "rightBlindspot", False)),
+      )
+      v_ego = car.vEgo
+
+      radar = latest("radarState", t)
+      lead = as_lead(getattr(radar, "leadOne", None) if radar is not None else None)
+      lead2 = as_lead(getattr(radar, "leadTwo", None) if radar is not None else None)
+
+      sradar = latest("starpilotRadarState", t)
+      lead_left = as_lead(getattr(sradar, "leadLeft", None) if sradar is not None else None)
+      lead_right = as_lead(getattr(sradar, "leadRight", None) if sradar is not None else None)
+
+      lplan = latest("longitudinalPlan", t)
+      sds = latest("selfdriveState", t)
+      splan = latest("starpilotPlan", t)
+      scs = latest("starpilotCarState", t)
+      model_v2 = latest("modelV2", t)
+      live_params = latest("liveParameters", t)
+      car_control = latest("carControl", t)
+      controls_state = latest("controlsState", t)
+      car_params = latest("carParams", t) or cp_candidate
+
+      controls_enabled = bool(getattr(sds, "enabled", False))
+      conditional_tracking_active = controls_enabled or bool(getattr(scs, "alwaysOnLateralEnabled", False))
+
+      v_cruise = float(getattr(splan, "vCruise", 0.0) or 0.0)
+      if not (v_cruise > 0):
+        v_cruise_kph = float(getattr(car_raw, "vCruise", 0.0) or 0.0)
+        if 0 < v_cruise_kph < V_CRUISE_UNSET:
+          v_cruise = min(v_cruise_kph, V_CRUISE_MAX) * CV.KPH_TO_MS
+        else:
+          v_cruise = v_ego
+
+      tracking_lead = bool(getattr(splan, "trackingLead", False))
+      if not tracking_lead:
+        tracking_lead = bool(getattr(lplan, "hasLead", False))
+
+      model_length = 0.0
+      if model_v2 is not None:
+        try:
+          pos_x = getattr(model_v2, "position", None)
+          if pos_x is not None and len(pos_x.x):
+            model_length = float(pos_x.x[-1])
+        except Exception:
+          model_length = 0.0
+
+      forcing_stop = bool(getattr(splan, "forcingStop", False))
+      raw_model_stopped = model_length < CRUISING_SPEED * PLANNER_TIME
+      model_stopped = raw_model_stopped or forcing_stop
+
+      road_curvature_detected = False
+      if model_v2 is not None:
+        try:
+          road_curvature, _time_to_curve = calculate_road_curvature(model_v2, v_ego)
+          road_curvature_detected = (
+            (1 / abs(road_curvature)) ** 0.5 < v_ego > CRUISING_SPEED
+            and not (car.leftBlinker or car.rightBlinker)
+          )
+        except Exception:
+          road_curvature_detected = False
+
+      try:
+        curvature = float(getattr(controls_state, "curvature", 0.0))
+      except Exception:
+        curvature = 0.0
+      lateral_accel = v_ego ** 2 * curvature
+      driving_in_curve = abs(lateral_accel) >= MINIMUM_LATERAL_ACCELERATION
+
+      lane_width_left = 0.0
+      lane_width_right = 0.0
+      if model_v2 is not None:
+        try:
+          lane_lines = getattr(model_v2, "laneLines", None)
+          road_edges = getattr(model_v2, "roadEdges", None)
+          if lane_lines is not None and len(lane_lines) >= 4:
+            edge_left = road_edges[0] if road_edges is not None and len(road_edges) else None
+            edge_right = road_edges[1] if road_edges is not None and len(road_edges) > 1 else None
+            lane_width_left = calculate_lane_width(lane_lines[0], lane_lines[1], edge_left)
+            lane_width_right = calculate_lane_width(lane_lines[3], lane_lines[2], edge_right)
+        except Exception:
+          pass
+
+      t_follow = float(getattr(splan, "tFollow", 1.45))
+      following_lead = tracking_lead and lead.dRel < (t_follow * 2) * v_ego
+      slower_lead = False
+      if (conditional_tracking_active and tracking_lead and toggles.conditional_slower_lead
+          and lead.status and lead.vLead < v_ego):
+        distance_factor = max(lead.dRel - (lead.vLead * t_follow), 1)
+        braking_offset = float(np.clip(min(v_ego - lead.vLead, lead.vLead) - COMFORT_BRAKE, 1, distance_factor))
+        slower_lead = braking_offset > 1
+
+      planner_state.lead_one = lead
+      planner_state.tracking_lead = tracking_lead
+      planner_state.model_length = model_length
+      planner_state.raw_model_stopped = raw_model_stopped
+      planner_state.model_stopped = model_stopped
+      planner_state.road_curvature_detected = road_curvature_detected
+      planner_state.driving_in_curve = driving_in_curve
+      planner_state.lane_width_left = lane_width_left
+      planner_state.lane_width_right = lane_width_right
+      planner_state.starpilot_following.following_lead = following_lead
+      planner_state.starpilot_following.slower_lead = slower_lead
+      planner_state.starpilot_vcruise.forcing_stop = forcing_stop
+
+      raw_personality = getattr(sds, "personality", None)
+      if hasattr(raw_personality, "raw"):
+        personality_val = int(raw_personality.raw)
+      elif isinstance(raw_personality, (int, float)):
+        personality_val = int(raw_personality)
+      else:
+        personality_val = 0
+
+      long_ctrl_state = getattr(controls_state, "longControlState", LongCtrlState.pid)
+
+      sm_dict = {
+        "carState": car,
+        "radarState": SimpleNamespace(leadOne=lead, leadTwo=lead2),
+        "starpilotRadarState": SimpleNamespace(leadLeft=lead_left, leadRight=lead_right),
+        "starpilotCarState": SimpleNamespace(
+          trafficModeEnabled=bool(getattr(scs, "trafficModeEnabled", False)),
+          alwaysOnLateralEnabled=bool(getattr(scs, "alwaysOnLateralEnabled", False)),
+          dashboardStopSign=int(getattr(scs, "dashboardStopSign", 0)),
+          accelPressed=bool(getattr(scs, "accelPressed", False)),
+        ),
+        "selfdriveState": SimpleNamespace(
+          enabled=controls_enabled,
+          experimentalMode=False,  # Enforce ACC/Chill mode evaluation in MPC
+          personality=personality_val,
+        ),
+        "longitudinalPlan": SimpleNamespace(
+          hasLead=bool(getattr(lplan, "hasLead", False)),
+          allowThrottle=bool(getattr(lplan, "allowThrottle", True)),
+          shouldStop=bool(getattr(lplan, "shouldStop", False)),
+          aTarget=float(getattr(lplan, "aTarget", 0.0)),
+        ),
+        "starpilotPlan": SimpleNamespace(
+          vCruise=v_cruise,
+          tFollow=t_follow,
+          trackingLead=tracking_lead,
+          redLight=bool(getattr(splan, "redLight", False)),
+          forcingStop=forcing_stop,
+          forcingStopLength=float(getattr(splan, "forcingStopLength", 100.0)),
+          minAcceleration=float(getattr(splan, "minAcceleration", -3.5)),
+          maxAcceleration=float(getattr(splan, "maxAcceleration", 1.5)),
+          accelerationJerk=float(getattr(splan, "accelerationJerk", 1.0)),
+          dangerJerk=float(getattr(splan, "dangerJerk", 1.0)),
+          speedJerk=float(getattr(splan, "speedJerk", 1.0)),
+          dangerFactor=float(getattr(splan, "dangerFactor", 1.0)),
+          disableThrottle=bool(getattr(splan, "disableThrottle", False)),
+        ),
+        "controlsState": SimpleNamespace(
+          longControlState=long_ctrl_state,
+          forceDecel=bool(getattr(controls_state, "forceDecel", False)),
+          curvature=curvature,
+        ),
+        "liveParameters": SimpleNamespace(
+          angleOffsetDeg=float(getattr(live_params, "angleOffsetDeg", 0.0)) if live_params else 0.0,
+        ),
+        "carControl": SimpleNamespace(
+          orientationNED=getattr(car_control, "orientationNED", [0.0, 0.0, 0.0]) if car_control else [0.0, 0.0, 0.0],
+        ),
+        "modelV2": model_v2,
+        "carParams": car_params,
+      }
+
+      # Run CEM / CCM State Machines
+      if conditional_tracking_active:
+        cem.update(v_ego, sm_dict, toggles, v_cruise)
+        ccm.update(v_ego, v_cruise, sm_dict, toggles)
+      else:
+        cem.experimental_mode = False
+        cem.status_value = CEStatus["OFF"]
+        ccm.experimental_mode = True
+        ccm.status_value = CCStatus["OFF"]
+
+      chill_active = (not ccm.experimental_mode) and ccm.status_value in CC_AUTO_STATUSES
+
+      # Compute accurate MPC Chill (ACC) Acceleration
+      chill_long_planner.update(sm_dict, toggles)
+      a_chill = float(chill_long_planner.output_a_target)
+
+      # Determine realistic Exp acceleration target
+      a_exp = 0.0
+      if model_v2 is not None:
+        try:
+          action_obj = getattr(model_v2, "action", None)
+          if action_obj is not None and hasattr(action_obj, "desiredAcceleration"):
+            a_exp = float(action_obj.desiredAcceleration)
+          else:
+            accel_x = getattr(model_v2, "acceleration", None)
+            if accel_x is not None and len(accel_x.x):
+              a_exp = float(accel_x.x[0])
+        except Exception:
+          a_exp = 0.0
+
+      # Hybrid Experimental Mode continuous fusion
+      a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp)
+      authority = hybrid.exp_authority
+
+      model_v0 = v_ego
+      if model_v2 is not None:
+        try:
+          vel_x = getattr(model_v2, "velocity", None)
+          if vel_x is not None and len(vel_x.x):
+            model_v0 = float(vel_x.x[0])
+        except Exception:
+          model_v0 = v_ego
+
+      out["v_ego"][i] = v_ego
+      out["v_cruise"][i] = v_cruise
+      out["model_v0"][i] = model_v0
+      out["lead_status"][i] = lead.status
+      out["lead_dRel"][i] = lead.dRel
+      out["lead_vLead"][i] = lead.vLead
+      out["cem_exp"][i] = bool(cem.experimental_mode)
+      out["cem_status"][i] = int(cem.status_value)
+      out["ccm_exp"][i] = bool(ccm.experimental_mode)
+      out["ccm_status"][i] = int(ccm.status_value)
+      out["chill_active"][i] = chill_active
+      out["logged_exp"][i] = bool(getattr(sds, "experimentalMode", False))
+      out["logged_aTarget"][i] = float(getattr(lplan, "aTarget", 0.0))
+      out["a_chill"][i] = a_chill
+      out["a_exp"][i] = a_exp
+      out["hem_a"][i] = a_hem
+      out["hem_authority"][i] = authority
+  finally:
+    time.monotonic = real_monotonic
+
+  return out
+
+
+def intervals_for(t, keys):
+  """Group consecutive identical non-zero status keys into [start, end, key] spans."""
+  spans = []
+  cur = 0
+  for i, k in enumerate(keys):
+    if k and k == cur:
+      spans[-1][1] = t[i]
+    elif k:
+      spans.append([t[i], t[i], k])
+      cur = k
+    else:
+      cur = 0
+  return spans
+
+
+def print_summary(out):
+  t = out["t"]
+  n = len(t)
+  if n == 0:
+    print("No simulation frames to summarize.")
+    return
+
+  v_ego_mph = out["v_ego"] * CV.MS_TO_MPH
+  v_cruise_mph = out["v_cruise"] * CV.MS_TO_MPH
+  standstill_mask = out["v_ego"] < 0.2
+  standstill_pct = 100.0 * np.mean(standstill_mask)
+
+  stops = 0
+  for i in range(1, n):
+    if standstill_mask[i] and not standstill_mask[i - 1]:
+      stops += 1
+
+  cem_switches = int(np.sum(np.diff(out["cem_exp"].astype(int)) != 0))
+  ccm_switches = int(np.sum(np.diff(out["chill_active"].astype(int)) != 0))
+
+  print("\n" + "=" * 78)
+  print(f" MODE SIMULATION SUMMARY & TELEMETRY LOG  ({t[0]:.1f}s -> {t[-1]:.1f}s | {n} frames)")
+  print("=" * 78)
+
+  print("\n[1. DRIVE PROFILE]")
+  print(f"  • Speed (v_ego)     : Min {v_ego_mph.min():.1f} mph | Avg {v_ego_mph.mean():.1f} mph | Max {v_ego_mph.max():.1f} mph")
+  print(f"  • Cruise Set Speed  : Min {v_cruise_mph.min():.1f} mph | Avg {v_cruise_mph.mean():.1f} mph | Max {v_cruise_mph.max():.1f} mph")
+  print(f"  • Standstill Time   : {standstill_pct:.1f}% ({stops} full stop events)")
+  print(f"  • Lead Presence     : {100.0 * np.mean(out['lead_status']):.1f}% of route")
+
+  print("\n[2. MODE INTENT & TRIGGER BREAKDOWN]")
+  print(f"  • Logged (Actual)   : Exp {100.0 * out['logged_exp'].mean():.1f}%")
+  print(f"  • CEM (Conditional) : Exp {100.0 * out['cem_exp'].mean():.1f}% | {cem_switches} mode toggles")
+  cem_reasons = []
+  for status_code, label in CE_LABELS.items():
+    pct = 100.0 * np.mean((out["cem_status"] == status_code) & out["cem_exp"])
+    if pct > 0.1:
+      cem_reasons.append(f"{label}: {pct:.1f}%")
+  if cem_reasons:
+    print(f"    └─ CEM Active Triggers -> {', '.join(cem_reasons)}")
+
+  print(f"  • Chill Mode (CCM)  : Chill {100.0 * out['chill_active'].mean():.1f}% | Exp {100.0 * out['ccm_exp'].mean():.1f}% | {ccm_switches} toggles")
+  ccm_reasons = []
+  for status_code, label in CC_LABELS.items():
+    pct = 100.0 * np.mean((out["ccm_status"] == status_code) & out["chill_active"])
+    if pct > 0.1:
+      ccm_reasons.append(f"{label}: {pct:.1f}%")
+  if ccm_reasons:
+    print(f"    └─ Chill Active Reasons -> {', '.join(ccm_reasons)}")
+
+  auth = out["hem_authority"]
+  chill_dom = 100.0 * np.mean(auth < 0.35)
+  blended = 100.0 * np.mean((auth >= 0.35) & (auth <= 0.65))
+  exp_dom = 100.0 * np.mean(auth > 0.65)
+  print(f"  • HEM (Hybrid)      : Mean Exp Authority {auth.mean():.2f}")
+  print(f"    └─ Distribution   -> Chill-dominant (<0.35): {chill_dom:.1f}% | Blended (0.35-0.65): {blended:.1f}% | Exp-dominant (>0.65): {exp_dom:.1f}%")
+
+  print("\n[3. ACCELERATION ENVELOPE (m/s²)]")
+  print(f"  {'Signal':<16} | {'Min (Max Brake)':<15} | {'Mean':<10} | {'Max (Max Throttle)':<18}")
+  print("  " + "-" * 66)
+  print(f"  {'Logged aTarget':<16} | {out['logged_aTarget'].min():<15.2f} | {out['logged_aTarget'].mean():<10.2f} | {out['logged_aTarget'].max():<18.2f}")
+  print(f"  {'Chill a':<16} | {out['a_chill'].min():<15.2f} | {out['a_chill'].mean():<10.2f} | {out['a_chill'].max():<18.2f}")
+  print(f"  {'Exp a':<16} | {out['a_exp'].min():<15.2f} | {out['a_exp'].mean():<10.2f} | {out['a_exp'].max():<18.2f}")
+  print(f"  {'HEM Fused a':<16} | {out['hem_a'].min():<15.2f} | {out['hem_a'].mean():<10.2f} | {out['hem_a'].max():<18.2f}")
+
+  print("\n[4. KEY EVENT CHRONOLOGY (Sampled Significant Transitions)]")
+  print(f"  {'Time (s)':<9} | {'vEgo':<8} | {'CEM Status':<12} | {'CCM':<7} | {'a_chill':<8} | {'a_exp':<8} | {'HEM a':<8} | {'Auth':<5} | Event Context")
+  print("  " + "-" * 95)
+
+  step = max(1, n // 18)
+  sample_indices = set(range(0, n, step))
+
+  hard_brakes = np.where(out["hem_a"] < -1.2)[0]
+  if len(hard_brakes) > 0:
+    sample_indices.update(hard_brakes[::max(1, len(hard_brakes)//4)])
+
+  standstills = np.where(standstill_mask)[0]
+  if len(standstills) > 0:
+    sample_indices.update(standstills[::max(1, len(standstills)//3)])
+
+  sorted_indices = sorted(list(sample_indices))[:22]
+
+  for idx in sorted_indices:
+    cur_t = t[idx] - t[0]
+    ego_mph = v_ego_mph[idx]
+    cem_st = CE_LABELS.get(out["cem_status"][idx], "OFF") if out["cem_exp"][idx] else "CHILL"
+    ccm_st = "CHILL" if out["chill_active"][idx] else "EXP"
+    ac = out["a_chill"][idx]
+    ae = out["a_exp"][idx]
+    ah = out["hem_a"][idx]
+    au = out["hem_authority"][idx]
+
+    ctx = []
+    if out["lead_status"][idx]:
+      ctx.append(f"Lead {out['lead_dRel'][idx]:.0f}m")
+    if ego_mph < 1.0:
+      ctx.append("Standstill")
+    elif ah < -1.0:
+      ctx.append("Braking")
+    elif ah > 0.8:
+      ctx.append("Accelerating")
+    else:
+      ctx.append("Cruising")
+
+    if abs(ac - ae) > 1.0:
+      ctx.append("Disagreement")
+
+    context_str = ", ".join(ctx)
+    print(f"  {cur_t:<9.1f} | {ego_mph:<5.1f}mph | {cem_st:<12} | {ccm_st:<7} | {ac:<8.2f} | {ae:<8.2f} | {ah:<8.2f} | {au:<5.2f} | {context_str}")
+
+  print("=" * 78 + "\n")
+
+
+def plot_results(out, toggles, args):
+  import matplotlib
+  if not args.show:
+    matplotlib.use("Agg")
+  import matplotlib.pyplot as plt
+
+  t = out["t"]
+  n = len(t)
+
+  fig, (ax_speed, ax_mode, ax_accel) = plt.subplots(
+    3, 1, figsize=(14, 10), sharex=True,
+    gridspec_kw={"height_ratios": [2, 3, 2]},
+  )
+  fig.suptitle(f"Mode scenario estimate  {args.route}  seg {args.segment}", fontsize=11)
+
+  ax_speed.plot(t, out["v_ego"] * CV.MS_TO_MPH, color="black", lw=1.4, label="v_ego")
+  ax_speed.plot(t, out["v_cruise"] * CV.MS_TO_MPH, color="tab:blue", lw=1.0, ls="--", label="v_cruise")
+  ax_speed.plot(t, out["model_v0"] * CV.MS_TO_MPH, color="tab:green", lw=1.0, alpha=0.7, label="model v[0]")
+  ax_speed.set_ylabel("mph")
+  ax_speed.legend(loc="upper right", fontsize=8, ncol=3)
+  ax_speed.grid(alpha=0.3)
+
+  rows = [
+    ("Exp (always)", out["t"], np.ones(n, dtype=bool), "tab:blue", 3.0),
+    ("CEM", out["t"], out["cem_exp"], "tab:orange", 2.0),
+    ("Chill", out["t"], out["chill_active"], "tab:green", 1.0),
+    ("Logged", out["t"], out["logged_exp"], "tab:gray", 0.0),
+  ]
+  for label, tt, active, color, ycenter in rows:
+    ax_mode.fill_between(tt, ycenter - 0.28, ycenter + 0.28, where=active,
+                         step="post", color=color, alpha=0.55, edgecolor="none")
+
+  cem_spans = intervals_for(t, [out["cem_status"][i] if out["cem_exp"][i] else 0 for i in range(n)])
+  for a, b, k in cem_spans:
+    if b - a > 0.8 and k in CE_LABELS:
+      ax_mode.text((a + b) / 2, 2.0 + 0.34, CE_LABELS[k], ha="center", fontsize=7, color="tab:orange")
+  cc_spans = intervals_for(t, [out["ccm_status"][i] if out["chill_active"][i] else 0 for i in range(n)])
+  for a, b, k in cc_spans:
+    if b - a > 0.8 and k in CC_LABELS:
+      ax_mode.text((a + b) / 2, 1.0 + 0.34, CC_LABELS[k], ha="center", fontsize=7, color="tab:green")
+
+  ax_hem = ax_mode.twinx()
+  ax_hem.plot(t, out["hem_authority"], color="tab:cyan", lw=1.2, label="HEM exp-authority")
+  ax_hem.set_ylim(0, 1)
+  ax_hem.set_ylabel("HEM exp authority", color="tab:cyan", fontsize=8)
+  ax_hem.tick_params(axis="y", labelcolor="tab:cyan", labelsize=7)
+
+  ax_mode.set_yticks([3.0, 2.0, 1.0, 0.0])
+  ax_mode.set_yticklabels(["Exp", "CEM", "Chill", "Logged"])
+  ax_mode.set_ylim(-0.6, 3.6)
+  ax_mode.grid(axis="y", alpha=0.3)
+
+  ax_accel.plot(t, out["logged_aTarget"], color="tab:gray", lw=1.2, label="logged aTarget")
+  ax_accel.plot(t, out["a_chill"], color="tab:blue", lw=1.0, ls="--", label="chill (ACC) a")
+  ax_accel.plot(t, out["a_exp"], color="tab:orange", lw=1.0, ls=":", label="exp (model) a")
+  ax_accel.plot(t, out["hem_a"], color="tab:cyan", lw=1.4, label="HEM fused a")
+  ax_accel.set_ylabel("accel (m/s²)")
+  ax_accel.set_xlabel("time (s, segment-relative)")
+  ax_accel.legend(loc="upper right", fontsize=8, ncol=2)
+  ax_accel.grid(alpha=0.3)
+
+  fig.tight_layout()
+  if args.out:
+    out_path = Path(args.out)
+    out_path.parent.mkdir(parents=True, exist_ok=True)
+    fig.savefig(out_path, dpi=150)
+    print(f"Saved graph to {out_path}")
+  if args.show:
+    plt.show()
+  plt.close(fig)
+
+
+def main(argv=None):
+  parser = argparse.ArgumentParser(
+    description="Estimate CEM/Chill/HEM/Exp mode intent from a logged route and plot a comparison graph.")
+  parser.add_argument("route", help="route (e.g. dongle|2023-07-27--13-01-19 or dongle/2023-07-27--13-01-19/0)")
+  parser.add_argument("--segment", type=int, default=0, help="segment index (default 0)")
+  parser.add_argument("--data_dir", help="local directory containing route files")
+  parser.add_argument("--start", type=float, default=None, help="start seconds (segment-relative)")
+  parser.add_argument("--end", type=float, default=None, help="end seconds (segment-relative)")
+  parser.add_argument("--out", default="mode_sim.png", help="output PNG path")
+  parser.add_argument("--show", action="store_true", help="show the plot window instead of saving only")
+  parser.add_argument("--csv", default=None, help="optional CSV output of the time series")
+  parser.add_argument("--set", action="append", default=[], metavar="KEY=value",
+                      help="override a mode toggle, e.g. --set CESpeed=20 --set CEModelStopTime=3.0 --set CCMLead=false")
+  args = parser.parse_args(argv)
+
+  overrides = parse_toggle_overrides(args.set)
+  defaults = dict(TOGGLE_DEFAULTS)
+  defaults.update(overrides)
+  toggles = SimpleNamespace(**defaults)
+  for key, value in overrides.items():
+    print(f"toggle override: {key} = {value}")
+
+  identifiers = resolve_segment_identifier(args.route, args.segment, args.data_dir)
+  if not identifiers:
+    print(f"No segment data found for {args.route} seg {args.segment}", file=sys.stderr)
+    return 1
+  print(f"Loading: {identifiers}")
+  bufs = load_buffers(identifiers, SERVICES)
+  if not any(bufs[s] for s in SERVICES):
+    print("No messages loaded.", file=sys.stderr)
+    return 1
+
+  all_min = min(t for s in SERVICES for t, _ in bufs[s])
+  all_max = max(t for s in SERVICES for t, _ in bufs[s])
+  start = all_min if args.start is None else all_min + args.start
+  end = all_max if args.end is None else min(all_max, all_min + args.end)
+  if end - start < DT_MDL:
+    print(f"Empty time window [{start:.1f}, {end:.1f}]. Check --start/--end.", file=sys.stderr)
+    return 1
+  grid = np.arange(start, end, DT_MDL)
+  print(f"Window: {start - all_min:.1f}s -> {end - all_min:.1f}s ({len(grid)} frames at {1 / DT_MDL:.0f} Hz)")
+
+  out = run_simulation(grid, bufs, toggles)
+
+  if args.csv:
+    import csv
+    csv_path = Path(args.csv)
+    csv_path.parent.mkdir(parents=True, exist_ok=True)
+    with open(csv_path, "w", newline="") as f:
+      writer = csv.writer(f)
+      writer.writerow(["t", "v_ego", "v_cruise", "model_v0", "cem_exp", "cem_status",
+                       "ccm_exp", "ccm_status", "chill_active", "logged_exp", "logged_aTarget",
+                       "a_chill", "a_exp", "hem_a", "hem_authority"])
+      for i in range(len(out["t"])):
+        writer.writerow([out["t"][i], out["v_ego"][i], out["v_cruise"][i], out["model_v0"][i],
+                         int(out["cem_exp"][i]), out["cem_status"][i],
+                         int(out["ccm_exp"][i]), out["ccm_status"][i], int(out["chill_active"][i]),
+                         int(out["logged_exp"][i]), out["logged_aTarget"][i],
+                         out["a_chill"][i], out["a_exp"][i], out["hem_a"][i], out["hem_authority"][i]])
+    print(f"Saved CSV to {csv_path}")
+
+  print_summary(out)
+  plot_results(out, toggles, args)
+  return 0
+
+
+if __name__ == "__main__":
+  raise SystemExit(main())