3use crate::apu::PwmClockShift;
4use bincode::{Decode, Encode};
5use dsp::design::FilterType;
6use dsp::iir::{FirstOrderIirFilter, SecondOrderIirFilter};
7use dsp::sinc::{PerformanceSincResampler, QualitySincResampler};
8use gba_config::GbaAudioInterpolation;
9use jgenesis_common::audio::CubicResampler;
10use jgenesis_common::frontend::AudioOutput;
11use std::array;
12use std::cmp::Ordering;
13use std::collections::VecDeque;

15#[derive(Debug, Clone, Encode, Decode)]
16pub struct BasicResampler {
17    resampler: QualitySincResampler<2>,
18}
19
20impl BasicResampler {
21    pub fn new(clock_shift: PwmClockShift, output_frequency: u64) -> Self {
22        Self {
23            resampler: QualitySincResampler::new(
24                clock_shift.source_frequency() as f64,
25                output_frequency as f64,
26            ),
27        }
28    }
29
30    pub fn push_mixed_sample(&mut self, sample: [f64; 2]) {
31        self.resampler.collect(sample);
32    }
33
34    pub fn update_source_frequency(&mut self, clock_shift: PwmClockShift) {
35        self.resampler.update_source_frequency(clock_shift.source_frequency() as f64);
36    }
37
38    pub fn update_output_frequency(&mut self, output_frequency: u64) {
39        self.resampler.update_output_frequency(output_frequency as f64);
40    }
41
42    pub fn drain_audio_output<A: AudioOutput>(
43        &mut self,
44        audio_output: &mut A,
45    ) -> Result<(), A::Err> {
46        while let Some([sample_l, sample_r]) = self.resampler.output_buffer_pop_front() {
47            audio_output.push_sample(sample_l, sample_r)?;
48        }
49
50        Ok(())
51    }
52}
53
54#[derive(Debug, Clone, Encode, Decode)]
55enum EnhancedResampler {
56    Cubic(CubicResampler<1>),
57    Sinc(QualitySincResampler<1>),
58}
59
60impl EnhancedResampler {
61    fn new(
62        interpolation: GbaAudioInterpolation,
63        source_frequency: f64,
64        output_frequency: u64,
65    ) -> Self {
66        match interpolation {
67            GbaAudioInterpolation::WindowedSinc => {
68                Self::Sinc(QualitySincResampler::new(source_frequency, output_frequency as f64))
69            }
70            _ => Self::Cubic(CubicResampler::new(source_frequency, output_frequency)),
71        }
72    }
73
74    fn collect(&mut self, sample: f64) {
75        match self {
76            Self::Cubic(resampler) => resampler.collect_sample([sample]),
77            Self::Sinc(resampler) => resampler.collect([sample]),
78        }
79    }
80
81    fn update_source_frequency(&mut self, source_frequency: f64) {
82        match self {
83            Self::Cubic(resampler) => resampler.update_source_frequency(source_frequency),
84            Self::Sinc(resampler) => resampler.update_source_frequency(source_frequency),
85        }
86    }
87
88    fn update_output_frequency(&mut self, output_frequency: u64) {
89        match self {
90            Self::Cubic(resampler) => resampler.update_output_frequency(output_frequency),
91            Self::Sinc(resampler) => resampler.update_output_frequency(output_frequency as f64),
92        }
93    }
94
95    fn output_buffer_pop_front(&mut self) -> Option<[f64; 1]> {
96        match self {
97            Self::Cubic(resampler) => resampler.output_buffer_pop_front(),
98            Self::Sinc(resampler) => resampler.output_buffer_pop_front(),
99        }
100    }
101}

104const PSG_SOURCE_FREQUENCY: f64 = (1 << 21) as f64;

