rasterwave 0.6.0

High-performance streaming SSTV, radiofax and RTTY encoders/decoders
Documentation
//! Small DSP building blocks used by the RTTY demodulator.

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,
        )
    }
}

/// Numerically controlled oscillator producing `exp(-j 2 pi f n / fs)` for down-mixing.
#[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());
    }

    /// Returns the current mixing phasor and advances by one sample.
    #[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
    }
}

/// Designs a linear-phase low-pass FIR with a Blackman window and unity DC gain.
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
}

/// Number of taps for a Blackman low-pass with the given transition width.
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
}

/// Complex FIR filter that keeps its own delay line.
#[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,
        }
    }

    /// Stores one sample without computing an output.
    #[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;
        }
    }

    /// Computes the output for the most recently pushed sample.
    #[inline]
    pub(crate) fn output(&self) -> Complex {
        let len = self.history.len();
        let mut re = 0.0;
        let mut im = 0.0;
        // history[cursor] is the oldest sample; taps are symmetric so order is irrelevant.
        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;
    }
}

/// In-place iterative radix-2 complex FFT with precomputed twiddles.
///
/// The scanner needs one modest power-of-two transform a few dozen times per
/// second, which does not justify an FFT dependency.
#[derive(Clone, Debug)]
pub(crate) struct Fft {
    twiddles: Vec<Complex>,
    reversed: Vec<u32>,
}

impl Fft {
    /// Creates a transform of `size` points; `size` must be a power of two of at least 2.
    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 }
    }

    /// Transform length.
    pub(crate) fn size(&self) -> usize {
        self.reversed.len()
    }

    /// Forward transform (`exp(-j 2 pi k n / N)` kernel, no scaling) of `data` in place.
    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);
    }
}