Skip to main content

qualia_audio/features/pitch/
yin_fft.rs

1//! YinFFT: the YIN difference function computed via an FFT autocorrelation
2//! rather than the `O(W · max_lag)` direct double loop.
3//!
4//! The YIN difference function expands as
5//!
6//! ```text
7//! d(τ) = Σ_{j<W}(x[j] − x[j+τ])²
8//!      = Σ_{j<W} x[j]²        (E0, constant)
9//!      + Σ_{j<W} x[j+τ]²      (sliding energy, from a prefix sum of squares)
10//!      − 2 · Σ_{j<W} x[j]x[j+τ]   (autocorrelation, from the FFT)
11//! ```
12//!
13//! The autocorrelation term is obtained as `IFFT(conj(FFT(a)) · FFT(b))` where
14//! `a` is the first `W` samples (zero-padded) and `b` is the whole frame
15//! (zero-padded), giving the linear correlation at lags `0..=max_lag` with no
16//! circular wrap. The two energy terms come from one prefix-sum pass. The
17//! result is the *same* difference function as [`super::yin`], so it feeds the
18//! identical CMND → threshold → parabolic-interpolation spine.
19//!
20//! Zero-heap hot path: a single caller `scratch` buffer is carved into the two
21//! complex FFT work areas, the prefix-sum table, and the difference/CMND buffer.
22//! Nothing is allocated per call.
23
24use crate::features::fft::radix2::fft_radix2;
25use crate::features::pitch::confidence::pitch_confidence;
26use crate::features::pitch::estimate::{
27    absolute_threshold, cmnd_in_place, parabolic_min, PitchEstimate,
28};
29use crate::types::AudioError;
30
31/// Minimum `scratch` length (in `f32`) required for a frame of `n` samples with
32/// a maximum lag of `max_lag`. `l` is the FFT size (`n` rounded up to a power of
33/// two). Exposed so callers can size the buffer without guessing.
34#[inline]
35pub fn yin_fft_scratch_len(n: usize, max_lag: usize) -> usize {
36    let l = n.max(1).next_power_of_two();
37    4 * l + n + max_lag + 2
38}
39
40/// Estimate the fundamental frequency of `frame` with YinFFT.
41///
42/// Semantics and return values match [`super::yin::yin_pitch`]; only the
43/// difference-function computation differs (FFT autocorrelation vs. direct
44/// loop). `scratch` must hold at least [`yin_fft_scratch_len`] floats.
45///
46/// # Errors
47/// - [`AudioError::InvalidParameter`] on a non-positive/inconsistent rate or
48///   band, or a frame too short to form a usable lag range.
49/// - [`AudioError::OutputBufferTooSmall`] if `scratch` is undersized.
50pub fn yin_fft_pitch(
51    frame: &[f32],
52    sample_rate: f32,
53    min_hz: f32,
54    max_hz: f32,
55    threshold: f32,
56    scratch: &mut [f32],
57) -> Result<PitchEstimate, AudioError> {
58    let params_ok = sample_rate.is_finite()
59        && sample_rate > 0.0
60        && min_hz.is_finite()
61        && min_hz > 0.0
62        && max_hz.is_finite()
63        && max_hz > min_hz
64        && threshold.is_finite();
65    if !params_ok {
66        return Err(AudioError::InvalidParameter);
67    }
68    let n = frame.len();
69    if n < 8 {
70        return Err(AudioError::InvalidParameter);
71    }
72
73    let min_lag = ((sample_rate / max_hz).floor() as usize).max(2);
74    let max_lag = ((sample_rate / min_hz).floor() as usize).min(n / 2);
75    if max_lag < min_lag + 2 {
76        return Err(AudioError::InvalidParameter);
77    }
78
79    let l = n.next_power_of_two();
80    if scratch.len() < yin_fft_scratch_len(n, max_lag) {
81        return Err(AudioError::OutputBufferTooSmall);
82    }
83    let w = n - max_lag; // integration window (a[j] support)
84
85    // Carve the scratch into disjoint work areas.
86    let (comp_a, rest) = scratch.split_at_mut(2 * l);
87    let (comp_b, rest) = rest.split_at_mut(2 * l);
88    let (psum, rest) = rest.split_at_mut(n + 1);
89    let (diff, _) = rest.split_at_mut(max_lag + 1);
90
91    // --- Pack a (first W samples) and b (whole frame) as zero-padded complex.
92    for i in 0..l {
93        let av = if i < w { frame[i] } else { 0.0 };
94        let bv = if i < n { frame[i] } else { 0.0 };
95        comp_a[2 * i] = av;
96        comp_a[2 * i + 1] = 0.0;
97        comp_b[2 * i] = bv;
98        comp_b[2 * i + 1] = 0.0;
99    }
100
101    // --- Autocorrelation via FFT: IFFT(conj(A) · B).
102    fft_radix2(comp_a, false)?;
103    fft_radix2(comp_b, false)?;
104    for k in 0..l {
105        let ar = comp_a[2 * k];
106        let ai = comp_a[2 * k + 1];
107        let br = comp_b[2 * k];
108        let bi = comp_b[2 * k + 1];
109        // conj(A) · B = (ar − i·ai)(br + i·bi)
110        comp_a[2 * k] = ar * br + ai * bi;
111        comp_a[2 * k + 1] = ar * bi - ai * br;
112    }
113    fft_radix2(comp_a, true)?; // IFFT (1/L normalised); real part = correlation
114
115    // --- Prefix sum of squares (f64 accumulate) for the energy terms.
116    let mut run = 0.0f64;
117    psum[0] = 0.0;
118    for k in 0..n {
119        let x = frame[k] as f64;
120        run += x * x;
121        psum[k + 1] = run as f32;
122    }
123    let e0 = psum[w];
124
125    // --- Reconstruct the difference function d(τ).
126    diff[0] = 0.0;
127    for tau in 1..=max_lag {
128        let sliding = psum[tau + w] - psum[tau];
129        let corr = comp_a[2 * tau];
130        let d = e0 + sliding - 2.0 * corr;
131        diff[tau] = if d > 0.0 { d } else { 0.0 };
132    }
133
134    // --- Shared CMND → threshold → parabolic spine.
135    cmnd_in_place(diff, max_lag);
136    let tau = absolute_threshold(diff, min_lag, max_lag, threshold);
137    let (better_tau, aperiodicity) = parabolic_min(diff, tau, min_lag, max_lag);
138
139    if !(better_tau.is_finite() && better_tau > 0.0) {
140        return Ok(PitchEstimate::unvoiced());
141    }
142    let confidence = pitch_confidence(aperiodicity);
143    if aperiodicity > 1.0 || (diff[tau] >= threshold && aperiodicity > 0.5) {
144        return Ok(PitchEstimate {
145            f0_hz: 0.0,
146            confidence,
147        });
148    }
149    Ok(PitchEstimate {
150        f0_hz: sample_rate / better_tau,
151        confidence,
152    })
153}
154
155#[cfg(test)]
156mod tests {
157    use super::*;
158    use core::f32::consts::TAU;
159
160    const SR: f32 = 44_100.0;
161
162    fn sine(freq: f32, n: usize, sr: f32) -> Vec<f32> {
163        (0..n).map(|i| (TAU * freq * i as f32 / sr).sin()).collect()
164    }
165
166    fn cents(a: f32, b: f32) -> f32 {
167        1200.0 * (a / b).log2().abs()
168    }
169
170    #[test]
171    fn recovers_440_within_one_percent() {
172        let frame = sine(440.0, 2048, SR);
173        let mut scratch = vec![0.0f32; yin_fft_scratch_len(2048, 551)];
174        let est = yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut scratch).expect("yinfft");
175        let err = (est.f0_hz - 440.0).abs() / 440.0;
176        assert!(err < 0.01, "f0={} err={err}", est.f0_hz);
177        assert!(
178            cents(est.f0_hz, 440.0) < 10.0,
179            "cents={}",
180            cents(est.f0_hz, 440.0)
181        );
182        assert!(est.confidence > 0.9, "conf={}", est.confidence);
183    }
184
185    #[test]
186    fn recovers_220_within_one_percent() {
187        let frame = sine(220.0, 2048, SR);
188        let mut scratch = vec![0.0f32; yin_fft_scratch_len(2048, 551)];
189        let est = yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut scratch).expect("yinfft");
190        let err = (est.f0_hz - 220.0).abs() / 220.0;
191        assert!(err < 0.01, "f0={} err={err}", est.f0_hz);
192        assert!(est.confidence > 0.9, "conf={}", est.confidence);
193    }
194
195    /// Half-integer true lag (sr/f = 100.5): only parabolic interpolation can
196    /// reach within a few cents — proves the interpolation on the FFT path too.
197    #[test]
198    fn parabolic_beats_integer_lag() {
199        let f0 = SR / 100.5;
200        let frame = sine(f0, 2048, SR);
201        let mut scratch = vec![0.0f32; yin_fft_scratch_len(2048, 551)];
202        let est = yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut scratch).expect("yinfft");
203        let c = cents(est.f0_hz, f0);
204        assert!(c < 5.0, "recovered {} Hz vs {f0} Hz ({c} cents)", est.f0_hz);
205        assert!(cents(SR / 100.0, f0) > 8.0);
206    }
207
208    /// The FFT path must agree with the direct-loop YIN on a clean tone.
209    #[test]
210    fn matches_direct_yin() {
211        use crate::features::pitch::yin::yin_pitch;
212        let frame = sine(329.63, 2048, SR); // E4
213        let mut s1 = vec![0.0f32; 1200];
214        let mut s2 = vec![0.0f32; yin_fft_scratch_len(2048, 551)];
215        let a = yin_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut s1).expect("yin");
216        let b = yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut s2).expect("yinfft");
217        assert!(
218            cents(a.f0_hz, b.f0_hz) < 2.0,
219            "yin={} yinfft={}",
220            a.f0_hz,
221            b.f0_hz
222        );
223    }
224
225    #[test]
226    fn rejects_short_scratch() {
227        let frame = sine(440.0, 2048, SR);
228        let mut scratch = vec![0.0f32; 16];
229        assert_eq!(
230            yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut scratch),
231            Err(AudioError::OutputBufferTooSmall)
232        );
233    }
234}