Skip to main content

qualia_audio/features/peaks/
spectral_peaks.rs

1//! Spectral peaks from a magnitude spectrum (parabolic sub-bin refinement).
2
3use crate::types::AudioError;
4
5/// Parabolic sub-bin offset and interpolated peak height for the three
6/// magnitude ordinates `(ym1, y0, yp1)` centred on a local maximum bin.
7/// Returns `(offset, peak_value)` with `offset` in `(-0.5, 0.5)`.
8#[inline]
9fn parabolic(ym1: f32, y0: f32, yp1: f32) -> (f32, f32) {
10    let denom = ym1 - 2.0 * y0 + yp1;
11    if denom == 0.0 || !denom.is_finite() {
12        return (0.0, y0);
13    }
14    let off = (0.5 * (ym1 - yp1) / denom).clamp(-0.5, 0.5);
15    // Interpolated vertex height y0 - 0.25*(ym1 - yp1)*offset.
16    let peak = y0 - 0.25 * (ym1 - yp1) * off;
17    (off, peak)
18}
19
20/// Extract the strongest spectral peaks from a magnitude spectrum `mag`.
21///
22/// `mag` is a one-sided magnitude spectrum covering DC..Nyquist inclusive, so a
23/// spectrum of `N` bins was produced by an FFT of size `2*(N-1)` and the bin
24/// spacing is `sample_rate / (2*(N-1))` Hz. Each interior local maximum is
25/// parabolically interpolated to a sub-bin frequency. The up-to-`max_peaks`
26/// strongest peaks are written **magnitude-descending** (strongest first):
27/// `out_freq[k]` the interpolated frequency in Hz, `out_mag[k]` the interpolated
28/// peak magnitude.
29///
30/// Returns the number of peaks written.
31///
32/// Zero-heap: the output arrays hold an insertion-sorted top-K; no allocation.
33///
34/// # Errors
35/// - [`AudioError::InvalidParameter`] if `sample_rate` is not a positive finite.
36pub fn spectral_peaks(
37    mag: &[f32],
38    sample_rate: f32,
39    max_peaks: usize,
40    out_freq: &mut [f32],
41    out_mag: &mut [f32],
42) -> Result<usize, AudioError> {
43    if sample_rate <= 0.0 || !sample_rate.is_finite() {
44        return Err(AudioError::InvalidParameter);
45    }
46    let cap = out_freq.len().min(out_mag.len()).min(max_peaks);
47    if mag.len() < 3 || cap == 0 {
48        return Ok(0);
49    }
50    let bin_hz = sample_rate / (2.0 * (mag.len() - 1) as f32);
51
52    let mut count: usize = 0;
53    for k in 1..mag.len() - 1 {
54        let y0 = mag[k];
55        if !(y0 > mag[k - 1] && y0 >= mag[k + 1] && y0 > 0.0) {
56            continue;
57        }
58        let (off, peak) = parabolic(mag[k - 1], y0, mag[k + 1]);
59        let freq = (k as f32 + off) * bin_hz;
60
61        // Insert into the magnitude-descending top-K held in the out arrays.
62        if count < cap {
63            // Shift down to open the insertion slot.
64            let mut j = count;
65            while j > 0 && out_mag[j - 1] < peak {
66                out_mag[j] = out_mag[j - 1];
67                out_freq[j] = out_freq[j - 1];
68                j -= 1;
69            }
70            out_mag[j] = peak;
71            out_freq[j] = freq;
72            count += 1;
73        } else if peak > out_mag[cap - 1] {
74            // Displace the current weakest, then bubble into place.
75            let mut j = cap - 1;
76            while j > 0 && out_mag[j - 1] < peak {
77                out_mag[j] = out_mag[j - 1];
78                out_freq[j] = out_freq[j - 1];
79                j -= 1;
80            }
81            out_mag[j] = peak;
82            out_freq[j] = freq;
83        }
84    }
85    Ok(count)
86}
87
88#[cfg(test)]
89mod tests {
90    use super::*;
91
92    /// Golden: a downward parabola with vertex at bin 100.3 in a 513-bin
93    /// (fft_size=1024) spectrum at 44100 Hz. Parabolic interpolation recovers
94    /// the vertex frequency exactly (parabola in, parabola out).
95    #[test]
96    fn golden_peak_frequency() {
97        let sr = 44100.0f32;
98        let n = 513usize; // fft_size = 1024
99        let bin_hz = sr / (2.0 * (n - 1) as f32); // 43.066 Hz
100        let center = 100.3f32;
101        let mut mag = vec![0.0f32; n];
102        for (k, m) in mag.iter_mut().enumerate() {
103            let v = 1.0 - 0.01 * (k as f32 - center) * (k as f32 - center);
104            *m = v.max(0.0);
105        }
106        let mut fr = [0.0f32; 4];
107        let mut mg = [0.0f32; 4];
108        let np = spectral_peaks(&mag, sr, 4, &mut fr, &mut mg).expect("peaks");
109        assert_eq!(np, 1);
110        let true_hz = center * bin_hz; // 4319.56 Hz
111        assert!(
112            (fr[0] - true_hz).abs() < 2.0,
113            "freq={} want={}",
114            fr[0],
115            true_hz
116        );
117        assert!(mg[0] > 0.99, "peak mag={}", mg[0]);
118    }
119
120    /// Two peaks -> strongest returned first, ordering is magnitude-descending.
121    #[test]
122    fn strongest_first() {
123        let sr = 8000.0f32;
124        let n = 129usize; // fft_size 256, bin_hz = 8000/256 = 31.25
125        let mut mag = vec![0.0f32; n];
126        // Weaker peak at bin 20, stronger at bin 60.
127        mag[19] = 0.5;
128        mag[20] = 1.0;
129        mag[21] = 0.5;
130        mag[59] = 1.5;
131        mag[60] = 3.0;
132        mag[61] = 1.5;
133        let mut fr = [0.0f32; 4];
134        let mut mg = [0.0f32; 4];
135        let np = spectral_peaks(&mag, sr, 4, &mut fr, &mut mg).expect("peaks");
136        assert_eq!(np, 2);
137        assert!(mg[0] > mg[1]);
138        // Strongest is bin 60 -> 60 * 31.25 = 1875 Hz (symmetric -> no offset).
139        assert!((fr[0] - 1875.0).abs() < 1.0, "freq0={}", fr[0]);
140        assert!((fr[1] - 625.0).abs() < 1.0, "freq1={}", fr[1]);
141    }
142
143    /// max_peaks bounds the output to the strongest K.
144    #[test]
145    fn max_peaks_limits() {
146        let sr = 8000.0f32;
147        let n = 129usize;
148        let mut mag = vec![0.0f32; n];
149        for c in [10usize, 30, 50, 70] {
150            mag[c - 1] = 0.5;
151            mag[c] = c as f32; // taller for larger bins
152            mag[c + 1] = 0.5;
153        }
154        let mut fr = [0.0f32; 2];
155        let mut mg = [0.0f32; 2];
156        let np = spectral_peaks(&mag, sr, 2, &mut fr, &mut mg).expect("peaks");
157        assert_eq!(np, 2);
158        // Two tallest are bins 70 and 50.
159        assert!((fr[0] - 70.0 * (sr / 256.0)).abs() < 1.0, "f0={}", fr[0]);
160        assert!((fr[1] - 50.0 * (sr / 256.0)).abs() < 1.0, "f1={}", fr[1]);
161    }
162
163    #[test]
164    fn rejects_bad_rate() {
165        let mag = [0.0f32, 1.0, 0.0];
166        let mut fr = [0.0f32; 2];
167        let mut mg = [0.0f32; 2];
168        assert_eq!(
169            spectral_peaks(&mag, 0.0, 2, &mut fr, &mut mg),
170            Err(AudioError::InvalidParameter)
171        );
172    }
173}