use crate::silk_lsf_recon::NlsfReconstructed;
use crate::silk_lsf_stage2::{D_LPC_MAX, D_LPC_NB_MB, D_LPC_WB};
use crate::toc::Bandwidth;
use crate::Error;
const NLSF_UPPER_Q15: i32 = 32768;
#[rustfmt::skip]
const NDELTA_MIN_Q15_NB_MB: [i32; D_LPC_NB_MB + 1] = [
250, 3, 6, 3, 3, 3, 4, 3, 3, 3, 461,
];
#[rustfmt::skip]
const NDELTA_MIN_Q15_WB: [i32; D_LPC_WB + 1] = [
100, 3, 40, 3, 3, 3, 5, 14, 14, 10, 11, 3, 8, 9, 7, 3, 347,
];
fn ndelta_min_q15(bandwidth: Bandwidth) -> Result<&'static [i32], Error> {
match bandwidth {
Bandwidth::Nb | Bandwidth::Mb => Ok(&NDELTA_MIN_Q15_NB_MB[..]),
Bandwidth::Wb => Ok(&NDELTA_MIN_Q15_WB[..]),
_ => Err(Error::MalformedPacket),
}
}
fn add_sat16(a: i32, b: i32) -> i32 {
(a + b).clamp(i16::MIN as i32, i16::MAX as i32)
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct NlsfStabilized {
len: u8,
nlsf_q15: [i16; D_LPC_MAX],
}
impl NlsfStabilized {
pub fn from_reconstructed(
bandwidth: Bandwidth,
recon: &NlsfReconstructed,
) -> Result<Self, Error> {
let ndelta = ndelta_min_q15(bandwidth)?;
let d_lpc = recon.len();
if d_lpc + 1 != ndelta.len() {
return Err(Error::MalformedPacket);
}
let mut nlsf = [0i32; D_LPC_MAX];
for (dst, &src) in nlsf.iter_mut().zip(recon.nlsf_q15()) {
*dst = src as i32;
}
stabilize_in_place(&mut nlsf[..d_lpc], ndelta);
let mut nlsf_q15 = [0i16; D_LPC_MAX];
for (dst, &src) in nlsf_q15.iter_mut().zip(nlsf.iter()) {
*dst = src as i16;
}
Ok(Self {
len: d_lpc as u8,
nlsf_q15,
})
}
pub fn len(&self) -> usize {
self.len as usize
}
pub fn is_empty(&self) -> bool {
self.len == 0
}
pub fn nlsf_q15(&self) -> &[i16] {
&self.nlsf_q15[..self.len()]
}
}
fn stabilize_in_place(nlsf: &mut [i32], ndelta: &[i32]) {
let d_lpc = nlsf.len();
debug_assert_eq!(ndelta.len(), d_lpc + 1);
for _ in 0..20 {
let mut min_i = 0usize;
let mut min_spacing = i32::MAX;
for i in 0..=d_lpc {
let upper = if i == d_lpc { NLSF_UPPER_Q15 } else { nlsf[i] };
let lower = if i == 0 { 0 } else { nlsf[i - 1] };
let spacing = upper - lower - ndelta[i];
if spacing < min_spacing {
min_spacing = spacing;
min_i = i;
}
}
if min_spacing >= 0 {
return;
}
let i = min_i;
if i == 0 {
nlsf[0] = ndelta[0];
} else if i == d_lpc {
nlsf[d_lpc - 1] = NLSF_UPPER_Q15 - ndelta[d_lpc];
} else {
let half = ndelta[i] >> 1;
let mut min_center = half;
for &nd in ndelta.iter().take(i) {
min_center += nd;
}
let mut max_center = NLSF_UPPER_Q15 - half;
for &nd in ndelta.iter().take(d_lpc + 1).skip(i + 1) {
max_center -= nd;
}
let midpoint = (nlsf[i - 1] + nlsf[i] + 1) >> 1;
let center_freq = min_center.max(midpoint.min(max_center));
nlsf[i - 1] = center_freq - half;
nlsf[i] = nlsf[i - 1] + ndelta[i];
}
}
nlsf.sort_unstable();
for k in 0..d_lpc {
let prev = if k == 0 { 0 } else { nlsf[k - 1] };
nlsf[k] = nlsf[k].max(add_sat16(prev, ndelta[k]));
}
for k in (0..d_lpc).rev() {
let next = if k + 1 == d_lpc {
NLSF_UPPER_Q15
} else {
nlsf[k + 1]
};
nlsf[k] = nlsf[k].min(next - ndelta[k + 1]);
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::range_decoder::RangeDecoder;
use crate::silk_lsf_recon::NlsfReconstructed;
use crate::silk_lsf_stage2::LsfStage2;
#[test]
fn table25_lengths() {
assert_eq!(NDELTA_MIN_Q15_NB_MB.len(), D_LPC_NB_MB + 1);
assert_eq!(NDELTA_MIN_Q15_WB.len(), D_LPC_WB + 1);
}
#[test]
fn table25_spot_checks() {
assert_eq!(NDELTA_MIN_Q15_NB_MB[0], 250);
assert_eq!(NDELTA_MIN_Q15_NB_MB[10], 461);
assert_eq!(NDELTA_MIN_Q15_WB[0], 100);
assert_eq!(NDELTA_MIN_Q15_WB[2], 40);
assert_eq!(NDELTA_MIN_Q15_WB[6], 5);
assert_eq!(NDELTA_MIN_Q15_WB[16], 347);
}
#[test]
fn ndelta_lookup_rejects_swb_fb() {
assert!(ndelta_min_q15(Bandwidth::Swb).is_err());
assert!(ndelta_min_q15(Bandwidth::Fb).is_err());
assert!(ndelta_min_q15(Bandwidth::Nb).is_ok());
assert!(ndelta_min_q15(Bandwidth::Mb).is_ok());
assert!(ndelta_min_q15(Bandwidth::Wb).is_ok());
}
#[test]
fn add_sat16_saturates() {
assert_eq!(add_sat16(100, 200), 300);
assert_eq!(add_sat16(32000, 1000), 32767);
assert_eq!(add_sat16(-32000, -2000), -32768);
assert_eq!(add_sat16(32767, 0), 32767);
}
fn assert_spacing_ok(nlsf: &[i32], ndelta: &[i32]) {
let d_lpc = nlsf.len();
for i in 0..=d_lpc {
let upper = if i == d_lpc { NLSF_UPPER_Q15 } else { nlsf[i] };
let lower = if i == 0 { 0 } else { nlsf[i - 1] };
assert!(
upper - lower >= ndelta[i],
"spacing constraint violated at i={i}: upper={upper} lower={lower} \
needs >= {} got {}",
ndelta[i],
upper - lower
);
}
}
#[test]
fn already_stable_is_unchanged_nb() {
let mut nlsf = [
2000, 5000, 8000, 11000, 14000, 17000, 20000, 23000, 26000, 29000,
];
let original = nlsf;
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_NB_MB);
assert_eq!(nlsf, original, "stable vector must be left untouched");
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_NB_MB);
}
#[test]
fn already_stable_is_unchanged_wb() {
let mut nlsf = [
1000, 3000, 5000, 7000, 9000, 11000, 13000, 15000, 17000, 19000, 21000, 23000, 25000,
27000, 29000, 31000,
];
let original = nlsf;
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_WB);
assert_eq!(nlsf, original);
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_WB);
}
#[test]
fn first_coeff_too_low_is_pushed_up() {
let mut nlsf = [
10, 5000, 8000, 11000, 14000, 17000, 20000, 23000, 26000, 29000,
];
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_NB_MB);
assert!(nlsf[0] >= 250);
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_NB_MB);
}
#[test]
fn last_coeff_too_high_is_pulled_down() {
let mut nlsf = [
2000, 5000, 8000, 11000, 14000, 17000, 20000, 23000, 26000, 32760,
];
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_NB_MB);
assert!(nlsf[9] <= NLSF_UPPER_Q15 - 461);
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_NB_MB);
}
#[test]
fn interior_pair_recentred() {
let mut nlsf = [
2000, 5000, 8000, 11000, 11000, 17000, 20000, 23000, 26000, 29000,
];
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_NB_MB);
assert!(nlsf[4] - nlsf[3] >= 3);
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_NB_MB);
}
#[test]
fn interior_recentre_exact_values() {
let mut nlsf = [
2000, 5000, 8000, 11000, 15000, 15001, 20000, 23000, 26000, 29000,
];
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_NB_MB);
assert_eq!(nlsf[4], 15000);
assert_eq!(nlsf[5], 15003);
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_NB_MB);
}
#[test]
fn fallback_fixes_reversed_input() {
let mut nlsf = [
29000, 26000, 23000, 20000, 17000, 14000, 11000, 8000, 5000, 2000,
];
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_NB_MB);
for w in nlsf.windows(2) {
assert!(w[0] <= w[1], "fallback output not ascending: {nlsf:?}");
}
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_NB_MB);
}
#[test]
fn all_zero_input_is_spread() {
let mut nlsf = [0i32; D_LPC_NB_MB];
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_NB_MB);
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_NB_MB);
for &v in nlsf.iter() {
assert!((0..=NLSF_UPPER_Q15).contains(&v));
}
}
#[test]
fn all_max_input_is_spread() {
let mut nlsf = [32767i32; D_LPC_WB];
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_WB);
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_WB);
for w in nlsf.windows(2) {
assert!(w[0] <= w[1]);
}
}
#[test]
fn fallback_does_not_wrap_near_i16_max() {
let mut nlsf = [
32767, 32767, 32767, 32767, 32767, 32767, 32767, 32767, 32767, 32767,
];
stabilize_in_place(&mut nlsf, &NDELTA_MIN_Q15_NB_MB);
assert_spacing_ok(&nlsf, &NDELTA_MIN_Q15_NB_MB);
for &v in nlsf.iter() {
assert!((0..=i16::MAX as i32).contains(&v), "wrapped: {nlsf:?}");
}
}
fn recon_then_stabilize(bandwidth: Bandwidth, i1: u8, buf: &[u8]) -> NlsfStabilized {
let mut rd = RangeDecoder::new(buf);
let stage2 = LsfStage2::decode(&mut rd, bandwidth, i1).expect("stage-2 decode");
let recon =
NlsfReconstructed::from_stage1_and_stage2(bandwidth, i1, &stage2).expect("recon");
NlsfStabilized::from_reconstructed(bandwidth, &recon).expect("stabilize")
}
#[test]
fn end_to_end_sweep_all_i1() {
let buf = [0x5A, 0xC3, 0x17, 0x9E, 0x42, 0xFB, 0x08, 0x71, 0x2D, 0xB6];
for bandwidth in [Bandwidth::Nb, Bandwidth::Mb, Bandwidth::Wb] {
let ndelta = ndelta_min_q15(bandwidth).unwrap();
for i1 in 0u8..32 {
let stab = recon_then_stabilize(bandwidth, i1, &buf);
let nlsf: Vec<i32> = stab.nlsf_q15().iter().map(|&v| v as i32).collect();
assert_spacing_ok(&nlsf, ndelta);
for &v in &nlsf {
assert!((0..=i16::MAX as i32).contains(&v));
}
for w in nlsf.windows(2) {
assert!(w[0] < w[1], "not strictly increasing: {nlsf:?}");
}
}
}
}
#[test]
fn end_to_end_length_matches_bandwidth() {
let buf = [0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA];
let nb = recon_then_stabilize(Bandwidth::Nb, 0, &buf);
assert_eq!(nb.len(), D_LPC_NB_MB);
let wb = recon_then_stabilize(Bandwidth::Wb, 0, &buf);
assert_eq!(wb.len(), D_LPC_WB);
}
#[test]
fn from_reconstructed_rejects_swb_fb() {
let buf = [0x5A, 0xC3, 0x17, 0x9E, 0x42, 0xFB, 0x08, 0x71, 0x2D, 0xB6];
let mut rd = RangeDecoder::new(&buf);
let stage2 = LsfStage2::decode(&mut rd, Bandwidth::Nb, 0).unwrap();
let recon = NlsfReconstructed::from_stage1_and_stage2(Bandwidth::Nb, 0, &stage2).unwrap();
assert!(NlsfStabilized::from_reconstructed(Bandwidth::Swb, &recon).is_err());
assert!(NlsfStabilized::from_reconstructed(Bandwidth::Fb, &recon).is_err());
}
#[test]
fn from_reconstructed_rejects_length_mismatch() {
let buf = [
0x5A, 0xC3, 0x17, 0x9E, 0x42, 0xFB, 0x08, 0x71, 0x2D, 0xB6, 0x4C, 0x8E,
];
let mut rd = RangeDecoder::new(&buf);
let stage2 = LsfStage2::decode(&mut rd, Bandwidth::Wb, 0).unwrap();
let recon = NlsfReconstructed::from_stage1_and_stage2(Bandwidth::Wb, 0, &stage2).unwrap();
assert!(NlsfStabilized::from_reconstructed(Bandwidth::Nb, &recon).is_err());
}
#[test]
fn typical_recon_unchanged_by_stabilization() {
let buf = [0x5A, 0xC3, 0x17, 0x9E, 0x42, 0xFB, 0x08, 0x71, 0x2D, 0xB6];
let mut rd = RangeDecoder::new(&buf);
let stage2 = LsfStage2::decode(&mut rd, Bandwidth::Nb, 0).unwrap();
let recon = NlsfReconstructed::from_stage1_and_stage2(Bandwidth::Nb, 0, &stage2).unwrap();
let stab = NlsfStabilized::from_reconstructed(Bandwidth::Nb, &recon).unwrap();
assert_eq!(stab.nlsf_q15(), recon.nlsf_q15());
}
}