orion-sdr 0.0.74

// Copyright (c) 2025-2026 G & R Associates LLC
// SPDX-License-Identifier: MIT OR Apache-2.0

use crate::core::{Block, WorkReport};
use crate::dsp::{LpCascade, Rotator, quadrature_discriminate};
use num_complex::Complex32 as C32;

// FM Quadrature Demod
#[derive(Debug, Clone)]
pub struct FmQuadratureDemod {
    fs: f32,
    k: f32,
    // optional translator
    xf: Option<Rotator>,
    prev: C32,
    // audio postfilter
    post_lp: LpCascade,
}

impl FmQuadratureDemod {
    pub fn new(fs: f32, dev_hz: f32, audio_bw_hz: f32) -> Self {
        let k = 1.0 / dev_hz.max(1.0);
        let post_lp = LpCascade::design(fs, audio_bw_hz * 0.9);
        Self {
            fs,
            k,
            xf: None,
            prev: C32::new(1.0, 0.0),
            post_lp,
        }
    }

    pub fn with_translate(mut self, freq_hz: f32) -> Self {
        self.xf = Some(Rotator::new(freq_hz, self.fs));
        self
    }
}

impl Block for FmQuadratureDemod {
    type In = C32;
    type Out = f32;

    #[inline(always)]
    fn process(&mut self, input: &[Self::In], output: &mut [Self::Out]) -> WorkReport {
        let n = input.len().min(output.len());
        if let Some(r) = &mut self.xf {
            for i in 0..n {
                let z = input[i] * r.next().conj();
                output[i] = quadrature_discriminate(z, self.prev, self.k, &mut self.post_lp);
                self.prev = z;
            }
        } else {
            for i in 0..n {
                let z = input[i];
                output[i] = quadrature_discriminate(z, self.prev, self.k, &mut self.post_lp);
                self.prev = z;
            }
        }

        WorkReport {
            in_read: n,
            out_written: n,
        }
    }
}