1use crate::apu::dsp::tables;
2
3const I15_MIN: f64 = -16384.0;
4const I15_MAX: f64 = 16383.0;
5
6pub struct InterpolateArgs {
7    pub pitch_counter: u16,
8    pub oldest: i16,
9    pub older: i16,
10    pub old: i16,
11    pub sample: i16,
12}
13
14pub fn gaussian(
15    InterpolateArgs { pitch_counter, oldest, older, old, sample }: InterpolateArgs,
16) -> i16 {
17    // Do math in 32 bits to avoid overflows
18    let sample: i32 = sample.into();
19    let old: i32 = old.into();
20    let older: i32 = older.into();
21    let oldest: i32 = oldest.into();
22
23    // Bits 4-11 of pitch counter are the interpolation index into the Gaussian interpolation table
24    let interpolation_idx = ((pitch_counter >> 4) & 0xFF) as usize;
25
26    // Sum the 3 older samples with 15-bit wrapping
27    let mut sum = (tables::GAUSSIAN[0x0FF - interpolation_idx] * oldest) >> 11;
28    sum += (tables::GAUSSIAN[0x1FF - interpolation_idx] * older) >> 11;
29    sum += (tables::GAUSSIAN[0x100 + interpolation_idx] * old) >> 11;
30
31    // Clip to 15 bits
32    sum = (((sum as i16) << 1) >> 1).into();
33
34    // Add in the current sample
35    sum += (tables::GAUSSIAN[interpolation_idx] * sample) >> 11;
36
37    // Clamp the final result to signed 15-bit
38    sum.clamp((i16::MIN >> 1).into(), (i16::MAX >> 1).into()) as i16
39}
40
41pub fn hermite(
42    InterpolateArgs { pitch_counter, oldest, older, old, sample }: InterpolateArgs,
43) -> i16 {
44    let samples = [oldest.into(), older.into(), old.into(), sample.into()];
45    let x = f64::from(pitch_counter & 0xFFF) / 4096.0;
46
47    let result = jgenesis_common::audio::interpolate_cubic_hermite_4p(samples, x);
48    result.round().clamp(I15_MIN, I15_MAX) as i16
49}