From 5b88c6b53577c51f27f70adab1b7fedee971f496 Mon Sep 17 00:00:00 2001
From: Andrey <rusefillc@gmail.com>
Date: Mon, 28 Nov 2022 19:22:45 -0500
Subject: [PATCH] exp average is pretty cool

---
 firmware/util/math/exp_average.h           | 21 +++++++++++++
 unit_tests/tests/util/test_exp_average.cpp | 36 ++++++++++++++++++++++
 unit_tests/tests/util/test_util.mk         |  1 +
 3 files changed, 58 insertions(+)
 create mode 100644 firmware/util/math/exp_average.h
 create mode 100644 unit_tests/tests/util/test_exp_average.cpp

diff --git a/firmware/util/math/exp_average.h b/firmware/util/math/exp_average.h
new file mode 100644
index 0000000000..d2ba98afd0
--- /dev/null
+++ b/firmware/util/math/exp_average.h
@@ -0,0 +1,21 @@
+/**
+ * @file exp_average.h
+ */
+
+#pragma once
+
+class ExpAverage {
+public:
+    float average(float value) {
+        current = smoothingFactor * value + (1 - smoothingFactor) * current;
+        return current;
+    }
+
+    void init(int length) {
+        smoothingFactor = 2 / (length + 1.0);
+    }
+
+private:
+    float current = 0;
+    float smoothingFactor = 0.5;
+};
diff --git a/unit_tests/tests/util/test_exp_average.cpp b/unit_tests/tests/util/test_exp_average.cpp
new file mode 100644
index 0000000000..eaef0890ed
--- /dev/null
+++ b/unit_tests/tests/util/test_exp_average.cpp
@@ -0,0 +1,36 @@
+#include "pch.h"
+
+#include "exp_average.h"
+
+TEST(exp_average, length1) {
+    ExpAverage ea;
+    ea.init(1);
+    ASSERT_NEAR(ea.average(3), 3, EPS2D);
+    ASSERT_NEAR(ea.average(8), 8, EPS2D);
+    ASSERT_NEAR(ea.average(3), 3, EPS2D);
+    ASSERT_NEAR(ea.average(8), 8, EPS2D);
+}
+
+TEST(exp_average, length2) {
+    ExpAverage ea;
+    ea.init(2);
+    ASSERT_NEAR(ea.average(3), 2, EPS2D);
+    ASSERT_NEAR(ea.average(8), 6, EPS2D);
+    ASSERT_NEAR(ea.average(3), 4, EPS2D);
+    ASSERT_NEAR(ea.average(8), 6.6666, EPS2D);
+}
+
+TEST(exp_average, length12) {
+    ExpAverage ea;
+    ea.init(12);
+    ASSERT_NEAR(ea.average(3), 0.4615, EPS2D);
+    ASSERT_NEAR(ea.average(8), 1.6213, EPS2D);
+    ASSERT_NEAR(ea.average(3), 1.8334, EPS2D);
+    ASSERT_NEAR(ea.average(8), 2.7821, EPS2D);
+
+    for (int i = 0;i<300;i++) {
+    	ea.average(3);
+    	ea.average(8);
+    }
+    ASSERT_NEAR(ea.average(3), 5.2916, EPS2D);
+}
diff --git a/unit_tests/tests/util/test_util.mk b/unit_tests/tests/util/test_util.mk
index 41a66da5df..2bfd5f43ad 100644
--- a/unit_tests/tests/util/test_util.mk
+++ b/unit_tests/tests/util/test_util.mk
@@ -2,6 +2,7 @@
 
 CPPSRC += 	$(PROJECT_DIR)/../unit_tests/tests/util/test_buffered_writer.cpp \
 	$(PROJECT_DIR)/../unit_tests/tests/util/test_error_accumulator.cpp \
+	$(PROJECT_DIR)/../unit_tests/tests/util/test_exp_average.cpp \
 
 INCDIR += $(PROJECT_DIR)/controllers/system	
 	
\ No newline at end of file