From fa2ba7f71a8fe7316d84e02df1eb8a4b4b318d69 Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Fri, 31 Jan 2025 16:48:13 +0100
Subject: [PATCH] add new preemp

---
 .vscode/.server-controller-port.log |  2 +-
 lib/filters.c                       | 56 ++++++++++++++++++++++-------
 lib/filters.h                       | 13 +++----
 src/fm95.c                          | 10 +++---
 4 files changed, 57 insertions(+), 24 deletions(-)

diff --git a/.vscode/.server-controller-port.log b/.vscode/.server-controller-port.log
index f5dab20..ffdf8bd 100644
--- a/.vscode/.server-controller-port.log
+++ b/.vscode/.server-controller-port.log
@@ -1,5 +1,5 @@
 {
   "port": 13452,
-  "time": 1738326357935,
+  "time": 1738336716329,
   "version": "0.0.3"
 }
\ No newline at end of file
diff --git a/lib/filters.c b/lib/filters.c
index fb5a21a..1ffb989 100644
--- a/lib/filters.c
+++ b/lib/filters.c
@@ -1,19 +1,51 @@
 #include "filters.h"
 
-void init_rc(ResistorCapacitor *rc, float alpha) {
-    rc->prev_sample = 0.0f;
-    rc->alpha = alpha;
-}
+void init_preemphasis(BiquadFilter *filter, float tau, float sample_rate) {
+    /*
+    premphasis should go like this:
+    cutoff: +0
+    2cutoff: +6
+    3cutoff: +12
+    4cutoff: +18
+    ...
+    */
 
-void init_rc_tau(ResistorCapacitor *rc, float tau, float sample_rate) {
-    rc->prev_sample = 0.0f;
-    rc->alpha = exp(-1 / (tau * sample_rate));
-}
+    // Calculate the cutoff frequency from tau (f_c = 1/(2πτ))
+    float cutoff_freq = 1.0f / (2.0f * M_PI * tau);
+    
+    // Pre-warp the cutoff frequency for the bilinear transform
+    float omega = 2.0f * M_PI * cutoff_freq / sample_rate;
+    float K = tanf(omega / 2.0f);
 
-float apply_pre_emphasis(ResistorCapacitor *rc, float sample) {
-    float audio = sample-rc->alpha*rc->prev_sample;
-    rc->prev_sample = audio;
-    return audio;
+    // Calculate filter coefficients for 1st-order high-pass (converted to biquad)
+    float alpha = 1.0f + K;
+    filter->b0 = 1.0f / alpha;
+    filter->b1 = -filter->b0;
+    filter->b2 = 0.0f;
+    filter->a1 = (K - 1.0f) / alpha;
+    filter->a2 = 0.0f;
+
+    // Reset state variables
+    filter->x1 = 0.0f;
+    filter->x2 = 0.0f;
+    filter->y1 = 0.0f;
+    filter->y2 = 0.0f;
+}
+float apply_preemphasis(BiquadFilter *filter, float input) {
+    // Direct Form I implementation
+    float output = filter->b0 * input
+                 + filter->b1 * filter->x1
+                 + filter->b2 * filter->x2
+                 - filter->a1 * filter->y1
+                 - filter->a2 * filter->y2;
+
+    // Update state variables
+    filter->x2 = filter->x1;
+    filter->x1 = input;
+    filter->y2 = filter->y1;
+    filter->y1 = output;
+
+    return output;
 }
 
 void init_lpf(FrequencyFilter* filter, float cutoffFreq, float sampleRate) {
diff --git a/lib/filters.h b/lib/filters.h
index 2a6b5e9..dbb2035 100644
--- a/lib/filters.h
+++ b/lib/filters.h
@@ -6,13 +6,14 @@
 #include "constants.h"
 
 typedef struct {
-    float alpha;
-    float prev_sample;
-} ResistorCapacitor; 
+    float b0, b1, b2;
+    float a1, a2;
+    float x1, x2;
+    float y1, y2;
+} BiquadFilter;
 
-void init_rc(ResistorCapacitor *pe, float alpha);
-void init_rc_tau(ResistorCapacitor *pe, float tau, float sample_rate);
-float apply_pre_emphasis(ResistorCapacitor *pe, float sample);
+void init_preemphasis(BiquadFilter *filter, float tau, float sample_rate);
+float apply_preemphasis(BiquadFilter *filter, float input);
 
 typedef struct {
     float coeffs[FILTER_TAPS];
diff --git a/src/fm95.c b/src/fm95.c
index 0bf29d1..8c9855f 100644
--- a/src/fm95.c
+++ b/src/fm95.c
@@ -456,9 +456,9 @@ int main(int argc, char **argv) {
     DelayLine monoDelay; // Hilbert introduces a delay, this should be here to sync the mono with stereo to a sample
     init_delay_line(&monoDelay, (HILBERT_TAPS-1)/2);
 
-    ResistorCapacitor preemp_l, preemp_r;
-    init_rc_tau(&preemp_l, preemphasis_tau, SAMPLE_RATE);
-    init_rc_tau(&preemp_r, preemphasis_tau, SAMPLE_RATE);
+    BiquadFilter preemp_l, preemp_r;
+    init_preemphasis(&preemp_l, preemphasis_tau, SAMPLE_RATE);
+    init_preemphasis(&preemp_r, preemphasis_tau, SAMPLE_RATE);
 
     FrequencyFilter lpf_l, lpf_r;
     init_lpf(&lpf_l, LPF_CUTOFF, SAMPLE_RATE);
@@ -514,8 +514,8 @@ int main(int argc, char **argv) {
             float ready_r = apply_frequency_filter(&lpf_r, l_in);
             ready_l = apply_frequency_filter(&hpf_l, ready_l);
             ready_r = apply_frequency_filter(&hpf_r, ready_r);
-            ready_l = apply_pre_emphasis(&preemp_l, ready_l)*2;
-            ready_r = apply_pre_emphasis(&preemp_r, ready_r)*2;
+            ready_l = apply_preemphasis(&preemp_l, ready_l);
+            ready_r = apply_preemphasis(&preemp_r, ready_r);
             ready_l = soft_clip(ready_l, soft_clipper_threshold);
             ready_r = soft_clip(ready_r, soft_clipper_threshold);
             ready_l = hard_clip(ready_l, clipper_threshold);