From 0c8822d2c5576ee6b84bd374ca5c9c3b873aa393 Mon Sep 17 00:00:00 2001
From: Thomas Schmid <tom@lfence.de>
Date: Sat, 18 Jun 2022 20:33:26 +0200
Subject: [PATCH] add fft code

Signed-off-by: Thomas Schmid <tom@lfence.de>
---
 firmware/include/fft.h |  6 ++++
 firmware/src/fft.cpp   | 62 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 68 insertions(+)
 create mode 100644 firmware/include/fft.h
 create mode 100644 firmware/src/fft.cpp

diff --git a/firmware/include/fft.h b/firmware/include/fft.h
new file mode 100644
index 0000000..fa164ee
--- /dev/null
+++ b/firmware/include/fft.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#include <complex>
+
+void fft(std::complex<float> *input, std::complex<float> *output, uint32_t N);
+void rfft(std::complex<float> *input, std::complex<float> *output, uint32_t N);
\ No newline at end of file
diff --git a/firmware/src/fft.cpp b/firmware/src/fft.cpp
new file mode 100644
index 0000000..35dd1de
--- /dev/null
+++ b/firmware/src/fft.cpp
@@ -0,0 +1,62 @@
+#include "fft.h"
+#include "math.h"
+
+unsigned int bitReverse(unsigned int x, int log2n)
+{
+    int n = 0;
+    for (int i = 0; i < log2n; i++)
+    {
+        n <<= 1;
+        n |= (x & 1);
+        x >>= 1;
+    }
+    return n;
+}
+
+void fft(std::complex<float> *samples, std::complex<float> *output, uint32_t N)
+{
+    uint8_t log2n = (uint8_t)std::log2(N) + 0.5f;
+    std::complex<float> I(0.0, 1.0);
+    if (N == 1) {
+        output[0] = samples[0];
+        return;
+    }
+
+    //shuffle array
+    for (int i = 0; i < N; i++) {
+
+        output[i] = samples[bitReverse(i, log2n)];
+    }
+    for(int s = 1; s <= log2n; s++) {
+        uint32_t m = 1 << s;                    // 2^s
+        std::complex<float> wm = std::exp(-2.0f*(float)M_PI*I/(std::complex<float>)m);
+        for (int k = 0; k < N; k += m) {
+            std::complex<float> w = 1.f;
+            for (int j = 0; j < m/2; j++) {
+                std::complex<float> t = w * output[k+j+m/2];
+                std::complex<float>  u = output[k+j];
+                output[k+j] = u+t;
+                output[k+j+m/2]  = u-t;
+                w = w*wm;
+            }
+        }
+    }
+}
+
+void rfft(std::complex<float> *input, std::complex<float> *output, uint32_t N){
+    std::complex<float> I(0.0, 1.0);
+    for(int i = 0; i< N/2; i++){
+        input[i] = input[i] + I * input[i + N/2];
+    }
+
+    fft(input, output, N/2);
+    
+    for(int i = 0; i < N/2; i++){
+        output[i] = (output[i] + std::conj(output[(N/2) - i]))/2.f;
+    }
+
+    for(int i = N/2; i < N; i++) {
+        output[i] = -I * (output[i] - std::conj(output[(N/2) - i]))/2.f;
+    }
+
+}
\ No newline at end of file