Merge branch 'master' into optional-turn-desire-nudge

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

common/params_keys.h

@@ -203,7 +203,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"LagdToggleDelay", {PERSISTENT | BACKUP, FLOAT, "0.2"}},
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
-    {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"LaneTurnDesire", {PERSISTENT | BACKUP, INT, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
 
     // mapd

selfdrive/controls/lib/desire_helper.py

@@ -1,4 +1,4 @@
-from cereal import log, custom
+from cereal import log
 from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode
@@ -31,12 +31,6 @@ DESIRES = {
   },
 }
 
-TURN_DESIRES = {
-  custom.TurnDirection.none: log.Desire.none,
-  custom.TurnDirection.turnLeft: log.Desire.turnLeft,
-  custom.TurnDirection.turnRight: log.Desire.turnRight,
-}
-
 
 class DesireHelper:
   def __init__(self):
@@ -48,8 +42,7 @@ class DesireHelper:
     self.prev_one_blinker = False
     self.desire = log.Desire.none
     self.alc = AutoLaneChangeController(self)
-    self.lane_turn_controller = LaneTurnController(self)
+    self.lane_turn_controller = LaneTurnController()
-    self.lane_turn_direction = custom.TurnDirection.none
 
   @staticmethod
   def get_lane_change_direction(CS):
@@ -57,16 +50,10 @@ class DesireHelper:
 
   def update(self, carstate, lateral_active, lane_change_prob):
     self.alc.update_params()
-    self.lane_turn_controller.update_params()
     v_ego = carstate.vEgo
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
     below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
 
-    # Lane turn controller update
-    self.lane_turn_controller.update_lane_turn(blindspot_left=carstate.leftBlindspot, blindspot_right=carstate.rightBlindspot,
-                                               left_blinker=carstate.leftBlinker, right_blinker=carstate.rightBlinker, v_ego=v_ego)
-    self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
-
     if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF:
       self.lane_change_state = LaneChangeState.off
       self.lane_change_direction = LaneChangeDirection.none
@@ -126,10 +113,13 @@ class DesireHelper:
 
     self.prev_one_blinker = one_blinker
 
-    if self.lane_turn_direction != custom.TurnDirection.none:
+    self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
-      self.desire = TURN_DESIRES[self.lane_turn_direction]
+
-    else:
+    # Lane turn controller update
-      self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+    turn_desire = self.lane_turn_controller.update(carstate.leftBlindspot, carstate.rightBlindspot, carstate.leftBlinker, carstate.rightBlinker,
+                                                   carstate.vEgo, carstate.steeringPressed, carstate.steeringTorque)
+    if turn_desire != log.Desire.none:
+      self.desire = turn_desire
 
     # Send keep pulse once per second during LaneChangeStart.preLaneChange
     if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):

selfdrive/modeld/modeld.py

@@ -398,7 +398,7 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
-      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
+      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_controller.turn_direction
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 

selfdrive/ui/sunnypilot/SConscript

