1#![allow(clippy::excessive_precision)]
2
3use bincode::{Decode, Encode};
4use dsp::design::FilterType;
5use dsp::iir::FirstOrderIirFilter;
6use dsp::sinc::QualitySincResampler;
7use genesis_config::{GenesisEmulatorConfig, LowPassSettings, S32XPwmResampling};
8use jgenesis_common::audio::CubicResampler;
9use std::collections::VecDeque;
10
11#[derive(Debug, Clone, Encode, Decode)]
12struct PwmAudioFilter {
13    gen_low_pass_setting: LowPassSettings,
14    apply_gen_lpf_to_pwm: bool,
15    pwm_frequency: f64,
16    pwm_lpf_l: FirstOrderIirFilter,
17    pwm_lpf_r: FirstOrderIirFilter,
18}
19
20fn new_pwm_low_pass(pwm_frequency: f64, cutoff_frequency: f64) -> FirstOrderIirFilter {
21    let pwm_nyquist = 0.5 * pwm_frequency;
22    if cutoff_frequency >= pwm_nyquist {
23        // A real low-pass filter will produce garbage; return a fake filter
24        return FirstOrderIirFilter::identity();
25    }
26
27    dsp::design::butterworth(cutoff_frequency, pwm_frequency, FilterType::LowPass)
28}
29
30impl PwmAudioFilter {
31    fn new(config: &GenesisEmulatorConfig, pwm_frequency: f64) -> Self {
32        let genesis_lpf_cutoff: f64 = config.genesis_lpf_cutoff.into();
33        Self {
34            gen_low_pass_setting: LowPassSettings::from_config(config),
35            apply_gen_lpf_to_pwm: config.sega_32x.apply_genesis_lpf_to_pwm,
36            pwm_frequency,
37            pwm_lpf_l: new_pwm_low_pass(pwm_frequency, genesis_lpf_cutoff),
38            pwm_lpf_r: new_pwm_low_pass(pwm_frequency, genesis_lpf_cutoff),
39        }
40    }
41
42    fn filter(&mut self, (sample_l, sample_r): (f64, f64)) -> (f64, f64) {
43        if !self.gen_low_pass_setting.genesis_enabled || !self.apply_gen_lpf_to_pwm {
44            return (sample_l, sample_r);
45        }
46
47        (self.pwm_lpf_l.filter(sample_l), self.pwm_lpf_r.filter(sample_r))
48    }
49
50    fn update_pwm_frequency(&mut self, pwm_frequency: f64) {
51        // Exact float comparison is fine here because PWM frequency is deterministically derived
52        // from the PWM cycle register value
53        #[allow(clippy::float_cmp)]
54        if pwm_frequency == self.pwm_frequency {
55            return;
56        }
57        self.pwm_frequency = pwm_frequency;
58
59        let genesis_lpf_cutoff: f64 = self.gen_low_pass_setting.genesis_cutoff.into();
60        self.pwm_lpf_l = new_pwm_low_pass(self.pwm_frequency, genesis_lpf_cutoff);
61        self.pwm_lpf_r = new_pwm_low_pass(self.pwm_frequency, genesis_lpf_cutoff);
62    }
63
64    fn reload_config(&mut self, config: &GenesisEmulatorConfig) {
65        if self.gen_low_pass_setting == LowPassSettings::from_config(config)
66            && self.apply_gen_lpf_to_pwm == config.sega_32x.apply_genesis_lpf_to_pwm
67        {
68            return;
69        }
70
71        *self = Self::new(config, self.pwm_frequency);
72    }
73}
74
75#[derive(Debug, Clone, Encode, Decode)]
76enum PwmResamplerImpl {
77    CubicHermite(CubicResampler<2>),
78    WindowedSinc(QualitySincResampler<2>),
79}
80
81impl PwmResamplerImpl {
82    fn new(resampling: S32XPwmResampling, pwm_frequency: f64, output_frequency: u64) -> Self {
83        match resampling {
84            S32XPwmResampling::CubicHermite => {
85                Self::CubicHermite(CubicResampler::new(pwm_frequency, output_frequency))
86            }
87            S32XPwmResampling::WindowedSinc => Self::WindowedSinc(QualitySincResampler::new(
88                pwm_frequency,
89                output_frequency as f64,
90            )),
91        }
92    }
93
94    fn resampling(&self) -> S32XPwmResampling {
95        match self {
96            Self::CubicHermite(_) => S32XPwmResampling::CubicHermite,
97            Self::WindowedSinc(_) => S32XPwmResampling::WindowedSinc,
98        }
99    }
100
101    fn collect_sample(&mut self, sample: [f64; 2]) {
102        match self {
103            Self::CubicHermite(resampler) => resampler.collect_sample(sample),
104            Self::WindowedSinc(resampler) => resampler.collect(sample),
105        }
106    }
107
108    fn output_buffer_pop_front(&mut self) -> Option<[f64; 2]> {
109        match self {
110            Self::CubicHermite(resampler) => resampler.output_buffer_pop_front(),
111            Self::WindowedSinc(resampler) => resampler.output_buffer_pop_front(),
112        }
113    }
114
115    fn update_source_frequency(&mut self, source_frequency: f64) {
116        match self {
117            Self::CubicHermite(resampler) => resampler.update_source_frequency(source_frequency),
118            Self::WindowedSinc(resampler) => resampler.update_source_frequency(source_frequency),
119        }
120    }
121
122    fn update_output_frequency(&mut self, output_frequency: u64) {
123        match self {
124            Self::CubicHermite(resampler) => resampler.update_output_frequency(output_frequency),
125            Self::WindowedSinc(resampler) => {
126                resampler.update_output_frequency(output_frequency as f64);
127            }
128        }
129    }
130}
131
132#[derive(Debug, Clone, Encode, Decode)]
133pub struct PwmResampler {
134    filter: PwmAudioFilter,
135    resampler: PwmResamplerImpl,
136    output: VecDeque<[f64; 2]>,
137    output_frequency: u64,
138}
139
140impl PwmResampler {
141    #[must_use]
142    pub fn new(config: &GenesisEmulatorConfig, output_frequency: u64) -> Self {
143        const INITIAL_PWM_FREQUENCY: f64 = 22000.0;
144
145        Self {
146            filter: PwmAudioFilter::new(config, INITIAL_PWM_FREQUENCY),
147            resampler: PwmResamplerImpl::new(
148                config.sega_32x.pwm_resampling,
149                INITIAL_PWM_FREQUENCY,
150                output_frequency,
151            ),
152            output: VecDeque::with_capacity(48000 / 30),
153            output_frequency,
154        }
155    }
156
157    pub fn collect_sample(&mut self, sample_l: f64, sample_r: f64) {
158        let (sample_l, sample_r) = self.filter.filter((sample_l, sample_r));
159
160        self.resampler.collect_sample([sample_l, sample_r]);
161        while let Some(output) = self.resampler.output_buffer_pop_front() {
162            self.output.push_back(output);
163        }
164    }
165
166    #[must_use]
167    pub fn output_buffer_len(&self) -> usize {
168        self.output.len()
169    }
170
171    pub fn output_buffer_pop_front(&mut self) -> Option<[f64; 2]> {
172        self.output.pop_front()
173    }
174
175    pub fn update_source_frequency(&mut self, source_frequency: f64) {
176        self.filter.update_pwm_frequency(source_frequency);
177        self.resampler.update_source_frequency(source_frequency);
178    }
179
180    pub fn reload_config(&mut self, config: &GenesisEmulatorConfig) {
181        self.filter.reload_config(config);
182
183        if config.sega_32x.pwm_resampling != self.resampler.resampling() {
184            self.resampler = PwmResamplerImpl::new(
185                config.sega_32x.pwm_resampling,
186                self.filter.pwm_frequency,
187                self.output_frequency,
188            );
189        }
190    }
191
192    pub fn update_output_frequency(&mut self, output_frequency: u64) {
193        self.resampler.update_output_frequency(output_frequency);
194        self.output_frequency = output_frequency;
195    }
196}