From 175243af40fbf70e4fe13da62e6c89a0d41b10ed Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 20 Feb 2020 23:27:11 +0100
Subject: [PATCH] Draw leadTwo chevron in ui (#1133)

* Add leadTwo

* Add leadTwo

* Draw leadTwo car indicator

* Reuse leaddatad

* refactor draw_lead

No idea if this is the correct syntax

* Python is too good to us

Add ;

* Only show if more than 3m apart

* Delete unused scene
---
 selfdrive/ui/paint.cc | 37 +++++++++++++++++++++----------------
 selfdrive/ui/ui.cc    |  5 +++++
 selfdrive/ui/ui.hpp   |  3 +++
 3 files changed, 29 insertions(+), 16 deletions(-)

diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
index 6d7b24c346..49bc0cbdf1 100644
--- a/selfdrive/ui/paint.cc
+++ b/selfdrive/ui/paint.cc
@@ -108,9 +108,23 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz,
   nvgRestore(s->vg);
 }
 
-static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) {
-  const UIScene *scene = &s->scene;
+static void draw_lead(UIState *s, float d_rel, float v_rel, float y_rel){
+    // Draw lead car indicator
+    float fillAlpha = 0;
+    float speedBuff = 10.;
+    float leadBuff = 40.;
+    if (d_rel < leadBuff) {
+      fillAlpha = 255*(1.0-(d_rel/leadBuff));
+      if (v_rel < 0) {
+        fillAlpha += 255*(-1*(v_rel/speedBuff));
+      }
+      fillAlpha = (int)(fmin(fillAlpha, 255));
+    }
+    draw_chevron(s, d_rel+2.7, y_rel, 25,
+                  nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255));
+}
 
+static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) {
   nvgSave(s->vg);
   nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space
   nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x
@@ -204,7 +218,7 @@ static void update_all_track_data(UIState *s) {
 
 
 static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) {
-const UIScene *scene = &s->scene;
+  const UIScene *scene = &s->scene;
   const PathData path = scene->model.path;
   const float *mpc_x_coords = &scene->mpc_x[0];
   const float *mpc_y_coords = &scene->mpc_y[0];
@@ -390,19 +404,10 @@ static void ui_draw_world(UIState *s) {
   ui_draw_vision_lanes(s);
 
   if (scene->lead_status) {
-    // Draw lead car indicator
-    float fillAlpha = 0;
-    float speedBuff = 10.;
-    float leadBuff = 40.;
-    if (scene->lead_d_rel < leadBuff) {
-      fillAlpha = 255*(1.0-(scene->lead_d_rel/leadBuff));
-      if (scene->lead_v_rel < 0) {
-        fillAlpha += 255*(-1*(scene->lead_v_rel/speedBuff));
-      }
-      fillAlpha = (int)(fmin(fillAlpha, 255));
-    }
-    draw_chevron(s, scene->lead_d_rel+2.7, scene->lead_y_rel, 25,
-                  nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255));
+    draw_lead(s,scene->lead_d_rel,scene->lead_v_rel,scene->lead_y_rel);
+  }
+  if ((scene->lead_status2) && (fabs(scene->lead_d_rel - scene->lead_d_rel2) > 3.0)) {
+    draw_lead(s,scene->lead_d_rel2,scene->lead_v_rel2,scene->lead_y_rel2);
   }
 }
 
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index db28e373c3..f1fee9f32d 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -362,6 +362,11 @@ void handle_message(UIState *s, Message * msg) {
     s->scene.lead_d_rel = leaddatad.dRel;
     s->scene.lead_y_rel = leaddatad.yRel;
     s->scene.lead_v_rel = leaddatad.vRel;
+    cereal_read_RadarState_LeadData(&leaddatad, datad.leadTwo);
+    s->scene.lead_status2 = leaddatad.status;
+    s->scene.lead_d_rel2 = leaddatad.dRel;
+    s->scene.lead_y_rel2 = leaddatad.yRel;
+    s->scene.lead_v_rel2 = leaddatad.vRel;
     s->livempc_or_radarstate_changed = true;
   } else if (eventd.which == cereal_Event_liveCalibration) {
     s->scene.world_objects_visible = true;
diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp
index 65471c3632..64d32c65c1 100644
--- a/selfdrive/ui/ui.hpp
+++ b/selfdrive/ui/ui.hpp
@@ -115,6 +115,9 @@ typedef struct UIScene {
 
   int lead_status;
   float lead_d_rel, lead_y_rel, lead_v_rel;
+  
+  int lead_status2;
+  float lead_d_rel2, lead_y_rel2, lead_v_rel2;
 
   int front_box_x, front_box_y, front_box_width, front_box_height;