Display openpilot logics for following lead vehicles

Added toggle to show openpilot's logic in regards to following lead vehicles.

selfdrive/ui/qt/onroad.cc

@@ -223,7 +223,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 
 // ExperimentalButton
 ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) {
-  setFixedSize(btn_size, btn_size);
+  setFixedSize(btn_size, btn_size + 10);
 
   engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size});
   experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size});
@@ -240,7 +240,7 @@ void ExperimentalButton::changeMode() {
   }
 }
 
-void ExperimentalButton::updateState(const UIState &s) {
+void ExperimentalButton::updateState(const UIState &s, bool leadInfo) {
   const auto cs = (*s.sm)["controlsState"].getControlsState();
   bool eng = cs.getEngageable() || cs.getEnabled() || scene.always_on_lateral_active;
   if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) {
@@ -250,12 +250,13 @@ void ExperimentalButton::updateState(const UIState &s) {
   }
 
   // FrogPilot variables
+  y_offset = leadInfo ? 10 : 0;
 }
 
 void ExperimentalButton::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   QPixmap img = experimental_mode ? experimental_img : engage_img;
-  drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !(engageable || scene.always_on_lateral_active)) ? 0.6 : 1.0);
+  drawIcon(p, QPoint(btn_size / 2, btn_size / 2 + y_offset), img, QColor(0, 0, 0, 166), (isDown() || !(engageable || scene.always_on_lateral_active)) ? 0.6 : 1.0);
 }
 
 
@@ -331,7 +332,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   status = s.status;
 
   // update engageability/experimental mode button
-  experimental_btn->updateState(s);
+  experimental_btn->updateState(s, leadInfo);
 
   // update DM icon
   auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState();
@@ -700,6 +701,28 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState
   }
   painter.drawPolygon(chevron, std::size(chevron));
 
+  // Add lead info
+  if (leadInfo) {
+    // Declare the variables
+    float lead_speed = std::max(lead_data.getVLead(), 0.0f);  // Ensure lead speed doesn't go under 0 m/s cause that's dumb
+
+    // Form the text and center it below the chevron
+    painter.setPen(Qt::white);
+    painter.setFont(InterFont(35, QFont::Bold));
+
+    QString text = QString("%1 %2 | %3 %4")
+                           .arg(qRound(d_rel * distanceConversion))
+                           .arg(leadDistanceUnit)
+                           .arg(qRound(lead_speed * speedConversion))
+                           .arg(leadSpeedUnit);
+
+    // Calculate the text starting position
+    QFontMetrics metrics(painter.font());
+    int middle_x = (chevron[2].x() + chevron[0].x()) / 2;
+    int textWidth = metrics.horizontalAdvance(text);
+    painter.drawText(middle_x - textWidth / 2, chevron[0].y() + metrics.height() + 5, text);
+  }
+
   painter.restore();
 }
 
@@ -859,11 +882,19 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
 
   experimentalMode = scene.experimental_mode;
 
+  leadInfo = scene.lead_info;
+  obstacleDistance = scene.obstacle_distance;
+  obstacleDistanceStock = scene.obstacle_distance_stock;
+
   mapOpen = scene.map_open;
 
   turnSignalLeft = scene.turn_signal_left;
   turnSignalRight = scene.turn_signal_right;
 
+  if (leadInfo) {
+    drawLeadInfo(p);
+  }
+
   if (alwaysOnLateral || conditionalExperimental) {
     drawStatusBar(p);
   }
@@ -912,6 +943,113 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
   }
 }
 
