From 18f1f8bea8d8747a3300b79b92298cc75b15996a Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Sun, 12 Jul 2015 20:24:38 -0300
Subject: [PATCH] Filter: add AverageFilter variant to save current sum

When the the variable in which we are saving the current sum is of
integral type we can use a much more optimized apply() method: instead
of looping the entire number of samples adding them up, we always keep
the current sum in a member.  It also allows the caller to decide the
type it wants to use to get the average: this may be dictated by another
interface it uses or it may want to avoid the implicit truncate in the
return of apply().
---
 libraries/Filter/AverageFilter.h | 75 +++++++++++++++++++++++++++++++-
 1 file changed, 73 insertions(+), 2 deletions(-)

diff --git a/libraries/Filter/AverageFilter.h b/libraries/Filter/AverageFilter.h
index 34a02bb84a..25d816e082 100644
--- a/libraries/Filter/AverageFilter.h
+++ b/libraries/Filter/AverageFilter.h
@@ -39,8 +39,9 @@ public:
     // reset - clear the filter
     virtual void        reset();
 
-private:
-    uint8_t        _num_samples; // the number of samples in the filter, maxes out at size of the filter
+protected:
+    // the number of samples in the filter, maxes out at size of the filter
+    uint8_t        _num_samples;
 };
 
 // Typedef for convenience (1st argument is the data type, 2nd is a larger datatype to handle overflows, 3rd is buffer size)
@@ -105,3 +106,73 @@ void AverageFilter<T,U,FILTER_SIZE>::        reset()
     // clear our variable
     _num_samples = 0;
 }
+
+/*
+ * This filter is intended to be used with integral types to be faster and
+ * avoid loss of precision on floating point arithmetic. The integral type
+ * chosen must be one that fits FILTER_SIZE values you are filtering.
+ *
+ * Differently from other average filters, the result is only returned when
+ * getf()/getd() is called
+ */
+template <class T, class U, uint8_t FILTER_SIZE>
+class AverageIntegralFilter : public AverageFilter<T,U,FILTER_SIZE>
+{
+public:
+    /*
+     * Add a new raw value to the filter: method signature is maintained from
+     * AverageFilter, but it doesn't retrieve the filtered value: return value
+     * is always 0. Call getf()/getd() in order to get the filtered value.
+     */
+    virtual T apply(T sample) override;
+
+    // get the current value as a float
+    virtual float getf();
+
+    // get the current value as a double
+    virtual double getd();
+protected:
+    // the current sum of samples
+    U _sum = 0;
+};
+
+template <class T, class U, uint8_t FILTER_SIZE>
+T AverageIntegralFilter<T,U,FILTER_SIZE>::apply(T sample)
+{
+    T curr = this->samples[this->sample_index];
+
+    // call parent's parent apply function to get the sample into the array
+    FilterWithBuffer<T,FILTER_SIZE>::apply(sample);
+
+    // increment the number of samples so far
+    this->_num_samples++;
+    if (this->_num_samples > FILTER_SIZE || this->_num_samples == 0) {
+        this->_num_samples = FILTER_SIZE;
+    }
+
+    _sum -= curr;
+    _sum += sample;
+
+    // don't return the value: caller is forced to call getf() or getd()
+    return 0;
+}
+
+template <class T, class U, uint8_t FILTER_SIZE>
+float AverageIntegralFilter<T,U,FILTER_SIZE>::getf()
+{
+    if (this->_num_samples == 0) {
+        return 0.f;
+    }
+
+    return (float)_sum / this->_num_samples;
+}
+
+template <class T, class U, uint8_t FILTER_SIZE>
+double AverageIntegralFilter<T,U,FILTER_SIZE>::getd()
+{
+    if (this->_num_samples == 0) {
+        return 0.f;
+    }
+
+    return (double)_sum / this->_num_samples;
+}