1 files changed, 71 insertions, 0 deletions
diff --git a/src/biquad_filter.c b/src/biquad_filter.c
new file mode 100644
index 0000000..ae33a4c
--- /dev/null
+++ b/src/biquad_filter.c
@@ -0,0 +1,71 @@
+//#include "biquad_filter.h"
+
+typedef struct biquad_filter_t {
+  // Filter coefficients
+  double b0, b1, b2; // Feedforward coefficients
+  double a1, a2;     // Feedback coefficients
+    
+  // Delay buffers (history of input and output)
+  double x1, x2; // Previous input samples
+  double y1, y2; // Previous output samples
+} biquad_filter_t;
+
+void
+biquad_calculate_coefficients(biquad_filter_t* filter, double freq, double Q, double sampleRate, char type)
+{
+  double omega = 2.0 * M_PI * freq / sampleRate;
+  double alpha = sin(omega) / (2.0 * Q);
+  double cos_omega = cos(omega);
+
+  // Initialize coefficients based on filter type
+  switch (type) {
+  case 'l': // Low-pass filter
+    filter->b0 = (1.0 - cos_omega) / 2.0;
+    filter->b1 = 1.0 - cos_omega;
+    filter->b2 = filter->b0;
+    filter->a1 = -2.0 * cos_omega;
+    filter->a2 = 1.0 - alpha;
+    break;
+  case 'h': // High-pass filter
+    filter->b0 = (1.0 + cos_omega) / 2.0;
+    filter->b1 = -(1.0 + cos_omega);
+    filter->b2 = filter->b0;
+    filter->a1 = -2.0 * cos_omega;
+    filter->a2 = 1.0 - alpha;
+    break;
+  case 'b': // Band-pass filter
+    filter->b0 = alpha;
+    filter->b1 = 0.0;
+    filter->b2 = -alpha;
+    filter->a1 = -2.0 * cos_omega;
+    filter->a2 = 1.0 - alpha;
+    break;
+  default:
+    exit(EXIT_FAILURE); // Unsupported filter type
+  }
+
+  // Normalize coefficients
+  double a0 = 1.0 + alpha;
+  filter->b0 /= a0;
+  filter->b1 /= a0;
+  filter->b2 /= a0;
+  filter->a1 /= a0;
+  filter->a2 /= a0;
+}
+
+double
+biquad_process(biquad_filter_t* filter, double input)
+{
+  double output = filter->b0 * input +
+    filter->b1 * filter->x1 +
+    filter->b2 * filter->x2 -
+    filter->a1 * filter->y1 -
+    filter->a2 * filter->y2;
+
+  filter->x2 = filter->x1;
+  filter->x1 = input;
+  filter->y2 = filter->y1;
+  filter->y1 = output;
+
+  return output;
+}