@@ -48,6 +48,7 @@ qt_src = [
 lateral_panel_qt_src = [
   "sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc",
+  "sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",
 ]

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.cc

@@ -0,0 +1,86 @@
+/**
+ * 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.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.h"
+
+
+LaneTurnSettings::LaneTurnSettings(QWidget* parent) : QWidget(parent) {
+  QVBoxLayout* main_layout = new QVBoxLayout(this);
+  main_layout->setContentsMargins(50, 20, 50, 20);
+  main_layout->setSpacing(20);
+
+  // Back button
+  PanelBackButton* back = new PanelBackButton(tr("Back"));
+  connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
+  main_layout->addWidget(back, 0, Qt::AlignLeft);
+
+  ListWidgetSP *list = new ListWidgetSP(this, false);
+  // Lane Turn Desire Control
+  laneTurnDesireControl = new LaneTurnDesireControl();
+  laneTurnDesireControl->setUpdateOtherToggles(true);
+  laneTurnDesireControl->showDescription();
+  connect(laneTurnDesireControl, &OptionControlSP::updateLabels, laneTurnDesireControl, &LaneTurnDesireControl::refresh);
+  list->addItem(laneTurnDesireControl);
+
+  // Lane Turn Value control
+  bool is_metric = params.getBool("IsMetric");
+  int per_value_change = is_metric ? 62 : 100; // ~1 km/h or 1 mph
+  laneTurnValueControl = new OptionControlSP("LaneTurnValue", tr("Adjust Lane Turn Speed"),
+    tr("Set the maximum speed for lane turn desires. Default is 19 %1.").arg(is_metric ? "km/h" : "mph"),
+    "", {500, 2000}, per_value_change, false, nullptr, true, true);
+  laneTurnValueControl->showDescription();
+  list->addItem(laneTurnValueControl);
+
+  // show/hide value control based on desire
+  connect(laneTurnDesireControl, &OptionControlSP::updateLabels, this, &LaneTurnSettings::updateValueControlVisibility);
+  connect(laneTurnValueControl, &OptionControlSP::updateLabels, this, &LaneTurnSettings::refreshLaneTurnValueControl);
+  main_layout->addWidget(new ScrollViewSP(list, this));
+}
+
+void LaneTurnSettings::showEvent(QShowEvent *event) {
+  updateValueControlVisibility();
+}
+
+void LaneTurnSettings::updateValueControlVisibility() {
+  QString option = QString::fromStdString(params.get("LaneTurnDesire"));
+  bool visible = (option != "0");
+  laneTurnValueControl->setVisible(visible);
+  if (visible) {
+    refreshLaneTurnValueControl();
+  }
+}
+
+void LaneTurnSettings::refreshLaneTurnValueControl() {
+  if (!laneTurnValueControl) return;
+  float stored_mph = QString::fromStdString(params.get("LaneTurnValue")).toFloat();
+  bool is_metric = params.getBool("IsMetric");
+  QString unit = is_metric ? "km/h" : "mph";
+  float display_value = is_metric ? stored_mph * 1.609344f : stored_mph;
+  laneTurnValueControl->setLabel(QString::number(qRound(display_value)) + " " + unit);
+}
+
+// Lane Turn Desire Control
+LaneTurnDesireControl::LaneTurnDesireControl() : OptionControlSP(
+  "LaneTurnDesire",
+  tr("Lane Turn Desires"),
+  tr("If you're driving at 20 mph (32 km/h) or below and have your blinker on, "
+     "the car will plan a turn in that direction at the nearest drivable path. "
+     "This prevents situations (like at red lights) where the car might plan the wrong turn direction."),
+  "../assets/offroad/icon_shell.png",
+  {0, 2}) {
+  refresh();
+}
+
+void LaneTurnDesireControl::refresh() {
+  QString option = QString::fromStdString(params.get("LaneTurnDesire"));
+  static const QMap<QString, QString> options = {
+    {"0", tr("Off")},
+    {"1", tr("Nudge")},
+    {"2", tr("Nudgeless")},
+  };
+  setLabel(options.value(option, tr("Off")));
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.h

@@ -0,0 +1,45 @@
+/**
+ * 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.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
+
+class LaneTurnDesireControl : public OptionControlSP {
+  Q_OBJECT
+
+public:
+  LaneTurnDesireControl();
+
+  void refresh();
+
+private:
+  Params params;
+};
+
+class LaneTurnSettings : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit LaneTurnSettings(QWidget* parent = nullptr);
+  void showEvent(QShowEvent *event) override;
+
+signals:
+  void backPress();
+
+private:
+  void refreshLaneTurnValueControl();
+  void updateValueControlVisibility();
+
+private:
+  Params params;
+  std::map<std::string, ParamControlSP*> toggles;
+
+  LaneTurnDesireControl *laneTurnDesireControl;
+  OptionControlSP *laneTurnValueControl;
+};

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc

@@ -59,7 +59,27 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
     sunnypilotScroller->restoreScrollPosition();
     main_layout->setCurrentWidget(sunnypilotScreen);
   });
-  list->addItem(laneChangeSettingsButton);
+
+  // Lane Turn Settings
+  laneTurnSettingsButton = new PushButtonSP(tr("Customize Lane Turn"));
+  laneTurnSettingsButton->setObjectName("lane_turn_btn");
+  connect(laneTurnSettingsButton, &QPushButton::clicked, [=]() {
+    sunnypilotScroller->setLastScrollPosition();
+    main_layout->setCurrentWidget(laneTurnWidget);
+  });
+
+  laneTurnWidget = new LaneTurnSettings(this);
+  connect(laneTurnWidget, &LaneTurnSettings::backPress, [=]() {
+    sunnypilotScroller->restoreScrollPosition();
+    main_layout->setCurrentWidget(sunnypilotScreen);
+  });
+
+  QWidget *laneButtonsWidget = new QWidget();
+  QHBoxLayout *laneButtonsLayout = new QHBoxLayout(laneButtonsWidget);
+  laneButtonsLayout->setContentsMargins(0, 0, 0, 0);
+  laneChangeSettingsButton->setFixedWidth(750); laneTurnSettingsButton->setFixedWidth(750);
+  laneButtonsLayout->addWidget(laneChangeSettingsButton); laneButtonsLayout->addWidget(laneTurnSettingsButton);
+  list->addItem(laneButtonsWidget);
 
   list->addItem(vertical_space(0));
   list->addItem(horizontal_line());
@@ -100,6 +120,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
   main_layout->addWidget(sunnypilotScreen);
   main_layout->addWidget(madsWidget);
   main_layout->addWidget(laneChangeWidget);
+  main_layout->addWidget(laneTurnWidget);
 
   setStyleSheet(R"(
     #back_btn {

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h

@@ -15,6 +15,7 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
@@ -43,6 +44,8 @@ private:
   MadsSettings *madsWidget = nullptr;
   PushButtonSP *laneChangeSettingsButton;
   LaneChangeSettings *laneChangeWidget = nullptr;
+  PushButtonSP *laneTurnSettingsButton;
+  LaneTurnSettings *laneTurnWidget = nullptr;
   NeuralNetworkLateralControl *nnlcToggle = nullptr;
   BlinkerPauseLateralSettings *blinkerPauseLateralSettings = nullptr;
 

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc

@@ -103,30 +103,6 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
   list->addItem(policyFrame);
   list->addItem(horizontal_line());
 
-  // Lane Turn Desire toggle
-  lane_turn_desire_toggle = new ParamControlSP("LaneTurnDesire", tr("Use Lane Turn Desires"),
-                            "If you’re driving at 20 mph (32 km/h) or below and have your blinker on, "
-                            "the car will plan a turn in that direction at the nearest drivable path. "
-                            "This prevents situations (like at red lights) where the car might plan the wrong turn direction.",
-                             "../assets/offroad/icon_shell.png");
-  list->addItem(lane_turn_desire_toggle);
-
-  // Lane Turn Value control
-  int max_value_mph = 20;
-  bool is_metric_initial = params.getBool("IsMetric");
-  const float K = 1.609344f;
-  int per_value_change_scaled = is_metric_initial ? static_cast<int>(std::round((1.0f / K) * 100.0f)) : 100; // 100 -> 1 mph
-  lane_turn_value_control = new OptionControlSP("LaneTurnValue", tr("Adjust Lane Turn Speed"),
-    tr("Set the maximum speed for lane turn desires. Default is 19 %1.").arg(is_metric_initial ? "km/h" : "mph"),
-    "", {5 * 100, max_value_mph * 100}, per_value_change_scaled, false, nullptr, true, true);
-  lane_turn_value_control->showDescription();
-  list->addItem(lane_turn_value_control);
-
-  // Show based on toggle
-  refreshLaneTurnValueControl();
-  connect(lane_turn_desire_toggle, &ParamControlSP::toggleFlipped, this, &ModelsPanel::refreshLaneTurnValueControl);
-  connect(lane_turn_value_control, &OptionControlSP::updateLabels, this, &ModelsPanel::refreshLaneTurnValueControl);
-
   // LiveDelay toggle
   lagd_toggle_control = new ParamControlSP("LagdToggle", tr("Live Learning Steer Delay"), "", "../assets/offroad/icon_shell.png");
   lagd_toggle_control->showDescription();
@@ -169,23 +145,6 @@ QFrame* ModelsPanel::createModelDetailFrame(QWidget *parent, QString &typeName,
   return frame;
 }
 
-void ModelsPanel::refreshLaneTurnValueControl() {
-  if (!lane_turn_value_control) return;
-  float stored_mph = QString::fromStdString(params.get("LaneTurnValue")).toFloat();
-  bool is_metric = params.getBool("IsMetric");
-  QString unit = is_metric ? "km/h" : "mph";
-  float display_value = stored_mph;
-  if (is_metric) {
-    display_value = stored_mph * 1.609344f;
-  }
-  lane_turn_value_control->setLabel(QString::number(static_cast<int>(std::round(display_value))) + " " + unit);
-  lane_turn_value_control->setVisible(params.getBool("LaneTurnDesire"));
-}
-
-/**
- * @brief Updates the UI with bundle download progress information
- * Reads status from modelManagerSP cereal message and displays status for all models
- */
 void ModelsPanel::handleBundleDownloadProgress() {
   supercomboFrame->setVisible(false);
   visionFrame->setVisible(false);
@@ -456,9 +415,6 @@ void ModelsPanel::updateLabels() {
     delay_control->setLabel(QString::number(value, 'f', 2) + "s");
   }
 
-  // Update lane turn desire label and visibility
-  refreshLaneTurnValueControl();
-
   clearModelCacheBtn->setValue(QString::number(calculateCacheSize(), 'f', 2) + " MB");
 }
 

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h

@@ -38,7 +38,6 @@ private:
   void updateLabels();
   void handleCurrentModelLblBtnClicked();
   void handleBundleDownloadProgress();
-  void refreshLaneTurnValueControl();
   void showResetParamsDialog();
   QProgressBar* createProgressBar(QWidget *parent);
   QFrame* createModelDetailFrame(QWidget *parent, QString &typeName, QProgressBar *progressBar);
@@ -83,6 +82,4 @@ private:
   Params params;
   ButtonControlSP *clearModelCacheBtn;
   ButtonControlSP *refreshAvailableModelsBtn;
-  ParamControlSP *lane_turn_desire_toggle;
-  OptionControlSP *lane_turn_value_control;
 };

