//! Continuous phase-wheel analysis on the native-rate audio stream.

const HILBERT_TAPS: usize = 33;
const HILBERT_HALF: usize = (HILBERT_TAPS - 1) / 2;
const BANDPASS_LOW_HZ: f64 = 300.0;
const BANDPASS_HIGH_HZ: f64 = 5_000.0;

#[derive(Clone, Copy, Debug, Default)]
pub struct PhaseWheelSnapshot {
    pub angle_rad: Option<f32>,
    pub coherence: f32,
    pub level: f32,
    pub peak: f32,
}

#[derive(Clone, Copy, Debug, Default)]
struct BandpassChannel {
    hp_x: f64,
    hp_y: f64,
    lp_y: f64,
}

impl BandpassChannel {
    fn process(&mut self, sample: f64, hp_alpha: f64, lp_alpha: f64) -> f64 {
        let hp = hp_alpha * (self.hp_y + sample - self.hp_x);
        self.hp_x = sample;
        self.hp_y = hp;
        self.lp_y = lp_alpha * hp + (1.0 - lp_alpha) * self.lp_y;
        self.lp_y
    }
}

pub struct PhaseWheelAnalyzer {
    sample_rate: u32,
    hp_alpha: f64,
    lp_alpha: f64,
    band_l: BandpassChannel,
    band_r: BandpassChannel,
    ring_l: [f64; HILBERT_TAPS],
    ring_r: [f64; HILBERT_TAPS],
    hilbert: [f64; HILBERT_TAPS],
    write: usize,
    fill: usize,
    cross_re: f64,
    cross_im: f64,
    weight_sum: f64,
    amplitude_sum: f64,
    amplitude_peak: f64,
    count: usize,
}

impl PhaseWheelAnalyzer {
    pub fn new(sample_rate: u32) -> Self {
        let mut analyzer = Self {
            sample_rate: 0,
            hp_alpha: 0.0,
            lp_alpha: 0.0,
            band_l: BandpassChannel::default(),
            band_r: BandpassChannel::default(),
            ring_l: [0.0; HILBERT_TAPS],
            ring_r: [0.0; HILBERT_TAPS],
            hilbert: build_hilbert_kernel(),
            write: 0,
            fill: 0,
            cross_re: 0.0,
            cross_im: 0.0,
            weight_sum: 0.0,
            amplitude_sum: 0.0,
            amplitude_peak: 0.0,
            count: 0,
        };
        analyzer.configure(sample_rate);
        analyzer
    }

    pub fn configure(&mut self, sample_rate: u32) {
        let rate = sample_rate.max(8_000);
        if self.sample_rate == rate {
            return;
        }
        self.sample_rate = rate;
        self.hp_alpha = highpass_alpha(rate, BANDPASS_LOW_HZ);
        self.lp_alpha = lowpass_alpha(rate, BANDPASS_HIGH_HZ);
        self.band_l = BandpassChannel::default();
        self.band_r = BandpassChannel::default();
        self.ring_l.fill(0.0);
        self.ring_r.fill(0.0);
        self.write = 0;
        self.fill = 0;
        self.clear_accumulator();
    }

    pub fn process(&mut self, left: f32, right: f32) {
        let filtered_l = self
            .band_l
            .process(left as f64, self.hp_alpha, self.lp_alpha);
        let filtered_r = self
            .band_r
            .process(right as f64, self.hp_alpha, self.lp_alpha);
        self.ring_l[self.write] = filtered_l;
        self.ring_r[self.write] = filtered_r;
        self.write = (self.write + 1) % HILBERT_TAPS;
        self.fill = (self.fill + 1).min(HILBERT_TAPS);
        if self.fill < HILBERT_TAPS {
            return;
        }

        // `write` points at the oldest sample. The real component is delayed
        // by half the FIR length, so it is aligned with the causal Hilbert FIR.
        let real_index = (self.write + HILBERT_HALF) % HILBERT_TAPS;
        let l_re = self.ring_l[real_index].clamp(-1.0, 1.0);
        let r_re = self.ring_r[real_index].clamp(-1.0, 1.0);
        let mut l_im = 0.0;
        let mut r_im = 0.0;
        // The ideal odd Hilbert kernel has zero coefficients at every even
        // offset; with a 33-tap kernel those are the even tap indices.
        for tap in (1..HILBERT_TAPS).step_by(2) {
            let index = (self.write + tap) % HILBERT_TAPS;
            l_im += self.ring_l[index] * self.hilbert[tap];
            r_im += self.ring_r[index] * self.hilbert[tap];
        }

        let mag_l = l_re.hypot(l_im).min(1.0);
        let mag_r = r_re.hypot(r_im).min(1.0);
        let weight = mag_l * mag_r;
        // zL * conj(zR): its argument is the energy-weighted L/R phase.
        self.cross_re += l_re * r_re + l_im * r_im;
        self.cross_im += l_im * r_re - l_re * r_im;
        self.weight_sum += weight;
        let amplitude = 0.5 * (mag_l + mag_r);
        self.amplitude_sum += amplitude;
        self.amplitude_peak = self.amplitude_peak.max(amplitude);
        self.count += 1;
    }