106#[derive(Debug, Clone, Encode, Decode)]
107struct PsgFilter {
108    max_pcm_frequency: Option<f64>,
109    source_frequency: u64,
110    psg_low_pass: bool,
111    low_pass: Option<[SecondOrderIirFilter; 2]>,
112    high_pass: [FirstOrderIirFilter; 2],
113}
114
115impl PsgFilter {
116    fn new(psg_low_pass: bool, pcm_frequencies: [Option<f64>; 2], source_frequency: u64) -> Self {
117        let high_pass = array::from_fn(|_| Self::new_high_pass());
118        let max_pcm_frequency = f64_option_max(pcm_frequencies);
119
120        let low_pass = if psg_low_pass {
121            max_pcm_frequency.and_then(|max_pcm_frequency| {
122                let lpf_cutoff = max_pcm_frequency * 0.5;
123
124                let source_frequency = source_frequency as f64;
125                let source_nyquist = source_frequency * 0.5;
126
127                // Cutoff must be less than source signal's Nyquist frequency or resulting filter will
128                // produce garbage (e.g. Golden Sun: The Lost Age)
129                (lpf_cutoff < source_nyquist)
130                    .then(|| array::from_fn(|_| Self::new_low_pass(lpf_cutoff, source_frequency)))
131            })
132        } else {
133            None
134        };
135
136        Self { max_pcm_frequency, source_frequency, psg_low_pass, low_pass, high_pass }
137    }
138
139    fn new_low_pass(cutoff_frequency: f64, source_frequency: f64) -> SecondOrderIirFilter {
140        dsp::design::butterworth(cutoff_frequency, source_frequency, FilterType::LowPass)
141    }
142
143    fn new_high_pass() -> FirstOrderIirFilter {
144        dsp::design::butterworth(5.0, PSG_SOURCE_FREQUENCY, FilterType::HighPass)
145    }
146
147    // Designed for output sample rate (e.g. 48000 Hz)
148    fn filter_low_pass(&mut self, sample: [f64; 2]) -> [f64; 2] {
149        self.low_pass
150            .as_mut()
151            .map(|filter| array::from_fn(|i| filter[i].filter(sample[i])))
152            .unwrap_or(sample)
153    }
154
155    // Designed for PSG source frequency (2 MHz)
156    fn filter_high_pass(&mut self, sample: [f64; 2]) -> [f64; 2] {
157        array::from_fn(|i| self.high_pass[i].filter(sample[i]))
158    }
159
160    fn reload(
161        &mut self,
162        psg_low_pass: bool,
163        pcm_frequencies: [Option<f64>; 2],
164        source_frequency: u64,
165    ) {
166        // For source frequencies, compare (freq-1000)/2000 instead of freq directly to avoid unnecessarily
167        // recreating the filter when dynamic resampling ratio changes the output sample rate
168        if psg_low_pass != self.psg_low_pass
169            || f64_option_max(pcm_frequencies) != self.max_pcm_frequency
170            || ((source_frequency - 1000) / 2000) != ((self.source_frequency - 1000) / 2000)
171        {
172            *self = Self::new(psg_low_pass, pcm_frequencies, source_frequency);
173        }
174    }
175}
176
177fn f64_option_max(values: impl IntoIterator<Item = Option<f64>>) -> Option<f64> {
178    // Less is arbitrary here - if there's ever a NaN frequency, there are bigger problems
179    values.into_iter().flatten().max_by(|&a, &b| a.partial_cmp(&b).unwrap_or(Ordering::Less))
180}
181
182#[derive(Debug, Clone, Encode, Decode)]
183pub struct InterpolatingResampler {
184    interpolation: GbaAudioInterpolation,
185    psg_low_pass: bool,
186    pcm_frequencies: [Option<f64>; 2],
187    pcm_resamplers: [Option<EnhancedResampler>; 2],
188    pcm_samples: [i8; 2],
189    psg_filter: PsgFilter,
190    psg_resampler: PerformanceSincResampler<2>,
191    psg_output: VecDeque<[f64; 2]>,
192    output_frequency: f64,
193}
194
195impl InterpolatingResampler {
196    pub fn new(
197        interpolation: GbaAudioInterpolation,
198        psg_low_pass: bool,
199        output_frequency: u64,
200        pcm_frequencies: [Option<f64>; 2],
201    ) -> Self {
202        Self {
203            interpolation,
204            psg_low_pass,
205            pcm_frequencies,
206            pcm_resamplers: pcm_frequencies.map(|frequency| {
207                frequency.map(|frequency| {
208                    EnhancedResampler::new(interpolation, frequency, output_frequency)
209                })
210            }),
211            pcm_samples: array::from_fn(|_| 0),
212            psg_filter: PsgFilter::new(psg_low_pass, pcm_frequencies, output_frequency),
213            psg_resampler: PerformanceSincResampler::new(
214                PSG_SOURCE_FREQUENCY,
215                output_frequency as f64,
216            ),
217            psg_output: VecDeque::with_capacity(48000 / 30),
218            output_frequency: output_frequency as f64,
219        }
220    }
221
222    pub fn update_psg_low_pass(&mut self, psg_low_pass: bool) {
223        self.psg_filter.reload(psg_low_pass, self.pcm_frequencies, self.output_frequency as u64);
224        self.psg_low_pass = psg_low_pass;
225    }
226
227    pub fn push_pcm_a(&mut self, sample: i8) {
228        self.push_pcm(0, sample);
229    }
230
231    pub fn push_pcm_b(&mut self, sample: i8) {
232        self.push_pcm(1, sample);
233    }
234
235    fn push_pcm(&mut self, i: usize, sample: i8) {
236        self.pcm_samples[i] = sample;
237
238        if let Some(resampler) = &mut self.pcm_resamplers[i] {
239            resampler.collect(f64::from(sample) / 128.0);
240        }
241    }
242
243    pub fn push_psg(&mut self, sample: (i16, i16)) {
244        let sample = [f64::from(sample.0) / 512.0, f64::from(sample.1) / 512.0];
245        let sample = self.psg_filter.filter_high_pass(sample);
246
247        self.psg_resampler.collect(sample);
248
249        while let Some(sample) = self.psg_resampler.output_buffer_pop_front() {
250            let sample = self.psg_filter.filter_low_pass(sample);
251            self.psg_output.push_back(sample);
252        }
253    }
254
255    pub fn update_pcm_a_frequency(&mut self, frequency: Option<f64>) {
256        self.update_pcm_frequency(0, frequency);
257    }
258
259    pub fn update_pcm_b_frequency(&mut self, frequency: Option<f64>) {
260        self.update_pcm_frequency(1, frequency);
261    }
262
263    fn update_pcm_frequency(&mut self, i: usize, frequency: Option<f64>) {
264        let prev_frequency = self.pcm_frequencies[i];
265        self.pcm_frequencies[i] = frequency;
266
267        if prev_frequency == frequency {
268            return;
269        }
270
271        match frequency {
272            Some(frequency) => {
273                self.pcm_resamplers[i]
274                    .get_or_insert_with(|| {
275                        EnhancedResampler::new(
276                            self.interpolation,
277                            frequency,
278                            self.output_frequency as u64,
279                        )
280                    })
281                    .update_source_frequency(frequency);
282            }
283            None => {
284                self.pcm_resamplers[i] = None;
285            }
286        }
287
288        self.psg_filter.reload(
289            self.psg_low_pass,
290            self.pcm_frequencies,
291            self.output_frequency as u64,
292        );
293    }
294
295    pub fn update_output_frequency(&mut self, output_frequency: u64) {
296        self.output_frequency = output_frequency as f64;
297
298        self.psg_resampler.update_output_frequency(self.output_frequency);
299        self.psg_filter.reload(self.psg_low_pass, self.pcm_frequencies, output_frequency);
300
301        for resampler in self.pcm_resamplers.iter_mut().flatten() {
302            resampler.update_output_frequency(output_frequency);
303        }
304    }
305
306    pub fn drain_audio_output<A: AudioOutput>(
307        &mut self,
308        audio_output: &mut A,
309        pcm_volume_shifts: [bool; 2],
310        psg_volume_shift: u8,
311        pcm_a_enabled: [bool; 2],
312        pcm_b_enabled: [bool; 2],
313    ) -> Result<(), A::Err> {
314        while let Some([mut psg_l, mut psg_r]) = self.psg_output.pop_front() {
315            if psg_volume_shift != 0 {
316                psg_l /= f64::from(1 << psg_volume_shift);
317                psg_r /= f64::from(1 << psg_volume_shift);
318            }
319
320            let mut pcm_a = self.pcm_resamplers[0]
321                .as_mut()
322                .and_then(EnhancedResampler::output_buffer_pop_front)
323                .unwrap_or_else(|| [f64::from(self.pcm_samples[0]) / 128.0])[0];
324            let mut pcm_b = self.pcm_resamplers[1]
325                .as_mut()
326                .and_then(EnhancedResampler::output_buffer_pop_front)
327                .unwrap_or_else(|| [f64::from(self.pcm_samples[1]) / 128.0])[0];
328
329            if pcm_volume_shifts[0] {
330                pcm_a *= 0.5;
331            }
332            if pcm_volume_shifts[1] {
333                pcm_b *= 0.5;
334            }
335
336            let pcm_l = f64::from(pcm_a_enabled[0]) * pcm_a + f64::from(pcm_b_enabled[0]) * pcm_b;
337            let pcm_r = f64::from(pcm_a_enabled[1]) * pcm_a + f64::from(pcm_b_enabled[1]) * pcm_b;
338
339            let mixed_l = (psg_l + pcm_l).clamp(-1.0, 1.0);
340            let mixed_r = (psg_r + pcm_r).clamp(-1.0, 1.0);
341
342            audio_output.push_sample(mixed_l, mixed_r)?;
343        }
344
345        Ok(())
346    }
347}