From 8faab9ce58560ef7179b484f402d5dbce460b1c3 Mon Sep 17 00:00:00 2001
From: Milosz Lagan <milosz.lagan@husarion.com>
Date: Tue, 19 Nov 2024 17:15:18 +0000
Subject: [PATCH] Rework moving average logic

---
 .../husarion_ugv_utils/moving_average.hpp     | 20 ++++++++--------
 .../test/test_moving_average.cpp              | 23 +++++++++++++++++--
 2 files changed, 31 insertions(+), 12 deletions(-)

diff --git a/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
index 0a6c76363..03fe6b07c 100644
--- a/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp
@@ -25,40 +25,40 @@ class MovingAverage
 {
 public:
   MovingAverage(const std::size_t window_size = 5, const T initial_value = T(0))
-  : window_size_(window_size), initial_value_(initial_value), sum_(T(0))
+  : window_size_(window_size), initial_value_(initial_value)
   {
   }
 
   void Roll(const T value)
   {
     values_.push_back(value);
-    sum_ += value;
 
     if (values_.size() > window_size_) {
-      sum_ -= values_.front();
       values_.pop_front();
     }
   }
 
-  void Reset()
-  {
-    values_.erase(values_.begin(), values_.end());
-    sum_ = T(0);
-  }
+  void Reset() { values_.erase(values_.begin(), values_.end()); }
 
   T GetAverage() const
   {
     if (values_.size() == 0) {
       return initial_value_;
     }
-    return sum_ / static_cast<T>(values_.size());
+
+    T sum = T(0);
+
+    for (const auto & value : values_) {
+      sum += value / static_cast<T>(values_.size());
+    }
+
+    return sum;
   }
 
 private:
   const std::size_t window_size_;
   std::deque<T> values_;
   const T initial_value_;
-  T sum_;
 };
 
 }  // namespace husarion_ugv_utils
diff --git a/husarion_ugv_utils/test/test_moving_average.cpp b/husarion_ugv_utils/test/test_moving_average.cpp
index 653483ce7..e520bc801 100644
--- a/husarion_ugv_utils/test/test_moving_average.cpp
+++ b/husarion_ugv_utils/test/test_moving_average.cpp
@@ -61,12 +61,12 @@ TEST(TestMovingAverage, TestHighOverload)
 
   double sum = 0.0;
   for (std::size_t i = 1; i <= window_len * 10; i++) {
-    sum += double(i);
+    sum += double(i) / double(window_len);
     ma.Roll(double(i));
 
     // test every 1000 rolls expected average
     if (i % window_len == 0) {
-      EXPECT_EQ(sum / double(window_len), ma.GetAverage());
+      EXPECT_LT(sum - ma.GetAverage(), std::numeric_limits<double>::epsilon());
       sum = 0.0;
     }
   }
@@ -107,6 +107,25 @@ TEST(TestMovingAverage, TestResetToInitialValue)
   EXPECT_EQ(7.0, ma.GetAverage());
 }
 
+TEST(TestMovingAverage, TestInfInjectionHandling)
+{
+  husarion_ugv_utils::MovingAverage<double> ma(4);
+  ma.Roll(1.0);
+  ma.Roll(2.0);
+  ma.Roll(3.0);
+  ma.Roll(4.0);
+  EXPECT_EQ(2.5, ma.GetAverage());
+
+  ma.Roll(std::numeric_limits<double>::infinity());
+  EXPECT_EQ(std::numeric_limits<double>::infinity(), ma.GetAverage());
+
+  ma.Roll(1.0);
+  ma.Roll(2.0);
+  ma.Roll(3.0);
+  ma.Roll(4.0);
+  EXPECT_EQ(2.5, ma.GetAverage());
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);