use alloc::boxed::Box;
use alloc::vec;
#[cfg(feature = "fixed-point")]
use alloc::vec::Vec;
use num_complex::Complex;
#[cfg(not(feature = "std"))]
use num_traits::Float;
use super::super::params::{COSTAS, COSTAS_POS, NN, NSPS, NTONES};
use super::types::{AudioSample, SAMPLE_RATE_HZ, TONE_SPACING_HZ, TX_START_OFFSET_S};
use crate::core::scalar::Cmplx;
pub fn goertzel_window_end_sample(dt_sec: f32) -> usize {
let i0 = ((TX_START_OFFSET_S + dt_sec) * SAMPLE_RATE_HZ).round() as i64;
(i0 + (NN as i64) * (NSPS as i64)).max(0) as usize
}
#[cfg(feature = "fft-rustfft")]
std::thread_local! {
static AUDIO_I16_SCRATCH: core::cell::RefCell<alloc::vec::Vec<i16>> =
const { core::cell::RefCell::new(alloc::vec::Vec::new()) };
}
#[cfg(feature = "fft-rustfft")]
use std::sync::OnceLock;
#[cfg(feature = "fft-rustfft")]
static SYMBOL_FFT_32: OnceLock<alloc::sync::Arc<dyn rustfft::Fft<f32>>> = OnceLock::new();
#[doc(hidden)]
pub fn symbol_spectra_direct<S: AudioSample>(
audio: &[S],
freq_hz: f32,
dt_sec: f32,
sym_mask: SymMask,
) -> Box<[[Cmplx<f32>; 8]; 79]> {
let mut out: Box<[[Cmplx<f32>; 8]; 79]> =
vec![[Cmplx::<f32>::default(); 8]; 79].try_into().unwrap();
fill_symbol_spectra(&mut out, audio, freq_hz, dt_sec, sym_mask, None);
out
}
#[doc(hidden)]
#[derive(Copy, Clone, Eq, PartialEq)]
pub enum SymMask {
SyncOnly,
SyncBlock0,
NotBlock0,
DataOnly,
SyncBlocks12,
}
#[inline]
fn sym_in_mask(sym: usize, mask: SymMask) -> bool {
let (in_block_a, in_block_b, in_block_c) = (
sym < COSTAS.len(), sym >= COSTAS_POS[1] && sym < COSTAS_POS[1] + COSTAS.len(), sym >= COSTAS_POS[2] && sym < COSTAS_POS[2] + COSTAS.len(), );
let is_sync = in_block_a || in_block_b || in_block_c;
match mask {
SymMask::SyncOnly => is_sync,
SymMask::SyncBlock0 => in_block_a,
SymMask::NotBlock0 => !in_block_a,
SymMask::DataOnly => !is_sync,
SymMask::SyncBlocks12 => in_block_b || in_block_c,
}
}
#[doc(hidden)]
#[cfg(feature = "fft-rustfft")]
pub fn fill_symbol_spectra<S: AudioSample>(
out: &mut [[Cmplx<f32>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
fft_cache: Option<&[Complex<f32>]>,
) {
fill_symbol_spectra_via_cd0(out, audio, freq_hz, dt_sec, mask, fft_cache);
}
#[doc(hidden)]
#[cfg(all(not(feature = "fft-rustfft"), not(feature = "fixed-point")))]
pub fn fill_symbol_spectra<S: AudioSample>(
out: &mut [[Cmplx<f32>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
fft_cache: Option<&[Complex<f32>]>,
) {
let _ = fft_cache;
fill_symbol_spectra_generic::<f32, S>(out, audio, freq_hz, dt_sec, mask);
}
#[cfg(feature = "fft-rustfft")]
fn fill_symbol_spectra_via_cd0<S: AudioSample>(
out: &mut [[Cmplx<f32>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
fft_cache: Option<&[Complex<f32>]>,
) {
use rustfft::FftPlanner;
extern crate alloc;
let cd0 = match fft_cache {
Some(cache) => crate::core::dsp::downsample::downsample_cached(
cache,
freq_hz,
&crate::ft8::downsample::FT8_CFG,
),
None => AUDIO_I16_SCRATCH.with_borrow_mut(|buf| {
buf.clear();
buf.reserve(audio.len());
buf.extend(audio.iter().map(|s| s.to_i16()));
crate::ft8::downsample::downsample(buf.as_slice(), freq_hz, None).0
}),
};
let ibest = ((TX_START_OFFSET_S + dt_sec) * 200.0).round() as i32;
let fft = SYMBOL_FFT_32
.get_or_init(|| {
let mut planner = FftPlanner::<f32>::new();
planner.plan_fft_forward(32)
})
.clone();
let mut buf = [Complex::new(0.0_f32, 0.0); 32];
const CS_SCALE: f32 = 1.0 / 1000.0;
let np2 = cd0.len() as i32;
for sym in 0..NN {
if !sym_in_mask(sym, mask) {
continue;
}
let i1 = ibest + (sym as i32) * 32;
if i1 >= 0 && i1 + 31 < np2 {
for j in 0..32 {
buf[j] = cd0[(i1 + j as i32) as usize];
}
} else {
for j in 0..32 {
buf[j] = Complex::new(0.0, 0.0);
}
}
fft.process(&mut buf);
for tone in 0..NTONES {
out[sym][tone] = Cmplx {
re: buf[tone].re * CS_SCALE,
im: buf[tone].im * CS_SCALE,
};
}
}
}
#[doc(hidden)]
#[cfg(not(feature = "fixed-point"))]
pub fn fill_symbol_spectra_generic<Sc: crate::core::scalar::SpecScalar, S: AudioSample>(
out: &mut [[Cmplx<Sc>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
) {
let i0 = ((TX_START_OFFSET_S + dt_sec) * SAMPLE_RATE_HZ).round() as i64;
let two_pi_over_fs = core::f32::consts::TAU / SAMPLE_RATE_HZ;
let mut rotators = [Complex::new(0.0f32, 0.0); NTONES];
for tone in 0..NTONES {
let tone_freq = freq_hz + tone as f32 * TONE_SPACING_HZ;
let dphi = -two_pi_over_fs * tone_freq;
rotators[tone] = Complex::new(dphi.cos(), dphi.sin());
}
let mut sym_buf = [0.0f32; NSPS];
if !Sc::NEEDS_AUTOGAIN {
for sym in 0..NN {
if !sym_in_mask(sym, mask) {
continue;
}
let sym_start = i0 + (sym as i64) * (NSPS as i64);
for k in 0..NSPS {
let idx = sym_start + k as i64;
sym_buf[k] = if idx >= 0 && (idx as usize) < audio.len() {
audio[idx as usize].to_f32()
} else {
0.0
};
}
for tone in 0..NTONES {
let rotator = rotators[tone];
let mut osc = Complex::new(1.0f32, 0.0);
let mut acc = Complex::new(0.0f32, 0.0);
for &s in sym_buf.iter() {
acc.re += s * osc.re;
acc.im += s * osc.im;
osc *= rotator;
}
out[sym][tone] = Cmplx {
re: Sc::from_f32(acc.re),
im: Sc::from_f32(acc.im),
};
}
}
return;
}
let mut tmp = [[Complex::new(0.0f32, 0.0); 8]; 79];
let mut peak: f32 = 0.0;
for sym in 0..NN {
if !sym_in_mask(sym, mask) {
continue;
}
let sym_start = i0 + (sym as i64) * (NSPS as i64);
for k in 0..NSPS {
let idx = sym_start + k as i64;
sym_buf[k] = if idx >= 0 && (idx as usize) < audio.len() {
audio[idx as usize].to_f32()
} else {
0.0
};
}
for tone in 0..NTONES {
let rotator = rotators[tone];
let mut osc = Complex::new(1.0f32, 0.0);
let mut acc = Complex::new(0.0f32, 0.0);
for &s in sym_buf.iter() {
acc.re += s * osc.re;
acc.im += s * osc.im;
osc *= rotator;
}
tmp[sym][tone] = acc;
peak = peak.max(acc.re.abs()).max(acc.im.abs());
}
}
let scale = if peak > 1e-9 {
(i16::MAX as f32 * 0.95) / peak
} else {
0.0
};
for sym in 0..NN {
if !sym_in_mask(sym, mask) {
continue;
}
for tone in 0..NTONES {
let c = tmp[sym][tone];
out[sym][tone] = Cmplx {
re: Sc::from_f32_scaled(c.re, scale),
im: Sc::from_f32_scaled(c.im, scale),
};
}
}
}
#[cfg(feature = "fixed-point")]
pub const BASIS_SCRATCH_LEN: usize = NTONES * NSPS;
#[doc(hidden)]
#[cfg(all(feature = "fixed-point", not(feature = "fft-rustfft")))]
pub fn fill_symbol_spectra<S: AudioSample>(
out: &mut [[Cmplx<f32>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
fft_cache: Option<&[Complex<f32>]>,
) {
let _ = fft_cache;
fill_symbol_spectra_generic::<f32, S>(out, audio, freq_hz, dt_sec, mask);
}
#[doc(hidden)]
#[cfg(feature = "fixed-point")]
pub fn fill_symbol_spectra_generic<Sc: crate::core::scalar::SpecScalar, S: AudioSample>(
out: &mut [[Cmplx<Sc>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
) {
let mut basis_re: Vec<i16> = alloc::vec![0i16; BASIS_SCRATCH_LEN];
let mut basis_im: Vec<i16> = alloc::vec![0i16; BASIS_SCRATCH_LEN];
fill_symbol_spectra_into_generic::<Sc, S>(
out,
audio,
freq_hz,
dt_sec,
mask,
&mut basis_re,
&mut basis_im,
);
}
#[doc(hidden)]
#[cfg(feature = "fixed-point")]
pub fn fill_symbol_spectra_into<S: AudioSample>(
out: &mut [[Cmplx<f32>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
basis_re: &mut [i16],
basis_im: &mut [i16],
) {
fill_symbol_spectra_into_generic::<f32, S>(
out, audio, freq_hz, dt_sec, mask, basis_re, basis_im,
);
}
#[doc(hidden)]
#[cfg(feature = "fixed-point")]
pub fn fill_symbol_spectra_into_generic<Sc: crate::core::scalar::SpecScalar, S: AudioSample>(
out: &mut [[Cmplx<Sc>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
basis_re: &mut [i16],
basis_im: &mut [i16],
) {
use crate::core::dotprod::dot_q15_i32;
debug_assert!(basis_re.len() >= BASIS_SCRATCH_LEN);
debug_assert!(basis_im.len() >= BASIS_SCRATCH_LEN);
let i0 = ((TX_START_OFFSET_S + dt_sec) * SAMPLE_RATE_HZ).round() as i64;
let two_pi_over_fs = core::f32::consts::TAU / SAMPLE_RATE_HZ;
for tone in 0..NTONES {
let tone_freq = freq_hz + tone as f32 * TONE_SPACING_HZ;
let dphi = -two_pi_over_fs * tone_freq;
let rot_re = (dphi.cos() * 32767.0).round() as i32;
let rot_im = (dphi.sin() * 32767.0).round() as i32;
let mut osc_re: i32 = 32767;
let mut osc_im: i32 = 0;
let base = tone * NSPS;
for k in 0..NSPS {
basis_re[base + k] = osc_re as i16;
basis_im[base + k] = osc_im as i16;
let new_re = ((osc_re * rot_re) - (osc_im * rot_im)) >> 15;
let new_im = ((osc_re * rot_im) + (osc_im * rot_re)) >> 15;
osc_re = new_re;
osc_im = new_im;
}
}
let mut sym_buf = [0i16; NSPS];
let mut tmp_re = [[0i32; 8]; 79];
let mut tmp_im = [[0i32; 8]; 79];
let mut peak: i32 = 0;
for sym in 0..NN {
if !sym_in_mask(sym, mask) {
continue;
}
let sym_start = i0 + (sym as i64) * (NSPS as i64);
for k in 0..NSPS {
let idx = sym_start + k as i64;
sym_buf[k] = if idx >= 0 && (idx as usize) < audio.len() {
audio[idx as usize].to_i16()
} else {
0
};
}
for tone in 0..NTONES {
let base = tone * NSPS;
let basis_re_tone = &basis_re[base..base + NSPS];
let basis_im_tone = &basis_im[base..base + NSPS];
let acc_re = dot_q15_i32(&sym_buf, basis_re_tone);
let acc_im = dot_q15_i32(&sym_buf, basis_im_tone);
if !Sc::NEEDS_AUTOGAIN {
out[sym][tone] = Cmplx {
re: Sc::from_f32(acc_re as f32),
im: Sc::from_f32(acc_im as f32),
};
} else {
tmp_re[sym][tone] = acc_re;
tmp_im[sym][tone] = acc_im;
peak = peak.max(acc_re.unsigned_abs() as i32);
peak = peak.max(acc_im.unsigned_abs() as i32);
}
}
}
if Sc::NEEDS_AUTOGAIN {
let scale = if peak > 0 {
(i16::MAX as f32 * 0.95) / peak as f32
} else {
0.0
};
for sym in 0..NN {
if !sym_in_mask(sym, mask) {
continue;
}
for tone in 0..NTONES {
out[sym][tone] = Cmplx {
re: Sc::from_f32_scaled(tmp_re[sym][tone] as f32, scale),
im: Sc::from_f32_scaled(tmp_im[sym][tone] as f32, scale),
};
}
}
}
}
pub fn fill_symbol_spectra_goertzel<S: AudioSample>(
out: &mut [[Cmplx<f32>; 8]; 79],
audio: &[S],
freq_hz: f32,
dt_sec: f32,
mask: SymMask,
) {
let two_pi_over_fs = core::f32::consts::TAU / SAMPLE_RATE_HZ;
let i0 = ((TX_START_OFFSET_S + dt_sec) * SAMPLE_RATE_HZ).round() as i64;
let mut cos_w = [0.0_f32; NTONES];
let mut sin_w = [0.0_f32; NTONES];
let mut coeff = [0.0_f32; NTONES];
for tone in 0..NTONES {
let tone_freq = freq_hz + (tone as f32) * TONE_SPACING_HZ;
let omega = two_pi_over_fs * tone_freq;
cos_w[tone] = omega.cos();
sin_w[tone] = omega.sin();
coeff[tone] = 2.0 * cos_w[tone];
}
for sym in 0..NN {
if !sym_in_mask(sym, mask) {
continue;
}
let sym_start = i0 + (sym as i64) * (NSPS as i64);
let mut s_prev = [0.0_f32; NTONES];
let mut s_prev2 = [0.0_f32; NTONES];
if sym_start >= 0 && (sym_start as usize) <= audio.len().saturating_sub(NSPS) {
let base = sym_start as usize;
for n in 0..NSPS {
let sample = audio[base + n].to_i16() as f32;
for tone in 0..NTONES {
let s = sample + coeff[tone] * s_prev[tone] - s_prev2[tone];
s_prev2[tone] = s_prev[tone];
s_prev[tone] = s;
}
}
} else {
let audio_len = audio.len() as i64;
for n in 0..NSPS {
let idx = sym_start + n as i64;
let sample = if idx >= 0 && idx < audio_len {
audio[idx as usize].to_i16() as f32
} else {
0.0
};
for tone in 0..NTONES {
let s = sample + coeff[tone] * s_prev[tone] - s_prev2[tone];
s_prev2[tone] = s_prev[tone];
s_prev[tone] = s;
}
}
}
for tone in 0..NTONES {
let re = s_prev[tone] - cos_w[tone] * s_prev2[tone];
let im = sin_w[tone] * s_prev2[tone];
out[sym][tone] = Cmplx { re, im };
}
}
}
#[doc(hidden)]
#[cfg(feature = "fixed-point")]
pub fn symbol_spectra_direct_into<S: AudioSample>(
audio: &[S],
freq_hz: f32,
dt_sec: f32,
sym_mask: SymMask,
basis_re: &mut [i16],
basis_im: &mut [i16],
) -> Box<[[Cmplx<f32>; 8]; 79]> {
let mut out: Box<[[Cmplx<f32>; 8]; 79]> =
vec![[Cmplx::<f32>::default(); 8]; 79].try_into().unwrap();
fill_symbol_spectra_into(
&mut out, audio, freq_hz, dt_sec, sym_mask, basis_re, basis_im,
);
out
}