rusty_mp3 0.4.1

//! Hybrid synthesis stage 2: the 32-band polyphase synthesis filterbank.
//!
//! Each of the 18 passes takes 32 subband samples and produces 32 PCM samples.
//! A 32→64 cosine matrixing feeds the 1024-sample FIFO `V[]`; a windowing step
//! gathers `U[]` from `V[]`, multiplies by the 512-tap `D[]` window, and sums to
//! 32 outputs. The matrixing is computed here; `D[]` is tabulated in [`tables`](crate::tables).

use std::f64::consts::PI;
use std::sync::OnceLock;

use crate::frame::{GRANULE_LINES, SUBBANDS, SUBBAND_LINES};

use super::synth_window::SYNTH_D;

/// Matrixing coefficients `N[i][k] = cos((16+i)·(2k+1)·π/64)`, 64×32. The dense
/// reference — still the source of truth that [`matrixing_fast`] is gated against.
fn matrix() -> &'static [[f32; SUBBANDS]; 64] {
    static T: OnceLock<[[f32; SUBBANDS]; 64]> = OnceLock::new();
    T.get_or_init(|| {
        let mut n = [[0f32; SUBBANDS]; 64];
        for (i, row) in n.iter_mut().enumerate() {
            for (k, c) in row.iter_mut().enumerate() {
                *c = (PI / 64.0 * (16 + i) as f64 * (2 * k + 1) as f64).cos() as f32;
            }
        }
        n
    })
}

/// Half-width DCT kernel `C[m][k] = cos(m·(2k+1)·π/64)`, `m=0..31`, `k=0..15` —
/// the 32 distinct matrixing values `G[m]` after both symmetries are folded out.
fn half_dct() -> &'static [[f32; 16]; 32] {
    static T: OnceLock<[[f32; 16]; 32]> = OnceLock::new();
    T.get_or_init(|| {
        let mut c = [[0f32; 16]; 32];
        for (m, row) in c.iter_mut().enumerate() {
            for (k, e) in row.iter_mut().enumerate() {
                *e = (PI / 64.0 * m as f64 * (2 * k + 1) as f64).cos() as f32;
            }
        }
        c
    })
}

/// **B3** — the 64 matrixing outputs `V[i] = Σ_k cos((16+i)(2k+1)π/64)·s[k]`,
/// computed via two exact cosine symmetries instead of a dense 64×32 product:
///
/// * within `k`: `cos(m·(63−2k)π/64) = (−1)^m cos(m·(2k+1)π/64)` folds the 32
///   inputs into 16 sum/difference terms, so each `G[m]` is 16 mults not 32;
/// * across `i`: every `V[i]` is a signed copy of one of just 32 values
///   `G[m] = Σ_k cos(m(2k+1)π/64)·s[k]` (`m=0..31`) — `V[16]` is identically 0.
///
/// 512 mults vs 2048. Equal to [`matrix`] up to float reassociation (the symmetry
/// is exact; only ULP rounding differs) — pinned by `fast_matrixing_matches_dense`.
fn matrixing_fast(s: &[f32; SUBBANDS]) -> [f32; 64] {
    let c = half_dct();
    // Fold k↔31−k: even-m terms use the sum, odd-m terms the difference.
    let mut plus = [0f32; 16];
    let mut minus = [0f32; 16];
    for k in 0..16 {
        plus[k] = s[k] + s[31 - k];
        minus[k] = s[k] - s[31 - k];
    }
    // The 32 distinct DCT outputs G[0..31].
    let mut g = [0f32; 32];
    for (m, gm) in g.iter_mut().enumerate() {
        let src = if m & 1 == 0 { &plus } else { &minus };
        let mut acc = 0f32;
        for k in 0..16 {
            acc += c[m][k] * src[k];
        }
        *gm = acc;
    }
    // Map G → the 64 V outputs by sign/index (V[16] = 0).
    let mut vv = [0f32; 64];
    for (i, vi) in vv.iter_mut().enumerate() {
        *vi = if i < 16 {
            g[16 + i]
        } else if i == 16 {
            0.0
        } else if i < 48 {
            -g[48 - i]
        } else {
            -g[i - 48]
        };
    }
    vv
}