    pub fn take_snapshot(&mut self) -> PhaseWheelSnapshot {
        let level = if self.count > 0 {
            (self.amplitude_sum / self.count as f64) as f32
        } else {
            0.0
        };
        let resultant = self.cross_re.hypot(self.cross_im);
        let angle_rad = if self.weight_sum > 1e-12 && resultant > self.weight_sum * 1e-9 {
            Some(self.cross_im.atan2(self.cross_re) as f32)
        } else {
            None
        };
        let coherence = if self.weight_sum > 1e-12 {
            (resultant / self.weight_sum).clamp(0.0, 1.0) as f32
        } else {
            0.0
        };
        let snapshot = PhaseWheelSnapshot {
            angle_rad,
            coherence,
            level,
            peak: self.amplitude_peak as f32,
        };
        self.clear_accumulator();
        snapshot
    }

    fn clear_accumulator(&mut self) {
        self.cross_re = 0.0;
        self.cross_im = 0.0;
        self.weight_sum = 0.0;
        self.amplitude_sum = 0.0;
        self.amplitude_peak = 0.0;
        self.count = 0;
    }
}

fn highpass_alpha(sample_rate: u32, cutoff: f64) -> f64 {
    let rc = 1.0 / (2.0 * std::f64::consts::PI * cutoff.max(1.0));
    let dt = 1.0 / sample_rate.max(1) as f64;
    (rc / (rc + dt)).clamp(0.0, 1.0)
}

fn lowpass_alpha(sample_rate: u32, cutoff: f64) -> f64 {
    let rc = 1.0 / (2.0 * std::f64::consts::PI * cutoff.max(1.0));
    let dt = 1.0 / sample_rate.max(1) as f64;
    (dt / (rc + dt)).clamp(0.0, 1.0)
}

fn build_hilbert_kernel() -> [f64; HILBERT_TAPS] {
    let mut kernel = [0.0; HILBERT_TAPS];
    for (index, value) in kernel.iter_mut().enumerate() {
        let offset = index as isize - HILBERT_HALF as isize;
        if offset == 0 || offset % 2 == 0 {
            continue;
        }
        let window = 0.54
            - 0.46
                * ((2.0 * std::f64::consts::PI * index as f64) / (HILBERT_TAPS - 1) as f64).cos();
        *value = 2.0 / (std::f64::consts::PI * offset as f64) * window;
    }
    kernel
}

#[cfg(test)]
mod tests {
    use super::*;

    fn feed_tone(analyzer: &mut PhaseWheelAnalyzer, phase: f64, samples: usize) {
        let omega = 2.0 * std::f64::consts::PI * 1_000.0 / 48_000.0;
        for index in 0..samples {
            let t = omega * index as f64;
            analyzer.process((0.5 * t.sin()) as f32, (0.5 * (t - phase).sin()) as f32);
        }
    }

    #[test]
    fn continuous_analyzer_tracks_tone_phase() {
        let mut analyzer = PhaseWheelAnalyzer::new(48_000);
        feed_tone(&mut analyzer, std::f64::consts::FRAC_PI_2, 4_800);
        let snapshot = analyzer.take_snapshot();
        let angle = snapshot.angle_rad.expect("coherent tone has a phase");
        assert!((angle.abs() - std::f32::consts::FRAC_PI_2).abs() < 0.03);
        assert!(
            snapshot.coherence > 0.9,
            "coherence was {}",
            snapshot.coherence
        );
        assert!(snapshot.level > 0.1);
        assert!(snapshot.peak >= snapshot.level);
    }

    #[test]
    fn snapshot_reset_does_not_reset_filter_or_hilbert_history() {
        let mut analyzer = PhaseWheelAnalyzer::new(48_000);
        feed_tone(&mut analyzer, 0.4, 2_400);
        let first = analyzer.take_snapshot().angle_rad.unwrap();
        feed_tone(&mut analyzer, 0.4, 800);
        let second = analyzer.take_snapshot().angle_rad.unwrap();
        assert!((first - second).abs() < 0.03);
    }

    #[test]
    fn one_sided_signal_does_not_invent_a_phase() {
        let mut analyzer = PhaseWheelAnalyzer::new(48_000);
        for index in 0..2_400 {
            let t = 2.0 * std::f64::consts::PI * 1_000.0 * index as f64 / 48_000.0;
            analyzer.process((0.5 * t.sin()) as f32, 0.0);
        }
        let snapshot = analyzer.take_snapshot();
        assert!(snapshot.angle_rad.is_none());
        assert_eq!(snapshot.coherence, 0.0);
    }

    #[test]
    fn energetic_component_dominates_a_quiet_conflicting_tone() {
        let mut analyzer = PhaseWheelAnalyzer::new(48_000);
        for index in 0..9_600 {
            let t = index as f64 / 48_000.0;
            let strong = 2.0 * std::f64::consts::PI * 1_000.0 * t;
            let quiet = 2.0 * std::f64::consts::PI * 2_000.0 * t;
            let left = 0.5 * strong.sin() + 0.04 * quiet.sin();
            let right = 0.5 * strong.sin() + 0.04 * (quiet - std::f64::consts::FRAC_PI_2).sin();
            analyzer.process(left as f32, right as f32);
        }
        let angle = analyzer.take_snapshot().angle_rad.unwrap();
        assert!(angle.abs() < 0.03, "quiet tone pulled phase to {angle}");
    }
}