sunnypilot/modeld/modeld.py

@@ -317,7 +317,7 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
-      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
+      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_controller.turn_direction
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 

sunnypilot/modeld_v2/modeld.py

@@ -343,7 +343,7 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
-      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
+      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_controller.turn_direction
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 

sunnypilot/selfdrive/controls/lib/lane_turn_desire.py

@@ -4,25 +4,36 @@ 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 cereal import custom, log
 
 from openpilot.common.constants import CV
 from openpilot.common.params import Params
 
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 
+TURN_STATE = {
+  'OFF': 0,
+  'NUDGE': 1,
+  'NUDGELESS': 2
+}
+
+TURN_DESIRES = {
+  custom.TurnDirection.none: log.Desire.none,
+  custom.TurnDirection.turnLeft: log.Desire.turnLeft,
+  custom.TurnDirection.turnRight: log.Desire.turnRight,
+}
+
 
 class LaneTurnController:
-  def __init__(self, desire_helper):
+  def __init__(self):
-    self.DH = desire_helper
     self.turn_direction = custom.TurnDirection.none
     self.params = Params()
     self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
     self.param_read_counter = 0
-    self.enabled = self.params.get_bool("LaneTurnDesire")
+    self.lane_turn_type = self.params.get("LaneTurnDesire", return_default=True)
 
   def read_params(self):