/// Run the synthesis filterbank for one channel's granule (subband-major `time`),
/// returning 576 PCM samples. `fifo` is the persistent `V[]` state.
pub fn polyphase(time: &[f32; GRANULE_LINES], fifo: &mut [f32; 1024]) -> [f32; GRANULE_LINES] {
    let d = &SYNTH_D;
    let mut pcm = [0f32; GRANULE_LINES];

    // **D2** — the V FIFO is addressed circularly rather than shifted. Logical
    // index `L` (0 = newest) lives at physical `(head + L) & 1023`, so advancing
    // the FIFO by 64 is a subtraction on `head` instead of a 960-float memmove.
    //
    // The old form moved 960 floats on every one of the 18 passes — 17,280 float
    // moves per granule per channel, ~17.5 GB across a 7-minute track. Here the
    // only movement is one 1024-float rotation at the end, 16.9x less.
    //
    // Every span stays contiguous, so nothing is given up in exchange: `head` is
    // always a multiple of 64, `i*128` a multiple of 128, and 1024 a multiple of
    // 32 — so each 32-long run sits inside one 32-aligned block and never wraps
    // mid-run.
    let mut head = 0usize;

    for v in 0..SUBBAND_LINES {
        // Gather this pass's 32 subband samples.
        let mut s = [0f32; SUBBANDS];
        for (k, sv) in s.iter_mut().enumerate() {
            *sv = time[k * SUBBAND_LINES + v];
        }
        // Advance V by 64 and matrix the new 64 values into the front.
        head = (head + 1024 - 64) & 1023;
        fifo[head..head + 64].copy_from_slice(&matrixing_fast(&s));
        // Build U from V, window with D, sum 16 taps → one PCM sample per j.
        //
        // `i` outer, `j` inner: each of the four operands is then a CONTIGUOUS
        // 32-float run, which auto-vectorizes. The old `j`-outer form walked the
        // FIFO with a stride of 128 floats in its inner loop, which cannot
        // vectorize, and it recomputed the two circular indices 32 times over.
        //
        // Each `out[j]` still accumulates its 16 taps in the original order
        // (i ascending, the `a` term before the `b` term), so this is
        // bit-identical, not merely close.
        let out = &mut pcm[v * 32..v * 32 + 32];
        out.fill(0.0);
        for i in 0..8 {
            let a = (head + i * 128) & 1023;
            let b = (head + i * 128 + 96) & 1023;
            let (fa, fb) = (&fifo[a..a + 32], &fifo[b..b + 32]);
            let (da, db) = (&d[i * 64..i * 64 + 32], &d[i * 64 + 32..i * 64 + 64]);
            for j in 0..32 {
                out[j] += fa[j] * da[j];
                out[j] += fb[j] * db[j];
            }
        }
    }
    // Restore the canonical `head == 0` layout the caller's state is defined in:
    // 18 passes advanced head by 18*64 = 1152 ≡ 128 (mod 1024), leaving it at 896.
    fifo.rotate_left(head);
    pcm
}

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

    #[test]
    fn matrix_matches_cosine_formula() {
        let n = matrix();
        // Spot-check a couple of entries against N[i][k] = cos((16+i)(2k+1)π/64).
        let e =
            |i: usize, k: usize| (PI / 64.0 * (16 + i) as f64 * (2 * k + 1) as f64).cos() as f32;
        assert!((n[0][0] - e(0, 0)).abs() < 1e-6);
        assert!((n[33][7] - e(33, 7)).abs() < 1e-6);
        assert!((n[63][31] - e(63, 31)).abs() < 1e-6);
    }

    /// The dense 64×32 matrixing — the reference `matrixing_fast` must reproduce.
    fn matrixing_dense(s: &[f32; SUBBANDS]) -> [f32; 64] {
        let n = matrix();
        let mut v = [0f32; 64];
        for (i, vi) in v.iter_mut().enumerate() {
            let mut acc = 0f32;
            for k in 0..SUBBANDS {
                acc += n[i][k] * s[k];
            }
            *vi = acc;
        }
        v
    }

    /// **B3 gate.** The fast matrixing must equal the dense reference to float
    /// precision (the symmetries are exact; only ULP reassociation differs) — for
    /// many random subband inputs. A bug in the index/sign map shows up as a large
    /// error here, long before it could reach the FFmpeg-conformance check.
    #[test]
    fn fast_matrixing_matches_dense() {
        let mut st = 0x1234_9ABCu32;
        let mut rng = || {
            st = st.wrapping_mul(1_664_525).wrapping_add(1_013_904_223);
            (st >> 8) as f32 / (1u32 << 24) as f32 * 2.0 - 1.0
        };
        let mut worst = 0f32;
        for _ in 0..2000 {
            let mut s = [0f32; SUBBANDS];
            for sv in s.iter_mut() {
                *sv = rng() * 100.0;
            }
            let dense = matrixing_dense(&s);
            let fast = matrixing_fast(&s);
            for i in 0..64 {
                // relative error vs the row's scale (sum of |s|).
                let scale = s.iter().map(|x| x.abs()).sum::<f32>().max(1.0);
                worst = worst.max((dense[i] - fast[i]).abs() / scale);
            }
        }
        eprintln!("[B3] worst relative matrixing error fast vs dense: {worst:.2e}");
        assert!(
            worst < 1e-5,
            "fast matrixing diverges from dense: {worst:.2e}"
        );
    }

    #[test]
    fn fifo_advances_and_output_is_finite() {
        // Feed every pass (all-ones) so the FIFO stays populated across the 18
        // shifts. With the placeholder D (zeros) the PCM is zero but finite; the
        // matrixing/FIFO must run cleanly and leave the FIFO non-empty.
        let time = [1f32; GRANULE_LINES];
        let mut fifo = [0f32; 1024];
        let pcm = polyphase(&time, &mut fifo);
        assert!(pcm.iter().all(|v| v.is_finite()));
        assert!(
            fifo.iter().any(|&v| v != 0.0),
            "matrixing must fill the FIFO"
        );
    }
}