diff --git a/lib/filters.c b/lib/filters.c
index c7810e4..1d81e36 100644
--- a/lib/filters.c
+++ b/lib/filters.c
@@ -26,22 +26,30 @@ void init_pll(PLL *pll, float output_freq, float reference_freq, float loop_filt
 }
 
 float apply_pll(PLL *pll, float ref_sample, float input_sample) {
-    float phase_inc = 2.0f * M_PI * pll->freq / pll->sample_rate;
-    pll->phase += phase_inc;
-    if (pll->phase > 2.0f * M_PI) {
+    float pll_output = sin(pll->phase);
+    
+    float phase_error;
+    if (pll->quadrature_mode) {
+        float quadrature_output = sin(pll->phase + M_PI/2.0f);
+        phase_error = input_sample * quadrature_output;
+    } else {
+        phase_error = input_sample * pll_output;
+    }
+    
+    pll->loop_filter_state += pll->ki * phase_error / pll->sample_rate;
+    float loop_filter_output = pll->loop_filter_state + pll->kp * phase_error;
+    
+    float phase_adjustment = loop_filter_output;
+    pll->phase += phase_adjustment;
+    
+    pll->phase += 2.0f * M_PI * pll->freq / pll->sample_rate;
+    
+    while (pll->phase >= 2.0f * M_PI) {
         pll->phase -= 2.0f * M_PI;
     }
-
-    float vco_out = sinf(pll->phase);
-    float phase_error = ref_sample * vco_out;
-
-    pll->loop_filter_state += pll->ki * phase_error;
-    float freq_offset = pll->kp * phase_error + pll->loop_filter_state;
-    pll->freq = pll->ref_freq + freq_offset;
-
-    if (pll->quadrature_mode) {
-        return cosf(pll->phase);
-    } else {
-        return vco_out;
+    while (pll->phase < 0.0f) {
+        pll->phase += 2.0f * M_PI;
     }
+    
+    return pll_output;
 }
\ No newline at end of file