-    self.enabled = self.params.get_bool("LaneTurnDesire")
+    self.lane_turn_type = self.params.get("LaneTurnDesire", return_default=True)
     value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
     self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value)
 
@@ -32,14 +43,30 @@ class LaneTurnController:
     self.param_read_counter += 1
 
   def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None:
-    if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left:
+    if self.lane_turn_type != TURN_STATE['OFF'] and left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left:
       self.turn_direction = custom.TurnDirection.turnLeft
-    elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right:
+    elif self.lane_turn_type != TURN_STATE['OFF'] and right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right:
       self.turn_direction = custom.TurnDirection.turnRight
     else:
       self.turn_direction = custom.TurnDirection.none
 
-  def get_turn_direction(self):
+  def return_desire(self, steering_pressed: bool, steering_torque: float) -> log.Desire:
-    if not self.enabled:
+    # disabled or no turn -> return none
-      return custom.TurnDirection.none
+    if self.lane_turn_type == TURN_STATE['OFF'] or self.turn_direction == custom.TurnDirection.none:
-    return self.turn_direction
+      return log.Desire.none
+
+    if self.lane_turn_type == TURN_STATE['NUDGE']:
+      # In nudge mode, require steering press for turn desire
+      turn_torque_applied = steering_pressed and ((steering_torque > 0 and self.turn_direction == custom.TurnDirection.turnLeft) or
+                             (steering_torque < 0 and self.turn_direction == custom.TurnDirection.turnRight))
+      if not turn_torque_applied:
+        return log.Desire.none
+
+    return TURN_DESIRES[self.turn_direction]
+
+  def update(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float,
+             steering_pressed: bool, steering_torque: float) -> log.Desire:
+    self.update_params()
+    self.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego)
+    turn_desire = self.return_desire(steering_pressed, steering_torque)
+    return turn_desire

sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -1,77 +1,68 @@
 import pytest
-from cereal import log
+import numpy as np
+from cereal import custom, log
 from openpilot.common.params import Params
 
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
+from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN, TURN_STATE
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
 
 
-class TurnDirection:
-    none = 0
-    turnLeft = 1
-    turnRight = 2
-
-
 @pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [
-    (True, False, 5, False, False, TurnDirection.turnLeft),
+    (True, False, 5, False, False, custom.TurnDirection.turnLeft),
-    (False, True, 6, False, False, TurnDirection.turnRight),
+    (False, True, 6, False, False, custom.TurnDirection.turnRight),
-    (True, False, 9, False, False, TurnDirection.none),
+    (True, False, 9, False, False, custom.TurnDirection.none),
-    (True, False, 7, True, False, TurnDirection.none),
+    (True, False, 7, True, False, custom.TurnDirection.none),
-    (False, True, 6, False, True, TurnDirection.none),
+    (False, True, 6, False, True, custom.TurnDirection.none),
-    (False, False, 5, False, False, TurnDirection.none),
+    (False, False, 5, False, False, custom.TurnDirection.none),
-    (True, True, 5, False, False, TurnDirection.none),
+    (True, True, 5, False, False, custom.TurnDirection.none),
-])
+], ids=["left blinker", "right blinker", "no blinkers", "left blindspot w/ left blinker", "right blindspot w/ left blinker",
+        "no blinkers", "both blinkers"])
 def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected):
-    dh = DesireHelper()
+    controller = LaneTurnController()
-    controller = LaneTurnController(dh)
+    controller.lane_turn_type = TURN_STATE['NUDGELESS']
-    controller.enabled = True
     controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
-    controller.turn_direction = TurnDirection.none
+    controller.turn_direction = custom.TurnDirection.none
     controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego)
-    assert controller.get_turn_direction() == expected
+    assert np.equal(controller.turn_direction, expected), f"Expected {expected}, speed {v_ego}, got {controller.turn_direction}"
 
 
 def test_lane_turn_desire_disabled():
-    dh = DesireHelper()
+    controller = LaneTurnController()
-    controller = LaneTurnController(dh)
+    controller.lane_turn_type = TURN_STATE['OFF']
-    controller.enabled = False
     controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
-    controller.turn_direction = TurnDirection.none
+    controller.turn_direction = custom.TurnDirection.none
-    controller.update_lane_turn(False, False, True, False, 7)
+    controller.update_lane_turn(False, False, True, False, 6)
-    assert controller.get_turn_direction() == TurnDirection.none
+    assert np.equal(controller.turn_direction , custom.TurnDirection.none), f"Expected no turn desire, got {controller.turn_direction}"
 
 
 def test_lane_turn_overrides_lane_change():
-    dh = DesireHelper()
+    controller = LaneTurnController()
-    controller = LaneTurnController(dh)
+    controller.lane_turn_type = TURN_STATE['NUDGELESS']
-    controller.enabled = True
     controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
-    controller.turn_direction = TurnDirection.none
+    controller.turn_direction = custom.TurnDirection.none
-    # left turn desire
     controller.update_lane_turn(False, False, True, False, 5)
-    assert controller.get_turn_direction() == TurnDirection.turnLeft
+    assert np.equal(controller.turn_direction, custom.TurnDirection.turnLeft), f"Expected left turn desire, got {controller.turn_direction}"
-    # right turn desire
+
     controller.update_lane_turn(False, False, False, True, 6)
-    assert controller.get_turn_direction() == TurnDirection.turnRight
+    assert np.equal(controller.turn_direction, custom.TurnDirection.turnRight), f"Expected right turn desire, got {controller.turn_direction}"
-    # no turn
+
     controller.update_lane_turn(False, False, False, False, 7)
