From d52a15eff764cb62ea5120ddd96c811542de0e14 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Mon, 11 Nov 2024 19:20:13 +0100
Subject: [PATCH] =?UTF-8?q?=E2=99=A8=EF=B8=8F=20Adds=20test=20for=20gait?=
 =?UTF-8?q?=20performance?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/platformio.ini            | 10 ++++++++++
 esp32/test/gait_performance.cpp | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 43 insertions(+)
 create mode 100644 esp32/test/gait_performance.cpp

diff --git a/esp32/platformio.ini b/esp32/platformio.ini
index 3a75b53..0ed838f 100644
--- a/esp32/platformio.ini
+++ b/esp32/platformio.ini
@@ -96,8 +96,18 @@ lib_deps =
 	adafruit/Adafruit HMC5883 Unified@^1.2.3
 	adafruit/Adafruit BMP085 Unified@^1.1.3
 	adafruit/Adafruit ADS1X15@^2.5.0
+	adafruit/Adafruit Unified Sensor@^1.1.14
 	fastled/FastLED@^3.7.0
     SPI
+	FS
+	WiFi
+	LittleFS
+	ESPmDNS
+	HTTPClient
+	DNSServer
+	Update
+	WiFiClientSecure
+	HTTPUpdate
 extra_scripts = 
 	pre:scripts/pre_build.py
     pre:scripts/build_app.py
diff --git a/esp32/test/gait_performance.cpp b/esp32/test/gait_performance.cpp
new file mode 100644
index 0000000..465ebd9
--- /dev/null
+++ b/esp32/test/gait_performance.cpp
@@ -0,0 +1,33 @@
+#include <Arduino.h>
+#include <unity.h>
+#include "Gait/GaitState.h"
+
+void test_gaitPlanner_calculateStep_time() {
+    BezierState gaitPlanner;
+    body_state_t body_state = {
+        128, 0, 0, 0, 0, 0, {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}};
+    ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
+    const int num_steps = 1000;
+
+    unsigned long start = millis();
+    for (int i = 0; i < num_steps; i++) {
+        gaitPlanner.step(body_state, command, 0.02f);
+    }
+    unsigned long end = millis();
+
+    unsigned long duration = end - start;
+    unsigned long max_duration = num_steps / 2; // Minimum 0.5 ms per step
+
+    char message[50];
+    snprintf(message, sizeof(message), "The step calculation took: %lu ms", duration);
+    TEST_ASSERT_MESSAGE(duration <= max_duration, message);
+}
+
+void setup() {
+    delay(2000); // Allow time for Serial to initialize if running on an ESP32
+    UNITY_BEGIN();
+    RUN_TEST(test_gaitPlanner_calculateStep_time);
+    UNITY_END();
+}
+
+void loop() {}
\ No newline at end of file