+void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
+  // Declare the variables
+  static QElapsedTimer timer;
+  static bool isFiveSecondsPassed = false;
+  constexpr int maxAccelDuration = 5000;
+
+  // Constants for units and conversions
+  QString accelerationUnit = " m/s²";
+  leadDistanceUnit = mapOpen ? "m" : "meters";
+  leadSpeedUnit = "m/2";
+
+  float accelerationConversion = 1.0f;
+  distanceConversion = 1.0f;
+  speedConversion = 1.0f;
+
+  if (!scene.use_si) {
+    if (is_metric) {
+      // Metric conversion
+      leadSpeedUnit = "kph";
+      speedConversion = MS_TO_KPH;
+    } else {
+      // US imperial conversion
+      accelerationUnit = " ft/s²";
+      leadDistanceUnit = mapOpen ? "ft" : "feet";
+      leadSpeedUnit = "mph";
+
+      accelerationConversion = METER_TO_FOOT;
+      distanceConversion = METER_TO_FOOT;
+      speedConversion = MS_TO_MPH;
+    }
+  }
+
+  // Update acceleration
+  double currentAcceleration = std::round(scene.acceleration * 100) / 100;
+  static double maxAcceleration = 0.0;
+
+  if (currentAcceleration > maxAcceleration && status == STATUS_ENGAGED) {
+    maxAcceleration = currentAcceleration;
+    isFiveSecondsPassed = false;
+    timer.start();
+  } else {
+    isFiveSecondsPassed = timer.hasExpired(maxAccelDuration);
+  }
+
+  // Construct text segments
+  auto createText = [&](const QString &title, const double data) {
+    return title + QString::number(std::round(data * distanceConversion)) + " " + leadDistanceUnit;
+  };
+
+  // Create segments for insights
+  QString accelText = QString("Accel: %1%2")
+    .arg(currentAcceleration * accelerationConversion, 0, 'f', 2)
+    .arg(accelerationUnit);
+
+  QString maxAccSuffix = QString(mapOpen ? "" : " - Max: %1%2")
+    .arg(maxAcceleration * accelerationConversion, 0, 'f', 2)
+    .arg(accelerationUnit);
+
+  QString obstacleText = createText(mapOpen ? " | Obstacle: " : "  |  Obstacle Factor: ", obstacleDistance);
+  QString stopText = createText(mapOpen ? " - Stop: " : "  -  Stop Factor: ", scene.stopped_equivalence);
+  QString followText = " = " + createText(mapOpen ? "Follow: " : "Follow Distance: ", scene.desired_follow);
+
+  // Check if the longitudinal toggles have an impact on the driving logics
+  auto createDiffText = [&](const double data, const double stockData) {
+    double difference = std::round((data - stockData) * distanceConversion);
+    return difference != 0 ? QString(" (%1%2)").arg(difference > 0 ? "+" : "").arg(difference) : QString();
+  };
+
+  // Prepare rectangle for insights
+  p.save();
+  QRect insightsRect(rect().left() - 1, rect().top() - 60, rect().width() + 2, 100);
+  p.setBrush(QColor(0, 0, 0, 150));
+  p.drawRoundedRect(insightsRect, 30, 30);
+  p.setFont(InterFont(30, QFont::DemiBold));
+  p.setRenderHint(QPainter::TextAntialiasing);
+
+  // Calculate positioning for text drawing
+  QRect adjustedRect = insightsRect.adjusted(0, 27, 0, 27);
+  int textBaseLine = adjustedRect.y() + (adjustedRect.height() + p.fontMetrics().height()) / 2 - p.fontMetrics().descent();
+
+  // Calculate the entire text width to ensure perfect centering
+  int totalTextWidth = p.fontMetrics().horizontalAdvance(accelText)
+                     + p.fontMetrics().horizontalAdvance(maxAccSuffix)
+                     + p.fontMetrics().horizontalAdvance(obstacleText)
+                     + p.fontMetrics().horizontalAdvance(createDiffText(obstacleDistance, obstacleDistanceStock))
+                     + p.fontMetrics().horizontalAdvance(stopText)
+                     + p.fontMetrics().horizontalAdvance(followText);
+
+  int textStartPos = adjustedRect.x() + (adjustedRect.width() - totalTextWidth) / 2;
+
+  // Draw the text
+  auto drawText = [&](const QString &text, const QColor color) {
+    p.setPen(color);
+    p.drawText(textStartPos, textBaseLine, text);
+    textStartPos += p.fontMetrics().horizontalAdvance(text);
+  };
+
+  drawText(accelText, Qt::white);
+  drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : Qt::red);
+  drawText(obstacleText, Qt::white);
+  drawText(createDiffText(obstacleDistance, obstacleDistanceStock), (obstacleDistance - obstacleDistanceStock) > 0 ? Qt::green : Qt::red);
+  drawText(stopText, Qt::white);
+  drawText(followText, Qt::white);
+
+  p.restore();
+}
+
 void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.save();