From 790ce02395e2079a6928363de4b97e5895d9f8ca Mon Sep 17 00:00:00 2001
From: Peter Jabra <jabrap1@mcmaster.ca>
Date: Mon, 4 Nov 2024 20:17:56 -0500
Subject: [PATCH] Brake Pedal Position and Brake Pedal Light Logic (#288)

* wrote brake_pedal_light

* Implemented recommended edits to functions and split brake_pedal_lights into two functions

* Changed line 14, the return statement, as requested

* removed unneeded import
---
 firmware/shared/controls/brake_pedal_lights.h | 24 +++++++++++
 .../controls/brake_pedal_lights_test.cc       | 40 +++++++++++++++++++
 2 files changed, 64 insertions(+)
 create mode 100644 firmware/shared/controls/brake_pedal_lights.h
 create mode 100644 firmware/shared/controls/brake_pedal_lights_test.cc

diff --git a/firmware/shared/controls/brake_pedal_lights.h b/firmware/shared/controls/brake_pedal_lights.h
new file mode 100644
index 00000000..8b098875
--- /dev/null
+++ b/firmware/shared/controls/brake_pedal_lights.h
@@ -0,0 +1,24 @@
+#pragma once
+#include <algorithm>
+
+namespace ctrl {
+
+template <typename T>
+T CalculateBrakePedalPosition(bool rear_brake_pedal_error,
+                              bool front_brake_pedal_error,
+                              T rear_brake_pedal_position,
+                              T front_brake_pedal_position) {
+    // Brake Error Handling
+    if (rear_brake_pedal_error || front_brake_pedal_error) {
+        return static_cast<T>(0);
+    } else {
+        return std::max(front_brake_pedal_position, rear_brake_pedal_position);
+    }
+}
+
+template <typename T>
+bool DetectBrakeLight(T brake_pedal_position) {
+    return (brake_pedal_position > 0.1);
+}
+
+}  // namespace ctrl
\ No newline at end of file
diff --git a/firmware/shared/controls/brake_pedal_lights_test.cc b/firmware/shared/controls/brake_pedal_lights_test.cc
new file mode 100644
index 00000000..ffbf3ffc
--- /dev/null
+++ b/firmware/shared/controls/brake_pedal_lights_test.cc
@@ -0,0 +1,40 @@
+#include "brake_pedal_lights.h"
+
+#include <iostream>
+
+#include "testing.h"
+
+int main() {
+    using namespace ctrl;
+
+    // Test Case 1: CalculateBrakePedalPosition with No Errors and Edge Values
+    ASSERT_CLOSE(CalculateBrakePedalPosition(false, false, 0.0f, 0.0f), 0.0f);
+
+    ASSERT_CLOSE(CalculateBrakePedalPosition(false, false, 1.0f, 0.0f), 1.0f);
+
+    ASSERT_CLOSE(CalculateBrakePedalPosition(false, false, 0.5f, 0.7f), 0.7f);
+
+    // Test Case 2: CalculateBrakePedalPosition with Brake Error
+    ASSERT_CLOSE(CalculateBrakePedalPosition(true, false, 0.8f, 0.9f), 0.0f);
+
+    ASSERT_CLOSE(CalculateBrakePedalPosition(false, true, 0.8f, 0.9f), 0.0f);
+
+    ASSERT_CLOSE(CalculateBrakePedalPosition(true, true, 0.8f, 0.9f), 0.0f);
+
+    // Test Case 3: DetectBrakeLight with Values Just Below and Above the
+    // Threshold
+    ASSERT_FALSE(DetectBrakeLight(0.05f));  // Below threshold
+
+    ASSERT_FALSE(DetectBrakeLight(0.09f));  // Near threshold
+
+    ASSERT_TRUE(DetectBrakeLight(0.11f));  // Just above threshold
+
+    // Test Case 4: DetectBrakeLight with High Pedal Positions
+    ASSERT_TRUE(DetectBrakeLight(0.5f));
+
+    ASSERT_TRUE(DetectBrakeLight(1.0f));
+
+    std::cout << "All test cases passed!" << std::endl;
+
+    return 0;
+}
\ No newline at end of file