diff --git a/firmware/include/fft.h b/firmware/include/fft.h
index fa164ee..db0ca2a 100644
--- a/firmware/include/fft.h
+++ b/firmware/include/fft.h
@@ -2,5 +2,75 @@
 
 #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
+template <class T>
+struct FFT
+{
+    static void fft(std::complex<T> *samples, std::complex<T> *output, uint32_t N)
+    {
+        uint8_t log2n = (uint8_t)std::log2(N) + 0.5f;
+        std::complex<T> 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[FFT::bitReverse(i, log2n)];
+        }
+        for (int s = 1; s <= log2n; s++)
+        {
+            uint32_t m = 1 << s; // 2^s
+            std::complex<T> wm = std::exp(-2.0f * (T)M_PI * I / (std::complex<T>)m);
+            for (int k = 0; k < N; k += m)
+            {
+                std::complex<T> w = 1.f;
+                for (int j = 0; j < m / 2; j++)
+                {
+                    std::complex<T> t = w * output[k + j + m / 2];
+                    std::complex<T> u = output[k + j];
+                    output[k + j] = u + t;
+                    output[k + j + m / 2] = u - t;
+                    w = w * wm;
+                }
+            }
+        }
+    }
+
+    static void rfft(std::complex<T> *input, std::complex<T> *output, uint32_t N)
+    {
+        std::complex<T> I(0.0, 1.0);
+        for (int i = 0; i < N / 2; i++)
+        {
+            input[i] = input[i] + I * input[i + N / 2];
+        }
+
+        FFT<T>::fft(input, output, N / 2);
+
+        for (int i = 0; i < N / 2; i++)
+        {
+            output[i] = (output[i] + std::conj(output[(N / 2) - i])) / 2.;
+        }
+
+        for (int i = N / 2; i < N; i++)
+        {
+            output[i] = -I * (output[i] - std::conj(output[(N / 2) - i])) / 2.;
+        }
+    }
+
+private:
+    static 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;
+    }
+};
\ No newline at end of file
diff --git a/firmware/src/applications/fft_test.cpp b/firmware/src/applications/fft_test.cpp
index 1d9956d..f52a884 100644
--- a/firmware/src/applications/fft_test.cpp
+++ b/firmware/src/applications/fft_test.cpp
@@ -4,13 +4,13 @@
 
 #define N_SAMPLES 256
 
-std::complex<float> samples[N_SAMPLES];
-std::complex<float> z[N_SAMPLES];
-double vReal[N_SAMPLES];
-double vImag[N_SAMPLES];
-uint32_t sample_counter = 0;
-unsigned long max_dt = 0;
-unsigned long last_sample = 0;
+static std::complex<float> samples[N_SAMPLES];
+static std::complex<float> z[N_SAMPLES];
+static double vReal[N_SAMPLES];
+static double vImag[N_SAMPLES];
+static uint32_t sample_counter = 0;
+static unsigned long max_dt = 0;
+static unsigned long last_sample = 0;
 
 void FFTTestApp::init()
 {
@@ -37,7 +37,7 @@ void FFTTestApp::loop()
         if (sample_counter == N_SAMPLES)
         {
             unsigned long start = micros();
-            fft(samples, z, N_SAMPLES);
+            FFT<float>::fft(samples, z, N_SAMPLES);
             unsigned long end = micros();
 
             float max = 0.f;
diff --git a/firmware/src/fft.cpp b/firmware/src/fft.cpp
deleted file mode 100644
index 15e0d83..0000000
--- a/firmware/src/fft.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-#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