WIP

src/modm/math/filter/moving_average.hpp

@@ -17,107 +17,103 @@
  */
 // ----------------------------------------------------------------------------
 #pragma once
+
 #include <cstddef>
 #include <cstdint>
 
+#include <concepts>
 #include <span>
 #include <algorithm>
+#include <numeric>
+
 #include <modm/math/utils/integer_traits.hpp>
 
-namespace modm
-{
-	namespace filter{
-		/**
-		 * \brief	Moving average filter
-		 *
-		 * Calculates the average of N newest values, i.a. the sum of the last N
-		 * values have been passed to update(...), divided by N. If less than N
-		 * values have been passed to the filter, the division factor is still N,
-		 * so missing values are assumed to be zero.
-		 *
-		 * For integer types this implementation stores the current sum of all values in the buffer
-		 * and updates this value with every call of update() by subtracting
-		 * the overwritten buffer index and adding the new one.
-		 *
-		 * Due to numerical instabillity for floating value types, inside the update function
-		 * the sum has to be recalculated making it less efficient.
-		 *
-		 * The internal sum is always up to date and the getValue()
-		 * method consists of only one division.
-		 *
-		 * \warning	Input range is limited by the following equation
-		 * 			\code N * input::maxValue < T::maxValue \endcode
-		 * 			The sum off the last N input values must not be greater than
-		 * 			the maximum value of T, otherwise an overflow will occur.
-		 *
-		 * \tparam	T	Input type
-		 * \tparam	N	Number of samples (maximum is 65356 or 2**16)
-		 *
-		 * \ingroup	modm_math_filter
-		 */
-		template<typename T, std::size_t N>
-		class MovingAverage
-		{
-		public:
-			MovingAverage(const T& initialValue = 0);
-
-			/// Reset whole buffer to 'input'
-			/// Next call of getValue() returns 'input'
-			void reset(const T& input);
-
-			/// Append new value
-			void
-			update(const T& input);
-
-			/// Get filtered value
-			const T
-			getValue() const;
-
-		private:
-			least_uint<N> index{0};
-			T buffer[N];
-			T sum;
-		};
-	}
-}
+#include <modm/debug/logger.hpp>
 
-// ----------------------------------------------------------------------------
-template<typename T, std::size_t N>
-modm::filter::MovingAverage<T, N>::MovingAverage(const T& initialValue)
-{
-	reset(initialValue);
-}
+namespace modm::filter{
 
-// ----------------------------------------------------------------------------
+/**
+ * \brief	Moving average filter
+ *
+ * Calculates the average of N newest values, i.a. the sum of the last N
+ * values have been passed to update(...), divided by N. If less than N
+ * values have been passed to the filter, the division factor is still N,
+ * so missing values are assumed to be zero.
+ *
+ * For integer types this implementation stores the current sum of all values in the buffer
+ * and updates this value with every call of update() by subtracting
+ * the overwritten buffer index and adding the new one.
+ *
+ * Due to numerical instabillity for floating value types, inside the update function
+ * the sum has to be recalculated making it less efficient.
+ *
+ * The internal sum is always up to date and the getValue()
+ * method consists of only one division.
+ *
+ * \warning	Input range is limited by the following equation
+ * 			\code N * input::maxValue < T::maxValue \endcode
+ * 			The sum off the last N input values must not be greater than
+ * 			the maximum value of T, otherwise an overflow will occur.
+ *
+ * \tparam	T	Input type
+ * \tparam	N	Number of samples
+ *
+ * \ingroup	modm_math_filter
+ */
 template<typename T, std::size_t N>
-void
-modm::filter::MovingAverage<T, N>::reset(const T& input)
+class MovingAverage
 {
-	std::fill(std::begin(buffer), std::end(buffer), input);
-	sum = N * input;
-}
+public:
+	constexpr MovingAverage(T initialValue = 0)
+	{
+		reset(initialValue);
+	}
 
-// ----------------------------------------------------------------------------
-template<typename T, std::size_t N>
-void
-modm::filter::MovingAverage<T, N>::update(const T& input)
-{
-	sum -= buffer[index];
-	sum += input;
+	/// Reset whole buffer to 'input'
+	/// Next call of getValue() returns 'input'
+	constexpr void reset(T input)
+	{
+		std::fill(std::begin(buffer), std::end(buffer), input);
+		sum = N * input;
+	}
 
-	buffer[index] = input;
+	/// Append new value
+	void
+	update(T input)
+	{
+		sum -= buffer[index];
+		sum += input;
 
-	if (++index >= N)
-		index = 0;
-}
+		buffer[index] = input;
 
-// -----------------------------------------------------------------------------
-template<typename T, std::size_t N>
-const T
-modm::filter::MovingAverage<T, N>::getValue() const
-{
-	return (sum / static_cast<T>(N));
-}
+		if (++index == N)
+			index = 0;
+	}
+
+	/// Append new value, constrained for float
+	void
+	update(T input)
+	requires std::floating_point<T>
+	{
+		buffer[index] = input;
+
+		sum = std::accumulate(std::begin(buffer), std::end(buffer), 0);
+
+		if (++index == N)
+			index = 0;
+	}
+
+	/// Get filtered value
+	T
+	constexpr getValue() const
+	{
+		return (sum / static_cast<T>(N));
+	}
 
+private:
+	least_uint<N> index{0};
+	T buffer[N];
+	T sum;
+};
 
-#include "moving_average_float_impl.hpp"
+} // namespace modm::filter