1use crate::audio::{RESAMPLE_SCALING_FACTOR, interpolate_cubic_hermite_6p};
2use bincode::{Decode, Encode};
3use std::array;
4use std::collections::VecDeque;
5
6const BUFFER_LEN: usize = 6;
7
8#[derive(Debug, Clone, Encode, Decode)]
9pub struct CubicResampler<const CHANNELS: usize> {
10    scaled_source_frequency: u64,
11    output_frequency: u64,
12    cycle_counter_product: u64,
13    scaled_x_counter: u64,
14    input_samples: VecDeque<[f64; CHANNELS]>,
15    output_samples: VecDeque<[f64; CHANNELS]>,
16}
17
18impl<const CHANNELS: usize> CubicResampler<CHANNELS> {
19    #[must_use]
20    pub fn new(source_frequency: f64, output_frequency: u64) -> Self {
21        let scaled_source_frequency = scale_source_frequency(source_frequency);
22
23        let mut resampler = Self {
24            scaled_source_frequency,
25            output_frequency,
26            cycle_counter_product: 0,
27            scaled_x_counter: 0,
28            input_samples: VecDeque::with_capacity(2 * BUFFER_LEN),
29            output_samples: VecDeque::with_capacity(48000 / 60 * 2),
30        };
31
32        resampler.input_samples.extend([[0.0; CHANNELS]; BUFFER_LEN]);
33
34        resampler
35    }
36
37    pub fn collect_sample(&mut self, samples: [f64; CHANNELS]) {
38        self.input_samples.push_back(samples);
39
40        let scaled_output_frequency = self.output_frequency * RESAMPLE_SCALING_FACTOR;
41        self.cycle_counter_product += scaled_output_frequency;
42        while self.cycle_counter_product >= self.scaled_source_frequency {
43            self.cycle_counter_product -= self.scaled_source_frequency;
44
45            // Having fewer than N samples in the buffers _shouldn't_ happen, but don't crash if it does
46            while self.input_samples.len() < BUFFER_LEN {
47                self.input_samples
48                    .push_front(self.input_samples.front().copied().unwrap_or([0.0; CHANNELS]));
49            }
50
51            let x = (self.scaled_x_counter as f64) / (scaled_output_frequency as f64);
52            let output: [f64; CHANNELS] = array::from_fn(|channel| {
53                let samples: [f64; 6] = array::from_fn(|i| self.input_samples[i][channel]);
54                interpolate_cubic_hermite_6p(samples, x).clamp(-1.0, 1.0)
55            });
56
57            self.output_samples.push_back(output);
58
59            self.scaled_x_counter += self.scaled_source_frequency;
60            while self.scaled_x_counter >= scaled_output_frequency {
61                self.scaled_x_counter -= scaled_output_frequency;
62
63                self.input_samples.pop_front();
64            }
65        }
66
67        // Having more than N+1 samples in the buffers here also _shouldn't_ happen, but do something reasonable if it does
68        while self.input_samples.len() > BUFFER_LEN + 1 {
69            self.input_samples.pop_front();
70        }
71    }
72
73    #[must_use]
74    pub fn output_buffer_len(&self) -> usize {
75        self.output_samples.len()
76    }
77
78    #[must_use]
79    pub fn output_buffer_pop_front(&mut self) -> Option<[f64; CHANNELS]> {
80        self.output_samples.pop_front()
81    }
82
83    pub fn update_source_frequency(&mut self, source_frequency: f64) {
84        self.convert_sample_distance_counter(source_frequency);
85        self.scaled_source_frequency = scale_source_frequency(source_frequency);
86    }
87
88    pub fn update_output_frequency(&mut self, output_frequency: u64) {
89        self.convert_interpolation_idx_counter(output_frequency);
90        self.output_frequency = output_frequency;
91    }
92
93    fn convert_sample_distance_counter(&mut self, new_source_frequency: f64) {
94        // cycle_counter_product represents the distance towards the next output sample as a percentage
95        // of scaled_source_frequency
96        let output_distance =
97            self.cycle_counter_product as f64 / self.scaled_source_frequency as f64;
98        let new_scaled_source_frequency = new_source_frequency * RESAMPLE_SCALING_FACTOR as f64;
99        self.cycle_counter_product = (output_distance * new_scaled_source_frequency).floor() as u64;
100    }
101
102    fn convert_interpolation_idx_counter(&mut self, new_output_frequency: u64) {
103        // scaled_x_counter represents the interpolation index as a percentage of scaled_output_frequency
104        let scaled_output_frequency = (self.output_frequency * RESAMPLE_SCALING_FACTOR) as f64;
105        let new_scaled_output_frequency = (new_output_frequency * RESAMPLE_SCALING_FACTOR) as f64;
106        let interpolation_idx = self.scaled_x_counter as f64 / scaled_output_frequency;
107        self.scaled_x_counter = (interpolation_idx * new_scaled_output_frequency).floor() as u64;
108    }
109}
110
111fn scale_source_frequency(source_frequency: f64) -> u64 {
112    (source_frequency * RESAMPLE_SCALING_FACTOR as f64).round() as u64
113}