use super::Sample;
use crate::fade::{self, RECOVER_MS};
const MIN_PITCH_HZ: u32 = 66;
const MAX_PITCH_HZ: u32 = 200;
const CORR_MS: u32 = 20;
const MAX_RATE: u32 = 16000;
const MAX_PERIOD: usize = (MAX_RATE / MIN_PITCH_HZ) as usize + 1;
const HISTORY: usize = (MAX_RATE * CORR_MS / 1000) as usize + MAX_PERIOD;
pub(crate) const LEAD: usize = 24;
pub(crate) struct PitchConcealer {
rate: u32,
history: [Sample; HISTORY],
filled: usize,
period: [Sample; MAX_PERIOD],
period_len: usize,
pos: usize,
concealed: usize,
lead: [Sample; LEAD],
tail: [Sample; LEAD],
}
impl PitchConcealer {
pub(crate) fn new(rate: u32) -> Self {
debug_assert!(rate <= MAX_RATE);
Self {
rate,
history: [0; HISTORY],
filled: 0,
period: [0; MAX_PERIOD],
period_len: 0,
pos: 0,
concealed: 0,
lead: [0; LEAD],
tail: [0; LEAD],
}
}
fn ms(&self, ms: u32) -> usize {
(self.rate * ms / 1000) as usize
}
fn needed(&self) -> usize {
self.ms(CORR_MS) + self.ms(1000 / MIN_PITCH_HZ)
}
fn remember(&mut self, samples: &[Sample]) {
let n = samples.len();
if n >= HISTORY {
self.history.copy_from_slice(&samples[n - HISTORY..]);
} else {
self.history.copy_within(n.., 0);
self.history[HISTORY - n..].copy_from_slice(samples);
}
self.filled = (self.filled + n).min(HISTORY);
}
#[cfg(test)]
pub(crate) fn recent(&self, n: usize) -> &[Sample] {
&self.history[HISTORY - n.min(HISTORY)..]
}
pub(crate) fn on_decoded(&mut self, samples: &mut [Sample]) {
if samples.is_empty() {
return;
}
if self.concealed > 0 && self.period_len > 0 {
let len = self.ms(RECOVER_MS).min(samples.len());
let gain = self.gain(self.concealed);
let total = (len + 1) as i32;
for (i, s) in samples[..len].iter_mut().enumerate() {
let t = self.next_period_sample() as i32 * gain / 100;
let w = (i + 1) as i32;
*s = ((*s as i32 * w + t * (total - w)) / total) as Sample;
}
}
self.concealed = 0;
self.remember(samples);
}
pub(crate) fn extrapolate(&mut self, out: &mut [Sample]) -> bool {
if self.filled < self.needed() {
out.fill(0);
return false;
}
if self.concealed == 0 || self.period_len == 0 {
self.pick_period();
self.lead.copy_from_slice(&self.history[HISTORY - LEAD..]);
} else {
self.lead = self.tail;
}
for s in out.iter_mut() {
*s = self.next_period_sample();
}
let n = out.len().min(LEAD);
self.tail.copy_within(n.., 0);
self.tail[LEAD - n..].copy_from_slice(&out[out.len() - n..]);
true
}
pub(crate) fn lead(&self) -> &[Sample; LEAD] {
&self.lead
}
pub(crate) fn audible(&self) -> bool {
self.gain(self.concealed) > 0
}
pub(crate) fn fade(&mut self, out: &mut [Sample]) {
for (i, s) in out.iter_mut().enumerate() {
*s = (*s as i32 * self.gain(self.concealed + i) / 100) as Sample;
}
self.concealed += out.len();
self.remember(out);
}
pub(crate) fn conceal(&mut self, out: &mut [Sample]) {
self.extrapolate(out);
self.fade(out);
}
fn gain(&self, t: usize) -> i32 {
fade::gain(t, self.rate)
}
pub(crate) fn peek(&self, k: usize) -> Sample {
if self.period_len == 0 {
return 0;
}
self.period[(self.pos + k) % self.period_len]
}
fn next_period_sample(&mut self) -> Sample {
let s = self.period[self.pos];
self.pos = (self.pos + 1) % self.period_len;
s
}
fn pick_period(&mut self) {
let min_lag = self.ms(1000 / MAX_PITCH_HZ).max(1);
let max_lag = self.ms(1000 / MIN_PITCH_HZ);
let win = self.ms(CORR_MS);
let h = &self.history;
let target = &h[HISTORY - win..];
let mut best = (min_lag, f32::MIN);
for lag in min_lag..=max_lag {
let cand = &h[HISTORY - win - lag..HISTORY - lag];
let (mut xy, mut yy) = (0f32, 0f32);
for (x, y) in target.iter().zip(cand) {
xy += *x as f32 * *y as f32;
yy += *y as f32 * *y as f32;
}
let score = if yy > 0.0 { xy * xy.abs() / yy } else { 0.0 };
if score > best.1 {
best = (lag, score);
}
}
let p = best.0;
self.period_len = p;
self.period[..p].copy_from_slice(&h[HISTORY - p..]);
let ola = p / 4;
let before = &h[HISTORY - p - ola..HISTORY - p];
let total = (ola + 1) as i32;
let tail = &mut self.period[p - ola..p];
for (i, (t, b)) in tail.iter_mut().zip(before).enumerate() {
let w = (i + 1) as i32;
*t = ((*t as i32 * (total - w) + *b as i32 * w) / total) as Sample;
}
self.pos = 0;
}
}
#[cfg(test)]
mod tests {
use super::*;
const RATE: u32 = 16000;
const FRAME: usize = 320;
fn tone(frame: usize, out: &mut [Sample]) {
for (i, s) in out.iter_mut().enumerate() {
let t = (frame * FRAME + i) as f32 / RATE as f32;
*s = (8000.0 * libm_sin(2.0 * core::f32::consts::PI * 150.0 * t)) as Sample;
}
}
fn libm_sin(x: f32) -> f32 {
let mut x = x % (2.0 * core::f32::consts::PI);
if x > core::f32::consts::PI {
x -= 2.0 * core::f32::consts::PI;
}
let x2 = x * x;
x * (1.0
- x2 / 6.0
* (1.0 - x2 / 20.0 * (1.0 - x2 / 42.0 * (1.0 - x2 / 72.0 * (1.0 - x2 / 110.0)))))
}
fn rms(s: &[Sample]) -> f32 {
let sum: f32 = s.iter().map(|x| *x as f32 * *x as f32).sum();
let mean = sum / s.len() as f32;
let mut r = mean.max(1.0);
for _ in 0..30 {
r = 0.5 * (r + mean / r);
}
r
}
fn max_step(prev: Sample, s: &[Sample]) -> i32 {
let mut last = prev as i32;
let mut max = 0;
for x in s {
max = max.max((*x as i32 - last).abs());
last = *x as i32;
}
max
}
fn primed() -> PitchConcealer {
let mut plc = PitchConcealer::new(RATE);
let mut buf = [0; FRAME];
for f in 0..5 {
tone(f, &mut buf);
plc.on_decoded(&mut buf);
}
plc
}
#[test]
fn silence_before_there_is_audio_to_conceal_from() {
let mut plc = PitchConcealer::new(RATE);
let mut out = [1; FRAME];
assert!(!plc.extrapolate(&mut out));
assert!(out.iter().all(|s| *s == 0));
}
#[test]
fn concealment_continues_the_waveform() {
let mut plc = primed();
let last = *plc.recent(1).last().unwrap();
let mut out = [0; FRAME];
plc.conceal(&mut out);
let mut reference = [0; FRAME];
tone(5, &mut reference);
assert!(rms(&out) > 0.5 * rms(&reference), "rms {}", rms(&out));
assert!(max_step(last, &out) < 1000, "step {}", max_step(last, &out));
}
#[test]
fn concealment_continues_from_any_phase() {
for offset in (0..FRAME).step_by(13) {
let mut plc = PitchConcealer::new(RATE);
let mut buf = [0; FRAME];
for f in 0..5 {
tone(f, &mut buf);
let shifted: [Sample; FRAME] = core::array::from_fn(|i| {
let mut one = [0; FRAME];
tone(f + (i + offset) / FRAME, &mut one);
one[(i + offset) % FRAME]
});
buf = shifted;
plc.on_decoded(&mut buf);
}
let last = *plc.recent(1).last().unwrap();
let mut out = [0; FRAME];
plc.extrapolate(&mut out);
let step = max_step(last, &out);
assert!(step < 1000, "offset {offset}: step {step}");
}
}
#[test]
fn long_loss_fades_to_silence() {
let mut plc = primed();
let mut frames = [[0; FRAME]; 4];
for f in frames.iter_mut() {
plc.conceal(f);
}
assert!(rms(&frames[1]) < rms(&frames[0]));
assert!(frames[3].iter().all(|s| *s == 0));
}
#[test]
fn recovery_is_crossfaded() {
let mut plc = primed();
let mut concealed = [0; FRAME];
plc.conceal(&mut concealed);
let mut real = [0; FRAME];
tone(6, &mut real);
for s in real.iter_mut() {
*s = -*s;
}
plc.on_decoded(&mut real);
let last = *concealed.last().unwrap();
assert!(
max_step(last, &real[..64]) < 2000,
"step {}",
max_step(last, &real[..64])
);
}
#[test]
fn works_at_8khz() {
let mut plc = PitchConcealer::new(8000);
let mut buf = [0; 160];
for f in 0..5 {
for (i, s) in buf.iter_mut().enumerate() {
let t = (f * 160 + i) as f32 / 8000.0;
*s = (8000.0 * libm_sin(2.0 * core::f32::consts::PI * 150.0 * t)) as Sample;
}
plc.on_decoded(&mut buf);
}
let mut out = [0; 160];
assert!(plc.extrapolate(&mut out));
assert!(rms(&out) > 2000.0);
}
}