From 0a91445a379c81326dafa92c18e3fe52d94ca231 Mon Sep 17 00:00:00 2001
From: TerraMaster85 <tm85@e3t.cc>
Date: Thu, 16 Jan 2025 00:23:58 -0500
Subject: [PATCH] rework with adapted stolen code :(

---
 src/audio_receiver/input_dsp.rs | 112 +++++++++-----------------------
 1 file changed, 31 insertions(+), 81 deletions(-)

diff --git a/src/audio_receiver/input_dsp.rs b/src/audio_receiver/input_dsp.rs
index 7aef7d3..f2297e4 100644
--- a/src/audio_receiver/input_dsp.rs
+++ b/src/audio_receiver/input_dsp.rs
@@ -28,92 +28,42 @@ pub fn take_input_pcm<T>(
 
     let mut fft_planner = FftPlanner::<f64>::new();
 
-    let mut window_offset = 0;
-    let mut prev_phase = f64::MAX;
+    let mut phase_in = 3.0_f64;
+    let freq_in = 1200.0_f64; // what does this mean?
+    let alpha = 0.5;//0.05_f64;
+    
+    let mut phase_out = 0.0_f64;
+    let mut freq_out = 0.0_f64;
+    let beta = 0.5 * alpha * alpha;
 
-    // window must not extend beyond pcm_buffer
-    while !carrier.acquired && window_offset + fft_size < pcm_buffer.len() {
-        trace!("carrier acq: trying, window offset {window_offset}");
-        let pcm_buffer_contiguous = pcm_buffer.make_contiguous();
-        // Zero-pad for a better freq resolution. 8192 fft should be fast enough
-        const MAX_ZERO_PAD: usize = 8192;
-        let mut fft_frame: Vec<Complex<f64>> = if fft_size < MAX_ZERO_PAD {
-            let mut frm = 
-                [Complex { re: 0.0f64, im: 0.0f64 }]
-                .repeat(MAX_ZERO_PAD - fft_size);
-            frm.extend(
-                &pcm_buffer_contiguous[window_offset..(fft_size+window_offset)]
-            );
-            frm
-        } else {
-            pcm_buffer_contiguous[window_offset..(fft_size+window_offset)]
-                .to_vec()
-        };
-        let fft_machine = fft_planner.plan_fft_forward(fft_frame.len());
-        fft_machine.process(&mut fft_frame);
+    let mut ok_in_a_row = 0;
+    
+    while pcm_buffer.len() > 1 {
+        // what on earth does this do??
+        //let signal_in = (Complex::i() * phase_in).exp();
+        // Where does the complex component come from????
+        let Some(signal_in) = pcm_buffer.pop_front() else { unreachable!() };
+        let signal_out = (Complex::i() * phase_out).exp();
+        let phase_error = (signal_in * signal_out.conj()).arg();
 
-        let clock_freq_index = 
-            (fft_size as f64 * carrier.clock_hz / sample_rate.0 as f64)
-            .round() as usize;
+        trace!(
+            "carrier acq: rsi:{}\tisi:{}\trso:{}\tiso:{}\terr:{}",
+            signal_in.re,
+            signal_in.im,
+            signal_out.re,
+            signal_out.im,
+            phase_error,
+        );
 
-        if let Some(bin) = fft_frame.get(clock_freq_index) {
-            // if too small to use. threshold is arbitrary and needs tuning
-            let threshold = 0.000000001;
-            if bin.re + bin.im < threshold { 
-                trace!(
-                    "carrier acq: nothing ({}) below cutoff threshold {}. stop",
-                    bin.re + bin.im,
-                    threshold,
-                );
-                break;
-            } 
-
-            let phase = f64::atan2(bin.im, bin.re);
-            let ampl = bin.norm();
-
-            // if the phase just passed 0, increasing:
-            if phase > 0.0 && prev_phase < 0.0 {
-                carrier.acquired = true;
-                debug!("carrier acq: success!");
-                debug!(
-                    "carrier acq: drift {} matching {} ampl {}, nearest bin {}",
-                    phase,
-                    window_offset,
-                    ampl,
-                    sample_rate.0 as usize * clock_freq_index / fft_size,
-                );
-            } else {
-                window_offset += 1;
-                if window_offset > fft_size {
-                    trace!("carrier acq: can't find what i smell. stop");
-                    break;
-                }
-                trace!(
-                    "carrier acq: carrier ampl {} but phase {}, spinning...",
-                    ampl,
-                    phase,                    
-                );
-            }
-            prev_phase = phase;
-        } else { 
-            warn!("carrier acq: window overran FFT frame! report this!");
-            break; 
+        if phase_error.abs() < 0.01 {
+            ok_in_a_row += 1;
+            if ok_in_a_row >= 25 { carrier.acquired = true; break; }
         }
-    }
 
-    while carrier.acquired && pcm_buffer.len() >= fft_size {
-        //fft_machine.process(&mut fft_frame);
-
-        /*for (i, s) in fft_frame.iter().enumerate() {
-            let freq = sample_rate.0 as usize * i / fft_size;
-            let ampl = s.norm();
-            let confidence = ampl;
-            if ampl > 50.0 {
-                println!("{:?}: {:?}", freq, ampl);
-            }
-
-            // soon: confidence = ampl / |freq_target - freq_observed|
-        }*/
+        phase_out += alpha * phase_error;
+        freq_out += beta * phase_error;
+        //phase_in += freq_in;
+        phase_out += freq_out;
     }
 }