From ea35f725280f2e397f88543775ecfdd5390b3a7b Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Wed, 9 Aug 2023 03:46:39 -0400
Subject: [PATCH] ui: driving path size scaling with MADS status (#223)

* ui: driving path size scaling with MADS status

* cleanup
---
 selfdrive/ui/ui.cc | 24 ++++++++++++++++++++++--
 selfdrive/ui/ui.h  | 10 ++++++++++
 2 files changed, 32 insertions(+), 2 deletions(-)

diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index a751c0e64d..0018869ef4 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -119,8 +119,8 @@ void update_model(UIState *s,
     max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
   }
   max_idx = get_path_length_idx(plan_position, max_distance);
-  update_line_data(s, plan_position, 0.9, 1.22, 1.22, &scene.track_vertices, max_idx, false);
-  update_line_data(s, plan_position, 1.0, 1.22, 1.22, &scene.track_edge_vertices, max_idx, false);
+  update_line_data(s, plan_position, 0.9 - scene.mads_path_range * scene.mads_path_scale, 1.22, 1.22, &scene.track_vertices, max_idx, false);
+  update_line_data(s, plan_position, 1.0 - scene.mads_path_range * scene.mads_path_scale - 0.1 * scene.mads_path_scale, 1.22, 1.22, &scene.track_edge_vertices, max_idx, false);
 }
 
 void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) {
@@ -276,12 +276,32 @@ void UIState::updateStatus() {
     auto controls_state = (*sm)["controlsState"].getControlsState();
     auto car_control = (*sm)["carControl"].getCarControl();
     auto car_state = (*sm)["carState"].getCarState();
+    auto mads_enabled = car_state.getMadsEnabled();
     auto state = controls_state.getState();
     if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) {
       status = STATUS_OVERRIDE;
     } else {
       status = car_state.getMadsEnabled() ? car_control.getLongActive() ? STATUS_ENGAGED : STATUS_MADS : STATUS_DISENGAGED;
     }
+
+    if (mads_enabled != last_mads_enabled) {
+      mads_path_state = true;
+    }
+    last_mads_enabled = mads_enabled;
+    if (mads_path_state) {
+      if (mads_enabled) {
+        mads_path_count = fmax(mads_path_count - 1, 0);
+        if (mads_path_count == 0) {
+          mads_path_state = false;
+        }
+      } else {
+        mads_path_count = fmin(mads_path_count + 1, mads_path_timestep);
+        if (mads_path_count == mads_path_timestep) {
+          mads_path_state = false;
+        }
+      }
+    }
+    scene.mads_path_scale = mads_path_count * (1 / mads_path_timestep);
   }
 
   // Handle onroad/offroad transition
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index 0b0fa508c9..07edbb0c6b 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -20,6 +20,8 @@ const int UI_BORDER_SIZE = 30;
 const int UI_HEADER_HEIGHT = 420;
 
 const QRect speed_sgn_rc(UI_BORDER_SIZE * 2, UI_BORDER_SIZE * 2.5 + 202, 184, 184);
+const float DRIVING_PATH_WIDE = 0.9;
+const float DRIVING_PATH_NARROW = 0.25;
 
 const int UI_FREQ = 20; // Hz
 const int BACKLIGHT_OFFROAD = 50;
@@ -209,6 +211,9 @@ typedef struct UIScene {
 
   // UI button sorting
   int gac_btn;
+
+  float mads_path_scale = DRIVING_PATH_WIDE - DRIVING_PATH_NARROW;
+  float mads_path_range = DRIVING_PATH_WIDE - DRIVING_PATH_NARROW;  // 0.9 - 0.25 = 0.65
 } UIScene;
 
 class UIState : public QObject {
@@ -251,6 +256,11 @@ private:
   bool started_prev = false;
   int prime_type = -1;
   uint64_t last_update_params_sidebar;
+
+  bool last_mads_enabled = false;
+  bool mads_path_state = false;
+  float mads_path_timestep = 4;  // UI runs at 20 Hz, therefore 0.2 second is [0.2 second / (1 / 20 Hz) = 4]
+  float mads_path_count = 4;     // UI runs at 20 Hz, therefore 0.2 second is [0.2 second / (1 / 20 Hz) = 4]
 };
 
 UIState *uiState();