//! Levels of the harmonics of a known fundamental, measured with Hann-windowed Goertzel
//! filters. Work is spread evenly over the samples, so there is no per-window spike.
use std::f32::consts::TAU;

pub const HARMONICS: usize = 32;

pub struct HarmonicMeter {
    rate: f32,
    window: Vec<f32>,
    count: usize,
    hz: f32,
    coef: [f32; HARMONICS],
    s1: [f32; HARMONICS],
    s2: [f32; HARMONICS],
    levels: [f32; HARMONICS],
}
impl HarmonicMeter {
    /// Window of about `ms`; 85 ms resolves harmonics of fundamentals down to 30 Hz.
    pub fn new(rate: f32, ms: f32) -> Self {
        let n = ((rate * ms * 0.001) as usize).max(64);
        Self {
            rate,
            window: (0..n)
                .map(|i| 0.5 - 0.5 * (TAU * i as f32 / n as f32).cos())
                .collect(),
            count: 0,
            hz: 0.,
            coef: [0.; HARMONICS],
            s1: [0.; HARMONICS],
            s2: [0.; HARMONICS],
            levels: [-120.; HARMONICS],
        }
    }
    /// Peak level in dBFS of each harmonic (index 0 is the fundamental) from the last
    /// complete window. Harmonics above 0.45 × the sample rate read −120.
    pub fn levels(&self) -> &[f32; HARMONICS] {
        &self.levels
    }
    /// Fundamental of the last complete window, 0 if none.
    pub fn hz(&self) -> f32 {
        self.hz
    }
    /// Feeds one sample. `hz` is read at the start of each window (0 = no note: skip it).
    /// Returns true when a window completed and `levels` changed.
    #[inline]
    pub fn push(&mut self, x: f32, hz: f32) -> bool {
        if self.count == 0 {
            self.hz = hz;
            for k in 0..HARMONICS {
                let f = hz * (k + 1) as f32;
                self.coef[k] = if hz > 0. && f < 0.45 * self.rate {
                    2. * (TAU * f / self.rate).cos()
                } else {
                    0.
                };
            }
            self.s1 = [0.; HARMONICS];
            self.s2 = [0.; HARMONICS];
        }
        let w = x * self.window[self.count];
        for k in 0..HARMONICS {
            let s = w + self.coef[k] * self.s1[k] - self.s2[k];
            self.s2[k] = self.s1[k];
            self.s1[k] = s;
        }
        self.count += 1;
        if self.count < self.window.len() {
            return false;
        }
        self.count = 0;
        let n = self.window.len() as f32;
        for k in 0..HARMONICS {
            let f = self.hz * (k + 1) as f32;
            self.levels[k] = if self.hz > 0. && f < 0.45 * self.rate {
                let c = self.coef[k];
                let p =
                    self.s1[k] * self.s1[k] + self.s2[k] * self.s2[k] - c * self.s1[k] * self.s2[k];
                // Hann coherent gain is 0.5: a sine of peak A gives |X| = A·N/4.
                let amp = 4. * p.max(0.).sqrt() / n;
                20. * amp.max(1e-6).log10()
            } else {
                -120.
            };
        }
        true
    }
    pub fn reset(&mut self) {
        self.count = 0;
        self.levels = [-120.; HARMONICS];
    }
}