-    assert controller.get_turn_direction() == TurnDirection.none
+    assert np.equal(controller.turn_direction, custom.TurnDirection.none), f"Expected no turn desire, got {controller.turn_direction}"
 
 
 @pytest.mark.parametrize("v_ego,expected", [
-    (8.93, TurnDirection.turnLeft),   # just below threshold
+    (8.93, custom.TurnDirection.turnLeft),   # just below threshold
-    (8.96, TurnDirection.none),       # above threshold
+    (8.96, custom.TurnDirection.none),       # above threshold
-    (8.95, TurnDirection.none),       # just above threshold
+    (8.95, custom.TurnDirection.none),       # just above threshold
-])
+], ids=["below threshold", "above threshold", "just above threshold"])
 def test_lane_turn_desire_speed_boundary(v_ego, expected):
-    dh = DesireHelper()
+    controller = LaneTurnController()
-    controller = LaneTurnController(dh)
+    controller.lane_turn_type = TURN_STATE['NUDGELESS']
-    controller.enabled = True
     controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
-    controller.turn_direction = TurnDirection.none
+    controller.turn_direction = custom.TurnDirection.none
     controller.update_lane_turn(False, True, True, False, v_ego)
-    assert controller.get_turn_direction() == expected
+    assert np.equal(controller.turn_direction, expected), f"Expected {expected} at speed {v_ego}, got {controller.turn_direction}"
 
 
 class DummyCarState:
@@ -86,28 +77,43 @@ class DummyCarState:
         self.steeringTorque = steeringTorque
         self.brakePressed = brakePressed
 
-@pytest.fixture
-def set_lane_turn_params():
-    params = Params()
-    params.put("LaneTurnDesire", True)
-    params.put("LaneTurnValue", 20.0)
 
-@pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [
+def nudgeless_params():
-    # Lane turn desire overrides lane change desire
+    params = Params()
-    (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnLeft),
+    params.put("LaneTurnDesire", TURN_STATE['NUDGELESS'])
-    (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnRight),
+    params.put("LaneTurnValue", 20.0)
-    # Lane change desire only (no turn desires)
+    params.put("AutoLaneChangeTimer", AutoLaneChangeMode.NUDGELESS)
-    (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+
-                   steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft),
+def nudge_params():
-    (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False,
+    params = Params()
-                   steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight),
+    params.put("LaneTurnDesire", TURN_STATE['NUDGE'])
-    # No desire (inactive)
+    params.put("LaneTurnValue", 20.0)
-    (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none),
+    params.put("AutoLaneChangeTimer", AutoLaneChangeMode.NUDGE)
-    (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none),  # No blinkers? no desire!
+
-])
+
-def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
+@pytest.mark.parametrize("carstate, expected_desire", [
+    (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), log.Desire.turnLeft),
+    (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), log.Desire.turnRight),
+    (DummyCarState(vEgo=5, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), log.Desire.none),
+    (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), log.Desire.none),
+], ids=["nudgeless left turn", "nudgeless right turn", "no blinkers", "both blinkers"])
+def test_lane_turn_nudgeless(carstate, expected_desire):
+    nudgeless_params()
     dh = DesireHelper()
-    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+    dh.update(carstate, True, 1.0)
-    for _ in range(10):
+    assert np.equal(dh.desire, expected_desire), f"Expected {expected_desire}, got {dh.desire}"
-        dh.update(carstate, lateral_active, lane_change_prob)
+
-    assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
+
+@pytest.mark.parametrize("carstate, expected_desire", [
+    (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+                   steeringPressed=True, steeringTorque=1), log.Desire.turnLeft),
+    (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False,
+                   steeringPressed=True, steeringTorque=-1), log.Desire.turnRight),
+    (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), log.Desire.none),
+    (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), log.Desire.none),
+], ids=["nudge left turn with input", "nudge right turn with input", "nudge left turn no input", "nudge right turn no input"])
+def test_lane_turn_nudge(carstate, expected_desire):
+    nudge_params()
+    dh = DesireHelper()
+    dh.update(carstate, True, 1.0)
+    assert np.equal(dh.desire, expected_desire), f"Expected {expected_desire}, got {dh.desire}"