use std::f64::consts::PI;
use std::sync::OnceLock;
use crate::frame::{GRANULE_LINES, SUBBANDS, SUBBAND_LINES};
use super::synth_window::SYNTH_D;
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
})
}
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
})
}
fn matrixing_fast(s: &[f32; SUBBANDS]) -> [f32; 64] {
let c = half_dct();
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];
}
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;
}
expand_g(&g)
}
fn half_dct_t() -> &'static [[f32; 32]; 16] {
static T: OnceLock<[[f32; 32]; 16]> = OnceLock::new();
T.get_or_init(|| {
let c = half_dct();
let mut t = [[0f32; 32]; 16];
for (m, row) in c.iter().enumerate() {
for (k, v) in row.iter().enumerate() {
t[k][m] = *v;
}
}
t
})
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx")]
unsafe fn matrixing_avx(s: &[f32; SUBBANDS]) -> [f32; 64] {
use std::arch::x86_64::*;
let ct = half_dct_t();
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];
}
#[allow(unused_unsafe)]
let mut acc = [unsafe { _mm256_setzero_ps() }; 4];
for k in 0..16 {
unsafe {
let src = _mm256_blend_ps(_mm256_set1_ps(plus[k]), _mm256_set1_ps(minus[k]), 0xAA);
for (v, accv) in acc.iter_mut().enumerate() {
let c = _mm256_loadu_ps(ct[k].as_ptr().add(v * 8));
*accv = _mm256_add_ps(*accv, _mm256_mul_ps(c, src));
}
}
}
let mut g = [0f32; 32];
for (v, accv) in acc.iter().enumerate() {
unsafe { _mm256_storeu_ps(g.as_mut_ptr().add(v * 8), *accv) };
}
expand_g(&g)
}
#[cfg(target_arch = "aarch64")]
unsafe fn matrixing_neon(s: &[f32; SUBBANDS]) -> [f32; 64] {
use std::arch::aarch64::*;
let ct = half_dct_t();
#[allow(unused_unsafe)]
let mut acc = [unsafe { vdupq_n_f32(0.0) }; 8];
for k in 0..16 {
let (p, m) = (s[k] + s[31 - k], s[k] - s[31 - k]);
let pm = [p, m, p, m];
unsafe {
let src = vld1q_f32(pm.as_ptr());
for (v, accv) in acc.iter_mut().enumerate() {
let c = vld1q_f32(ct[k].as_ptr().add(v * 4));
*accv = vaddq_f32(*accv, vmulq_f32(c, src));
}
}
}
let mut g = [0f32; 32];
for (v, accv) in acc.iter().enumerate() {
unsafe { vst1q_f32(g.as_mut_ptr().add(v * 4), *accv) };
}
expand_g(&g)
}
#[inline]
unsafe fn matrixing_simd(s: &[f32; SUBBANDS]) -> [f32; 64] {
#[cfg(target_arch = "x86_64")]
unsafe {
matrixing_avx(s)
}
#[cfg(target_arch = "aarch64")]
unsafe {
matrixing_neon(s)
}
#[cfg(not(any(target_arch = "x86_64", target_arch = "aarch64")))]
matrixing_fast(s)
}
#[inline]
fn expand_g(g: &[f32; 32]) -> [f32; 64] {
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
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx")]
unsafe fn window_avx(fifo: &[f32; 1024], d: &[f32; 512], head: usize, out: &mut [f32; 32]) {
use std::arch::x86_64::*;
let head = head & !63;
#[allow(unused_unsafe)]
let mut acc = [unsafe { _mm256_setzero_ps() }; 4];
for i in 0..8 {
let a = (head + i * 128) & 0x3C0; let b = (head + i * 128 + 96) & 1023;
for (v, accv) in acc.iter_mut().enumerate() {
unsafe {
let fa = _mm256_loadu_ps(fifo.as_ptr().add(a + v * 8));
let da = _mm256_loadu_ps(d.as_ptr().add(i * 64 + v * 8));
*accv = _mm256_add_ps(*accv, _mm256_mul_ps(fa, da));
let fb = _mm256_loadu_ps(fifo.as_ptr().add(b + v * 8));
let db = _mm256_loadu_ps(d.as_ptr().add(i * 64 + 32 + v * 8));
*accv = _mm256_add_ps(*accv, _mm256_mul_ps(fb, db));
}
}
}
for (v, accv) in acc.iter().enumerate() {
unsafe { _mm256_storeu_ps(out.as_mut_ptr().add(v * 8), *accv) };
}
}
#[cfg(target_arch = "aarch64")]
unsafe fn window_neon(fifo: &[f32; 1024], d: &[f32; 512], head: usize, out: &mut [f32; 32]) {
use std::arch::aarch64::*;
let head = head & !63;
#[allow(unused_unsafe)]
let mut acc = [unsafe { vdupq_n_f32(0.0) }; 8];
for i in 0..8 {
let a = (head + i * 128) & 0x3C0; let b = (head + i * 128 + 96) & 1023;
for (v, accv) in acc.iter_mut().enumerate() {
unsafe {
let fa = vld1q_f32(fifo.as_ptr().add(a + v * 4));
let da = vld1q_f32(d.as_ptr().add(i * 64 + v * 4));
*accv = vaddq_f32(*accv, vmulq_f32(fa, da));
let fb = vld1q_f32(fifo.as_ptr().add(b + v * 4));
let db = vld1q_f32(d.as_ptr().add(i * 64 + 32 + v * 4));
*accv = vaddq_f32(*accv, vmulq_f32(fb, db));
}
}
}
for (v, accv) in acc.iter().enumerate() {
unsafe { vst1q_f32(out.as_mut_ptr().add(v * 4), *accv) };
}
}
#[inline]
unsafe fn window_simd(fifo: &[f32; 1024], d: &[f32; 512], head: usize, out: &mut [f32; 32]) {
#[cfg(target_arch = "x86_64")]
unsafe {
window_avx(fifo, d, head, out);
}
#[cfg(target_arch = "aarch64")]
unsafe {
window_neon(fifo, d, head, out)
}
#[cfg(not(any(target_arch = "x86_64", target_arch = "aarch64")))]
window_scalar(fifo, d, head, out)
}
#[inline]
fn window_scalar(fifo: &[f32; 1024], d: &[f32; 512], head: usize, out: &mut [f32; 32]) {
out.fill(0.0);
for i in 0..8 {
let a = (head + i * 128) & 0x3C0; 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];
}
}
}
#[cfg(test)]
fn have_simd() -> bool {
super::isa::simd_available()
}
pub fn polyphase(time: &[f32; GRANULE_LINES], fifo: &mut [f32; 1024]) -> [f32; GRANULE_LINES] {
let simd = super::isa::use_simd();
super::prof::kernel_tally(
if simd {
&super::prof::K_SYNTH_SIMD
} else {
&super::prof::K_SYNTH_SCALAR
},
SUBBAND_LINES as u64,
);
#[cfg(target_arch = "x86_64")]
if simd {
return unsafe { polyphase_avx(time, fifo) };
}
polyphase_impl(time, fifo, simd)
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx")]
unsafe fn polyphase_avx(
time: &[f32; GRANULE_LINES],
fifo: &mut [f32; 1024],
) -> [f32; GRANULE_LINES] {
polyphase_impl(time, fifo, true)
}
#[allow(clippy::inline_always)]
#[inline(always)]
fn polyphase_impl(
time: &[f32; GRANULE_LINES],
fifo: &mut [f32; 1024],
simd: bool,
) -> [f32; GRANULE_LINES] {
let d = &SYNTH_D;
let mut pcm = [0f32; GRANULE_LINES];
let mut head = 0usize;
for v in 0..SUBBAND_LINES {
let mut s = [0f32; SUBBANDS];
for (k, sv) in s.iter_mut().enumerate() {
*sv = time[k * SUBBAND_LINES + v];
}
head = (head + 1024 - 64) & 1023;
let vv = if simd {
unsafe { matrixing_simd(&s) }
} else {
matrixing_fast(&s)
};
fifo[head..head + 64].copy_from_slice(&vv);
let out: &mut [f32; 32] = pcm[v * 32..]
.first_chunk_mut()
.expect("granule is 18 blocks of 32");
if simd {
unsafe { window_simd(fifo, d, head, out) };
} else {
window_scalar(fifo, d, head, out);
}
}
fifo.rotate_left(head);
pcm
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn matrixing_simd_matches_scalar() {
if !have_simd() {
eprintln!("no SIMD twin on this host - scalar path only, gate skipped");
return;
}
let mut st = 0x3C6E_F372u32;
let mut rng = || {
st = st.wrapping_mul(1_664_525).wrapping_add(1_013_904_223);
(st >> 8) as f32 / (1u32 << 24) as f32 - 0.5
};
for trial in 0..64 {
let mut s = [0f32; SUBBANDS];
for v in &mut s {
*v = rng();
}
let a = matrixing_fast(&s);
let b = unsafe { matrixing_simd(&s) };
assert_eq!(a, b, "matrixing SIMD/scalar mismatch on trial {trial}");
}
}
#[test]
fn window_simd_matches_scalar() {
if !have_simd() {
eprintln!("no SIMD twin on this host - scalar path only, gate skipped");
return;
}
let mut st = 0x7F4A_7C15u32;
let mut rng = || {
st = st.wrapping_mul(1_664_525).wrapping_add(1_013_904_223);
(st >> 8) as f32 / (1u32 << 24) as f32 - 0.5
};
let mut fifo = [0f32; 1024];
for f in &mut fifo {
*f = rng();
}
let d = &SYNTH_D;
for head in (0..1024).step_by(64) {
let mut a = [0f32; 32];
let mut b = [0f32; 32];
window_scalar(&fifo, d, head, &mut a);
unsafe { window_simd(&fifo, d, head, &mut b) };
assert_eq!(a, b, "SIMD/scalar mismatch at head={head}");
}
}
#[test]
fn matrix_matches_cosine_formula() {
let n = matrix();
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);
}
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
}
#[test]
#[cfg_attr(
miri,
ignore = "end-to-end / exhaustive: too slow to interpret under Miri (> 5 min)"
)]
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 &mut s {
*sv = rng() * 100.0;
}
let dense = matrixing_dense(&s);
let fast = matrixing_fast(&s);
for i in 0..64 {
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() {
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"
);
}
}