From 181a16154acef377071031dbc8622abc54e69136 Mon Sep 17 00:00:00 2001
From: Phil Schatzmann <phil.schatzmann@gmail.com>
Date: Thu, 12 Oct 2023 15:54:02 +0200
Subject: [PATCH] MedianFilter

---
 src/AudioFilter/MedianFilter.h | 120 +++++++++++++++++++++++++++++++++
 src/AudioTools.h               |   1 +
 2 files changed, 121 insertions(+)
 create mode 100644 src/AudioFilter/MedianFilter.h

diff --git a/src/AudioFilter/MedianFilter.h b/src/AudioFilter/MedianFilter.h
new file mode 100644
index 0000000000..dcca8da4cf
--- /dev/null
+++ b/src/AudioFilter/MedianFilter.h
@@ -0,0 +1,120 @@
+#pragma once
+#include "Filter.h"
+
+namespace audio_tools {
+
+
+/**
+ * @brief An embedded friendly, fast one-dimensional median filter algorithm implementation in C and C++
+ * Useful for spike and noise removal from analog signals or other DSP
+ * Also known as "salt-and-pepper noise" or "impulse noise" filter
+ **/
+template <typename T>
+class MedianFilter : public Filter<T> {
+  public:
+    MedianFilter(int size=7) {
+        medianBuffer.resize(size);
+        medianFilter.numNodes = size;
+        medianFilter.medianBuffer = medianBuffer;
+        init(&medianFilter);
+    };
+
+    virtual T process(T in) override {
+        return insert(&medianFilter, in);;
+    }
+
+  protected:
+    struct sMedianNode_t {
+        T value;                      //sample value
+        struct sMedianNode *nextAge;    //pointer to next oldest value
+        struct sMedianNode *nextValue;  //pointer to next smallest value
+        struct sMedianNode *prevValue;  //pointer to previous smallest value
+    };
+
+    struct sMedianFilter_t {
+        unsigned int numNodes;          //median node buffer length
+        sMedianNode_t *medianBuffer;    //median node buffer
+        sMedianNode_t *ageHead;         //pointer to oldest value
+        sMedianNode_t *valueHead;       //pointer to smallest value
+        sMedianNode_t *medianHead;      //pointer to median value
+    };
+
+    sMedianFilter_t medianFilter;
+    Vector<sMedianNode_t> medianBuffer;
+
+    int init(sMedianFilter_t *medianFilter){
+        if(medianFilter && medianFilter->medianBuffer &&
+            (medianFilter->numNodes % 2) && (medianFilter->numNodes > 1)){
+            //initialize buffer nodes
+            for(unsigned int i = 0; i < medianFilter->numNodes; i++){
+                medianFilter->medianBuffer[i].value = 0;
+                medianFilter->medianBuffer[i].nextAge = &medianFilter->medianBuffer[(i + 1) % medianFilter->numNodes];
+                medianFilter->medianBuffer[i].nextValue = &medianFilter->medianBuffer[(i + 1) % medianFilter->numNodes];
+                medianFilter->medianBuffer[(i + 1) % medianFilter->numNodes].prevValue = &medianFilter->medianBuffer[i];
+            }
+            //initialize heads
+            medianFilter->ageHead = medianFilter->medianBuffer;
+            medianFilter->valueHead = medianFilter->medianBuffer;
+            medianFilter->medianHead = &medianFilter->medianBuffer[medianFilter->numNodes / 2];
+
+            return 0;
+        }
+
+        return -1;
+    }
+
+    int insert(sMedianFilter_t *medianFilter, T sample) {
+        unsigned int i;
+        sMedianNode_t *newNode, *it;
+
+        if(medianFilter->ageHead == medianFilter->valueHead){
+             //if oldest node is also the smallest node, increment value head
+            medianFilter->valueHead = medianFilter->valueHead->nextValue;
+        }
+
+        if((medianFilter->ageHead == medianFilter->medianHead) ||
+            (medianFilter->ageHead->value > medianFilter->medianHead->value)){
+           //prepare for median correction
+            medianFilter->medianHead = medianFilter->medianHead->prevValue;
+        }
+
+        //replace age head with new sample
+        newNode = medianFilter->ageHead;
+        newNode->value = sample;
+
+        //remove age head from list
+        medianFilter->ageHead->nextValue->prevValue = medianFilter->ageHead->prevValue;
+        medianFilter->ageHead->prevValue->nextValue = medianFilter->ageHead->nextValue;
+        //increment age head
+        medianFilter->ageHead = medianFilter->ageHead->nextAge;
+
+        //find new node position
+        it = medianFilter->valueHead; //set iterator as value head
+        for(i = 0; i < medianFilter->numNodes - 1; i++){
+            if(sample < it->value){
+                if(i == 0){   //replace value head if new node is the smallest
+                    medianFilter->valueHead = newNode;
+                }
+                break;
+            }
+            it = it->nextValue;
+        }
+
+        //insert new node in list
+        it->prevValue->nextValue = newNode;
+        newNode->prevValue = it->prevValue;
+        it->prevValue = newNode;
+        newNode->nextValue = it;
+
+        //adjust median node
+        if(i >= (medianFilter->numNodes / 2)){
+            medianFilter->medianHead = medianFilter->medianHead->nextValue;
+        }
+
+        return medianFilter->medianHead->value;
+    }
+
+};
+
+
+}
\ No newline at end of file
diff --git a/src/AudioTools.h b/src/AudioTools.h
index d710f3f1e3..233a540080 100644
--- a/src/AudioTools.h
+++ b/src/AudioTools.h
@@ -56,6 +56,7 @@
 #include "AudioTools/BaseConverter.h"
 #include "AudioFilter/Filter.h"
 #include "AudioFilter/Equilizer.h"
+#include "AudioFilter/MedianFilter.h"
 #include "AudioTools/MusicalNotes.h"
 #include "AudioI2S/I2SStream.h"
 #include "AudioPWM/AudioPWM.h"