154 lines
4.0 KiB
Rust
154 lines
4.0 KiB
Rust
use crate::iir_filter::IIRFilter;
|
|
|
|
type Sample = f64;
|
|
|
|
#[derive(Clone)]
|
|
pub struct SampleRate {
|
|
pub factor: f64,
|
|
pub filter_cutoff: f64,
|
|
pub down: DownSampler,
|
|
pub up: UpSampler,
|
|
}
|
|
|
|
#[derive(Clone)]
|
|
pub struct DownSampler {
|
|
pub lp: IIRFilter,
|
|
pub last_sample: Sample,
|
|
pub in_index: f64,
|
|
}
|
|
|
|
#[derive(Clone)]
|
|
pub struct UpSampler {
|
|
pub lp: IIRFilter,
|
|
pub current_sample: Sample,
|
|
pub last_sample: Sample,
|
|
pub in_index: f64,
|
|
}
|
|
|
|
impl SampleRate {
|
|
pub fn init(&mut self, low_samplerate: f64, high_samplerate: f64, filter_cutoff: f64) -> Result<(), i32> {
|
|
// clear state
|
|
self.factor = 0.0;
|
|
self.filter_cutoff = 0.0;
|
|
|
|
self.factor = high_samplerate / low_samplerate;
|
|
if self.factor < 1.0 {
|
|
eprintln!("Software error: Low sample rate must be lower than high sample rate, aborting!");
|
|
return Err(-1);
|
|
}
|
|
|
|
self.filter_cutoff = filter_cutoff;
|
|
if self.filter_cutoff > 0.0 {
|
|
self.up.lp.lowpass_init(filter_cutoff, high_samplerate as i32, 2);
|
|
self.down.lp.lowpass_init(filter_cutoff, high_samplerate as i32, 2);
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
pub fn downsample_input_num(&mut self, output_num: i32) -> i32 {
|
|
let factor = self.factor;
|
|
let mut in_index = self.down.in_index;
|
|
let mut idx = 0;
|
|
|
|
for _ in 0..output_num {
|
|
in_index += factor;
|
|
idx = in_index as i32;
|
|
}
|
|
|
|
idx
|
|
}
|
|
|
|
pub fn downsample_output_num(&mut self, input_num: i32) -> i32 {
|
|
let factor = self.factor;
|
|
let mut in_index = self.down.in_index;
|
|
let mut output_num = 0;
|
|
let mut idx;
|
|
|
|
loop {
|
|
idx = in_index as i32;
|
|
if idx >= input_num {
|
|
break;
|
|
}
|
|
output_num += 1;
|
|
in_index += factor;
|
|
}
|
|
|
|
output_num
|
|
}
|
|
|
|
pub fn downsample(&mut self, samples: &mut [Sample], input_num: i32) -> i32 {
|
|
let factor = self.factor;
|
|
let mut output_num = 0;
|
|
let mut in_index = self.down.in_index;
|
|
let mut last_sample = self.down.last_sample;
|
|
let mut idx;
|
|
let mut diff;
|
|
|
|
// filter down if cutoff is set
|
|
if self.filter_cutoff > 0.0 {
|
|
self.down.lp.process(&mut samples[..input_num as usize]);
|
|
}
|
|
|
|
// allocate output buffer w/ safety margin
|
|
let output_size = ((input_num as f64) / factor + 0.5) as usize + 10;
|
|
let mut output = vec![0.0; output_size];
|
|
|
|
for i in 0.. {
|
|
idx = in_index as i32;
|
|
if idx >= input_num {
|
|
break;
|
|
}
|
|
diff = in_index - idx as f64;
|
|
if idx > 0 {
|
|
output[i] = samples[(idx-1) as usize] * (1.0 - diff) + samples[idx as usize] * diff;
|
|
} else {
|
|
output[i] = last_sample * (1.0 - diff) + samples[idx as usize] * diff;
|
|
}
|
|
output_num += 1;
|
|
in_index += factor;
|
|
}
|
|
|
|
// store last sample
|
|
if input_num > 0 {
|
|
self.down.last_sample = samples[(input_num -1) as usize];
|
|
}
|
|
|
|
// adjust in_index
|
|
in_index -= input_num as f64;
|
|
if (in_index as i32) < 0 {
|
|
in_index = 0.0;
|
|
}
|
|
self.down.in_index = in_index;
|
|
|
|
// copy output back to samples
|
|
for i in 0..output_num {
|
|
samples[i as usize] = output[i as usize];
|
|
}
|
|
|
|
output_num
|
|
}
|
|
|
|
pub fn upsample_input_num(&mut self, output_num: i32) -> i32 {
|
|
let factor = 1.0 / self.factor;
|
|
let mut in_index = self.up.in_index;
|
|
let mut idx = 0;
|
|
|
|
for _ in 0..output_num {
|
|
in_index += factor;
|
|
if in_index >= 1.0 {
|
|
idx += 1;
|
|
in_index -= 1.0;
|
|
}
|
|
}
|
|
idx
|
|
}
|
|
|
|
pub fn upsample_output_num(&mut self, input_num: i32) -> i32 {
|
|
Ok(())
|
|
}
|
|
|
|
pub fn upsample(&mut self, state: SampleRate, input: Sample, input_num: i32, output: Sample, output_num: i32) {
|
|
|
|
}
|
|
|
|
} |