use std::f64::consts::{PI, TAU};
use std::ops::{Add, Mul, Sub};
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub(crate) struct Complex {
pub(crate) re: f64,
pub(crate) im: f64,
}
impl Complex {
pub(crate) const ZERO: Self = Self { re: 0.0, im: 0.0 };
#[inline]
pub(crate) const fn new(re: f64, im: f64) -> Self {
Self { re, im }
}
#[inline]
pub(crate) fn norm_sqr(self) -> f64 {
self.re * self.re + self.im * self.im
}
#[inline]
pub(crate) fn norm(self) -> f64 {
self.norm_sqr().sqrt()
}
#[inline]
pub(crate) fn conj(self) -> Self {
Self::new(self.re, -self.im)
}
#[inline]
pub(crate) fn arg(self) -> f64 {
self.im.atan2(self.re)
}
#[inline]
pub(crate) fn scale(self, factor: f64) -> Self {
Self::new(self.re * factor, self.im * factor)
}
}
impl Add for Complex {
type Output = Self;
#[inline]
fn add(self, rhs: Self) -> Self {
Self::new(self.re + rhs.re, self.im + rhs.im)
}
}
impl Sub for Complex {
type Output = Self;
#[inline]
fn sub(self, rhs: Self) -> Self {
Self::new(self.re - rhs.re, self.im - rhs.im)
}
}
impl Mul for Complex {
type Output = Self;
#[inline]
fn mul(self, rhs: Self) -> Self {
Self::new(
self.re * rhs.re - self.im * rhs.im,
self.re * rhs.im + self.im * rhs.re,
)
}
}
#[derive(Clone, Debug)]
pub(crate) struct Nco {
phasor: Complex,
step: Complex,
until_normalize: u32,
}
impl Nco {
pub(crate) fn new(frequency_hz: f64, sample_rate: f64) -> Self {
let mut nco = Self {
phasor: Complex::new(1.0, 0.0),
step: Complex::new(1.0, 0.0),
until_normalize: 1024,
};
nco.set_frequency(frequency_hz, sample_rate);
nco
}
pub(crate) fn set_frequency(&mut self, frequency_hz: f64, sample_rate: f64) {
let omega = -TAU * frequency_hz / sample_rate;
self.step = Complex::new(omega.cos(), omega.sin());
}
#[inline]
pub(crate) fn next(&mut self) -> Complex {
let current = self.phasor;
self.phasor = self.phasor * self.step;
self.until_normalize -= 1;
if self.until_normalize == 0 {
let magnitude = self.phasor.norm();
if magnitude > f64::EPSILON {
self.phasor = self.phasor.scale(1.0 / magnitude);
} else {
self.phasor = Complex::new(1.0, 0.0);
}
self.until_normalize = 1024;
}
current
}
}
pub(crate) fn design_lowpass(cutoff_hz: f64, sample_rate: f64, taps: usize) -> Vec<f64> {
let taps = taps.max(3) | 1;
let center = (taps - 1) as f64 / 2.0;
let normalized = cutoff_hz / sample_rate;
let mut coefficients: Vec<f64> = (0..taps)
.map(|index| {
let x = index as f64 - center;
let sinc = if x == 0.0 {
2.0 * normalized
} else {
(TAU * normalized * x).sin() / (PI * x)
};
let phase = TAU * index as f64 / (taps - 1) as f64;
let window = 0.42 - 0.5 * phase.cos() + 0.08 * (2.0 * phase).cos();
sinc * window
})
.collect();
let sum: f64 = coefficients.iter().sum();
if sum.abs() > f64::EPSILON {
for value in &mut coefficients {
*value /= sum;
}
}
coefficients
}
pub(crate) fn blackman_taps(transition_hz: f64, sample_rate: f64, max_taps: usize) -> usize {
let estimate = (5.5 * sample_rate / transition_hz.max(1.0)).ceil() as usize;
estimate.clamp(15, max_taps) | 1
}
#[derive(Clone, Debug)]
pub(crate) struct ComplexFir {
taps: Vec<f64>,
history: Vec<Complex>,
cursor: usize,
}
impl ComplexFir {
pub(crate) fn new(taps: Vec<f64>) -> Self {
let len = taps.len();
Self {
taps,
history: vec![Complex::ZERO; len],
cursor: 0,
}
}
#[inline]
pub(crate) fn push(&mut self, sample: Complex) {
self.history[self.cursor] = sample;
self.cursor += 1;
if self.cursor == self.history.len() {
self.cursor = 0;
}
}
#[inline]
pub(crate) fn output(&self) -> Complex {
let len = self.history.len();
let mut re = 0.0;
let mut im = 0.0;
let (newer, older) = self.history.split_at(self.cursor);
for (tap, sample) in self.taps.iter().zip(older.iter().chain(newer.iter())) {
re += tap * sample.re;
im += tap * sample.im;
}
debug_assert_eq!(older.len() + newer.len(), len);
Complex::new(re, im)
}
#[inline]
pub(crate) fn filter(&mut self, sample: Complex) -> Complex {
self.push(sample);
self.output()
}
pub(crate) fn reset(&mut self) {
self.history.fill(Complex::ZERO);
self.cursor = 0;
}
}
#[derive(Clone, Debug)]
pub(crate) struct Fft {
twiddles: Vec<Complex>,
reversed: Vec<u32>,
}
impl Fft {
pub(crate) fn new(size: usize) -> Self {
debug_assert!(size.is_power_of_two() && size >= 2);
let bits = size.trailing_zeros();
let reversed = (0..size)
.map(|index| (index as u32).reverse_bits() >> (u32::BITS - bits))
.collect();
let twiddles = (0..size / 2)
.map(|k| {
let angle = -TAU * k as f64 / size as f64;
Complex::new(angle.cos(), angle.sin())
})
.collect();
Self { twiddles, reversed }
}
pub(crate) fn size(&self) -> usize {
self.reversed.len()
}
pub(crate) fn forward(&self, data: &mut [Complex]) {
let size = self.size();
debug_assert_eq!(data.len(), size);
for (index, &target) in self.reversed.iter().enumerate() {
let target = target as usize;
if target > index {
data.swap(index, target);
}
}
let mut len = 2;
while len <= size {
let half = len / 2;
let stride = size / len;
for block in data.chunks_exact_mut(len) {
let (lower, upper) = block.split_at_mut(half);
for (k, (low, high)) in lower.iter_mut().zip(upper.iter_mut()).enumerate() {
let product = *high * self.twiddles[k * stride];
*high = *low - product;
*low = *low + product;
}
}
len <<= 1;
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn fft_matches_direct_dft() {
let size = 64;
let input: Vec<Complex> = (0..size)
.map(|n| Complex::new((n as f64 * 0.37).sin() + 0.1 * n as f64, (n as f64).cos()))
.collect();
let mut fast = input.clone();
Fft::new(size).forward(&mut fast);
for (k, value) in fast.iter().enumerate() {
let mut direct = Complex::ZERO;
for (n, sample) in input.iter().enumerate() {
let angle = -TAU * (k * n) as f64 / size as f64;
direct = direct + *sample * Complex::new(angle.cos(), angle.sin());
}
assert!((direct - *value).norm() < 1e-9, "bin {k}");
}
}
#[test]
fn lowpass_passes_dc_and_rejects_stopband() {
let taps = design_lowpass(200.0, 1_500.0, 63);
let response = |freq: f64| {
let (mut re, mut im) = (0.0, 0.0);
for (n, tap) in taps.iter().enumerate() {
let w = TAU * freq / 1_500.0 * n as f64;
re += tap * w.cos();
im -= tap * w.sin();
}
(re * re + im * im).sqrt()
};
assert!((response(0.0) - 1.0).abs() < 1e-9);
assert!(response(100.0) > 0.95);
assert!(response(500.0) < 0.002);
}
#[test]
fn nco_down_mixes_a_tone_to_dc() {
let rate = 12_000.0;
let mut nco = Nco::new(1_000.0, rate);
let mut sum = Complex::ZERO;
for n in 0..12_000 {
let phase = TAU * 1_000.0 * n as f64 / rate;
let sample = Complex::new(phase.cos(), phase.sin());
sum = sum + sample * nco.next();
}
assert!((sum.re / 12_000.0 - 1.0).abs() < 1e-6);
}
}