use alloc::vec;
use alloc::vec::Vec;
use core::f32::consts::PI;
use num_complex::Complex;
#[cfg(not(feature = "std"))]
use num_traits::Float;
#[cfg(feature = "parallel")]
use rayon::prelude::*;
use super::{Protocol, SpectrumWindow};
use crate::engine::fft::default_planner;
#[derive(Debug, Clone)]
pub struct SyncCandidate {
pub freq_hz: f32,
pub dt_sec: f32,
pub score: f32,
}
pub fn bootstrap_dt_median(cands: &[SyncCandidate], top_k: usize) -> Option<f32> {
if cands.is_empty() || top_k == 0 {
return None;
}
let mut refs: Vec<&SyncCandidate> = cands.iter().collect();
let k = top_k.min(refs.len());
if k < refs.len() {
refs.select_nth_unstable_by(k - 1, |a, b| {
b.score
.partial_cmp(&a.score)
.unwrap_or(core::cmp::Ordering::Equal)
});
}
let mut dts: Vec<f32> = refs[..k].iter().map(|c| c.dt_sec).collect();
dts.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap_or(core::cmp::Ordering::Equal));
let n = dts.len();
Some(if n % 2 == 1 {
dts[n / 2]
} else {
0.5 * (dts[n / 2 - 1] + dts[n / 2])
})
}
#[derive(Copy, Clone, Debug)]
pub struct SyncDims {
pub nfft1: usize,
pub nstep: usize,
pub nsps: usize,
pub nssy: usize,
pub nfos: usize,
pub nmax: usize,
pub nhsym: usize,
pub nh1: usize,
pub df: f32,
pub tstep: f32,
pub jstrt: i32,
pub jz: i32,
pub ds_spb: usize,
pub ds_rate: f32,
}
impl SyncDims {
#[inline]
pub fn of<P: Protocol>(sample_rate_hz: f32) -> Self {
let nsps = (P::SYMBOL_DT * sample_rate_hz).round() as usize;
let nstep = nsps / P::NSTEP_PER_SYMBOL as usize;
let nfft1 = nsps * P::NFFT_PER_SYMBOL_FACTOR as usize;
let nmax = (P::T_SLOT_S * sample_rate_hz) as usize;
let tstep = nstep as f32 / sample_rate_hz;
let ndown = P::NDOWN as usize;
Self {
nfft1,
nstep,
nsps,
nssy: P::NSTEP_PER_SYMBOL as usize,
nfos: P::NFFT_PER_SYMBOL_FACTOR as usize,
nmax,
nhsym: nmax / nstep - 3,
nh1: nfft1 / 2,
df: sample_rate_hz / nfft1 as f32,
tstep,
jstrt: (P::TX_START_OFFSET_S / tstep) as i32,
jz: (2.5 / tstep) as i32,
ds_spb: P::NSPS as usize / ndown,
ds_rate: 12_000.0 / ndown as f32,
}
}
}
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct RxGrid {
pub sample_rate_hz: f32,
pub center_hz: f32,
pub complex_input: bool,
}
impl RxGrid {
pub fn real(sample_rate_hz: f32) -> Self {
Self {
sample_rate_hz,
center_hz: 0.0,
complex_input: false,
}
}
pub fn complex(sample_rate_hz: f32, center_hz: f32) -> Self {
Self {
sample_rate_hz,
center_hz,
complex_input: true,
}
}
#[inline]
pub fn bin_of(&self, d: &SyncDims, hz: f32) -> usize {
if self.complex_input {
(((hz - self.center_hz) / d.df) + d.nfft1 as f32 / 2.0).round() as usize
} else {
(hz / d.df).round() as usize
}
}
#[inline]
pub fn hz_of(&self, d: &SyncDims, bin: usize) -> f32 {
if self.complex_input {
self.center_hz + (bin as f32 - d.nfft1 as f32 / 2.0) * d.df
} else {
bin as f32 * d.df
}
}
#[inline]
pub fn usable_bins(&self, d: &SyncDims) -> usize {
if self.complex_input { d.nfft1 } else { d.nh1 }
}
}
#[derive(Copy, Clone)]
pub enum AudioSource<'a> {
Real(&'a [i16]),
Complex(&'a [f32], &'a [f32]),
}
pub struct Spectrogram {
pub n_freq: usize,
pub n_time: usize,
freq_offset: usize,
data: Vec<f32>,
}
impl Spectrogram {
#[inline]
fn get(&self, freq: usize, time: usize) -> f32 {
debug_assert!(
freq >= self.freq_offset,
"Spectrogram::get: freq {freq} below cropped range starting at {}",
self.freq_offset
);
self.data[(freq - self.freq_offset) * self.n_time + time]
}
#[inline]
pub fn freq_offset(&self) -> usize {
self.freq_offset
}
pub fn avg_power_per_bin(&self) -> Vec<f32> {
let inv_t = 1.0 / self.n_time as f32;
let mut out = vec![0.0f32; self.n_freq];
for f in 0..self.n_freq {
let base = f * self.n_time;
let mut s = 0.0f32;
for t in 0..self.n_time {
s += self.data[base + t];
}
out[f] = s * inv_t;
}
out
}
}
pub(crate) fn nuttall_window(n: usize) -> Vec<f32> {
const A0: f32 = 0.3635819;
const A1: f32 = 0.4891775;
const A2: f32 = 0.1365995;
const A3: f32 = 0.0106411;
let mut w = vec![0.0f32; n];
if n < 2 {
if n == 1 {
w[0] = 1.0;
}
return w;
}
let two_pi = 2.0 * PI;
let denom = (n - 1) as f32;
for (k, slot) in w.iter_mut().enumerate() {
let x = k as f32 / denom;
*slot = A0 - A1 * (two_pi * x).cos() + A2 * (2.0 * two_pi * x).cos()
- A3 * (3.0 * two_pi * x).cos();
}
w
}
pub fn compute_spectra<P: Protocol>(
audio: AudioSource,
bin_lo: usize,
bin_hi_incl: usize,
grid: RxGrid,
) -> Spectrogram {
let d = SyncDims::of::<P>(grid.sample_rate_hz);
let fac = 1.0f32 / 300.0;
let mut planner = default_planner();
let fft = planner.plan_forward(d.nfft1);
let window: Option<Vec<f32>> = match P::SPECTRUM_WINDOW {
SpectrumWindow::Rectangular => None,
SpectrumWindow::Nuttall4 => Some(nuttall_window(d.nsps)),
};
let usable = grid.usable_bins(&d);
let bin_lo = bin_lo.min(usable.saturating_sub(1));
let bin_hi_incl = bin_hi_incl.min(usable.saturating_sub(1)).max(bin_lo);
let n_freq = bin_hi_incl - bin_lo + 1;
let mut data = vec![0.0f32; n_freq * d.nhsym];
let mut buf = vec![Complex::new(0.0f32, 0.0); d.nfft1];
for j in 0..d.nhsym {
let ia = j * d.nstep;
for (k, c) in buf.iter_mut().enumerate() {
*c = if k < d.nsps {
match audio {
AudioSource::Real(pcm) => {
let sample = if ia + k < pcm.len() {
let raw = pcm[ia + k] as f32 * fac;
match &window {
Some(w) => raw * w[k],
None => raw,
}
} else {
0.0
};
Complex::new(sample, 0.0)
}
AudioSource::Complex(xi, xq) => {
if ia + k < xi.len() {
let (mut si, mut sq) = (xi[ia + k], xq[ia + k]);
if let Some(w) = &window {
si *= w[k];
sq *= w[k];
}
Complex::new(si, sq)
} else {
Complex::new(0.0, 0.0)
}
}
}
} else {
Complex::new(0.0, 0.0)
};
}
fft.process(&mut buf);
if grid.complex_input {
for shifted in bin_lo..=bin_hi_incl {
let k = (shifted + d.nfft1 / 2) % d.nfft1;
data[(shifted - bin_lo) * d.nhsym + j] = buf[k].norm_sqr();
}
} else {
for i in bin_lo..=bin_hi_incl {
data[(i - bin_lo) * d.nhsym + j] = buf[i].norm_sqr();
}
}
}
Spectrogram {
n_freq,
n_time: d.nhsym,
freq_offset: bin_lo,
data,
}
}
pub struct SpectrogramBuilder {
d: SyncDims,
grid: RxGrid,
bin_lo: usize,
bin_hi_incl: usize,
n_freq: usize,
window: Option<Vec<f32>>,
fft: alloc::boxed::Box<dyn crate::engine::fft::Fft>,
hist_i: Vec<f32>,
hist_q: Vec<f32>,
abs_base: usize,
j: usize,
data: Vec<f32>,
buf: Vec<Complex<f32>>,
}
impl SpectrogramBuilder {
pub fn new<P: Protocol>(bin_lo: usize, bin_hi_incl: usize, grid: RxGrid) -> Self {
let d = SyncDims::of::<P>(grid.sample_rate_hz);
let window: Option<Vec<f32>> = match P::SPECTRUM_WINDOW {
SpectrumWindow::Rectangular => None,
SpectrumWindow::Nuttall4 => Some(nuttall_window(d.nsps)),
};
let usable = grid.usable_bins(&d);
let bin_lo = bin_lo.min(usable.saturating_sub(1));
let bin_hi_incl = bin_hi_incl.min(usable.saturating_sub(1)).max(bin_lo);
let n_freq = bin_hi_incl - bin_lo + 1;
let mut planner = default_planner();
let fft = planner.plan_forward(d.nfft1);
let nfft1 = d.nfft1;
let nhsym = d.nhsym;
Self {
d,
grid,
bin_lo,
bin_hi_incl,
n_freq,
window,
fft,
hist_i: Vec::new(),
hist_q: Vec::new(),
abs_base: 0,
j: 0,
data: vec![0.0f32; n_freq * nhsym],
buf: vec![Complex::new(0.0f32, 0.0); nfft1],
}
}
pub fn push(&mut self, xi: &[f32], xq: &[f32]) {
debug_assert_eq!(xi.len(), xq.len());
self.hist_i.extend_from_slice(xi);
self.hist_q.extend_from_slice(xq);
self.drain_ready(false);
}
pub fn finish(mut self) -> Spectrogram {
self.drain_ready(true);
Spectrogram {
n_freq: self.n_freq,
n_time: self.d.nhsym,
freq_offset: self.bin_lo,
data: self.data,
}
}
fn drain_ready(&mut self, flush: bool) {
while self.j < self.d.nhsym {
let ia = self.j * self.d.nstep;
let need_end = ia + self.d.nsps;
let have_end = self.abs_base + self.hist_i.len();
if !flush && have_end < need_end {
break;
}
self.emit_row(ia);
self.j += 1;
}
if self.j < self.d.nhsym {
let keep_from = self.j * self.d.nstep;
let drop = keep_from
.saturating_sub(self.abs_base)
.min(self.hist_i.len());
if drop > 0 {
self.hist_i.drain(..drop);
self.hist_q.drain(..drop);
self.abs_base += drop;
}
}
}
fn emit_row(&mut self, ia: usize) {
let d = &self.d;
for (k, c) in self.buf.iter_mut().enumerate() {
*c = if k < d.nsps {
let abs = ia + k;
if abs >= self.abs_base && abs - self.abs_base < self.hist_i.len() {
let idx = abs - self.abs_base;
let (mut si, mut sq) = (self.hist_i[idx], self.hist_q[idx]);
if let Some(w) = &self.window {
si *= w[k];
sq *= w[k];
}
Complex::new(si, sq)
} else {
Complex::new(0.0, 0.0)
}
} else {
Complex::new(0.0, 0.0)
};
}
self.fft.process(&mut self.buf);
let j = self.j;
let nhsym = d.nhsym;
let nfft1 = d.nfft1;
if self.grid.complex_input {
for shifted in self.bin_lo..=self.bin_hi_incl {
let k = (shifted + nfft1 / 2) % nfft1;
self.data[(shifted - self.bin_lo) * nhsym + j] = self.buf[k].norm_sqr();
}
} else {
for i in self.bin_lo..=self.bin_hi_incl {
self.data[(i - self.bin_lo) * nhsym + j] = self.buf[i].norm_sqr();
}
}
}
}
pub fn coarse_sync<P: Protocol>(
audio: AudioSource,
freq_min: f32,
freq_max: f32,
sync_min: f32,
freq_hint: Option<f32>,
max_cand: usize,
grid: RxGrid,
) -> Vec<SyncCandidate> {
let Some((bin_lo, bin_hi)) = spectra_crop_for::<P>(freq_min, freq_max, grid) else {
return Vec::new();
};
let s = compute_spectra::<P>(audio, bin_lo, bin_hi, grid);
coarse_sync_from_spectra::<P>(&s, freq_min, freq_max, sync_min, freq_hint, max_cand, grid)
}
pub fn spectra_crop_for<P: Protocol>(
freq_min: f32,
freq_max: f32,
grid: RxGrid,
) -> Option<(usize, usize)> {
let d = SyncDims::of::<P>(grid.sample_rate_hz);
let ntones = P::NTONES as usize;
let usable = grid.usable_bins(&d);
let ia = grid.bin_of(&d, freq_min);
let headroom = d.nfos * (ntones - 1) + 1;
let ib = grid
.bin_of(&d, freq_max)
.min(usable.saturating_sub(headroom));
if ib < ia {
return None;
}
Some((ia, (ib + headroom).min(usable.saturating_sub(1))))
}
const MAX_SYNC_BLOCKS: usize = 8;
#[derive(Copy, Clone, Debug)]
pub struct Sync2dShape {
pub d: SyncDims,
pub ia: usize,
pub n_freq: usize,
pub n_lag: usize,
usable: usize,
}
impl Sync2dShape {
pub fn len(&self) -> usize {
self.n_freq * self.n_lag
}
pub fn is_empty(&self) -> bool {
self.len() == 0
}
}
pub fn sync2d_shape<P: Protocol>(
freq_min: f32,
freq_max: f32,
grid: RxGrid,
) -> Option<Sync2dShape> {
let d = SyncDims::of::<P>(grid.sample_rate_hz);
let ntones = P::NTONES as usize;
let usable = grid.usable_bins(&d);
let ia = grid.bin_of(&d, freq_min);
let headroom = d.nfos * (ntones - 1) + 1;
let ib = grid
.bin_of(&d, freq_max)
.min(usable.saturating_sub(headroom));
if ib < ia {
return None;
}
Some(Sync2dShape {
d,
ia,
n_freq: ib - ia + 1,
n_lag: (2 * d.jz + 1) as usize,
usable,
})
}
#[inline]
pub fn fill_sync2d_row<P: Protocol>(
s: &Spectrogram,
shape: &Sync2dShape,
fi: usize,
row: &mut [f32],
) {
debug_assert_eq!(row.len(), shape.n_lag);
let d = &shape.d;
let ntones = P::NTONES as usize;
let usable = shape.usable;
let num_blocks = P::SYNC_MODE.blocks().len();
debug_assert!(
num_blocks <= MAX_SYNC_BLOCKS,
"protocol has more sync blocks than MAX_SYNC_BLOCKS accounts for"
);
let i = shape.ia + fi;
let mut t_blocks = [0.0f32; MAX_SYNC_BLOCKS];
let mut t0_blocks = [0.0f32; MAX_SYNC_BLOCKS];
for (jlag, lag) in (-d.jz..=d.jz).enumerate() {
t_blocks[..num_blocks].fill(0.0);
t0_blocks[..num_blocks].fill(0.0);
for (bk, block) in P::SYNC_MODE.blocks().iter().enumerate() {
let block_offset = d.nssy as i32 * block.start_symbol as i32;
for (n, &costas_n) in block.pattern.iter().enumerate() {
let m = lag + d.jstrt + block_offset + (d.nssy * n) as i32;
let tone_bin = i + d.nfos * costas_n as usize;
if m >= 0 && (m as usize) < d.nhsym && tone_bin < usable {
let m = m as usize;
t_blocks[bk] += s.get(tone_bin, m);
t0_blocks[bk] += (0..ntones)
.map(|k| s.get((i + d.nfos * k).min(usable - 1), m))
.sum::<f32>();
}
}
}
let t_all: f32 = t_blocks[..num_blocks].iter().sum();
let t0_all: f32 = t0_blocks[..num_blocks].iter().sum();
let t0_ref = (t0_all - t_all) / (ntones as f32 - 1.0);
let sync_all = if t0_ref > f32::EPSILON {
t_all / t0_ref
} else if t_all > 0.0 {
t_all
} else {
0.0
};
let score = if num_blocks > 1 {
let t_tail: f32 = t_blocks[1..num_blocks].iter().sum();
let t0_tail: f32 = t0_blocks[1..num_blocks].iter().sum();
let t0_tail_ref = (t0_tail - t_tail) / (ntones as f32 - 1.0);
let sync_tail = if t0_tail_ref > f32::EPSILON {
t_tail / t0_tail_ref
} else if t_tail > 0.0 {
t_tail
} else {
0.0
};
sync_all.max(sync_tail)
} else {
sync_all
};
row[jlag] = score;
}
}
#[allow(clippy::too_many_arguments)]
pub fn coarse_sync_from_spectra<P: Protocol>(
s: &Spectrogram,
freq_min: f32,
freq_max: f32,
sync_min: f32,
freq_hint: Option<f32>,
max_cand: usize,
grid: RxGrid,
) -> Vec<SyncCandidate> {
let Some(shape) = sync2d_shape::<P>(freq_min, freq_max, grid) else {
return Vec::new();
};
let mut sync2d = alloc::vec![0.0f32; shape.len()];
#[cfg(feature = "parallel")]
sync2d
.par_chunks_mut(shape.n_lag)
.enumerate()
.for_each(|(fi, row)| fill_sync2d_row::<P>(s, &shape, fi, row));
#[cfg(not(feature = "parallel"))]
for (fi, row) in sync2d.chunks_mut(shape.n_lag).enumerate() {
fill_sync2d_row::<P>(s, &shape, fi, row);
}
coarse_sync_from_sync2d::<P>(s, &sync2d, &shape, sync_min, freq_hint, max_cand, grid)
}
#[allow(clippy::too_many_arguments)]
pub fn coarse_sync_from_sync2d<P: Protocol>(
s: &Spectrogram,
sync2d: &[f32],
shape: &Sync2dShape,
sync_min: f32,
freq_hint: Option<f32>,
max_cand: usize,
grid: RxGrid,
) -> Vec<SyncCandidate> {
debug_assert_eq!(sync2d.len(), shape.len());
let d = shape.d;
let ntones = P::NTONES as usize;
let usable = shape.usable;
let ia = shape.ia;
let n_freq = shape.n_freq;
let n_lag = shape.n_lag;
let idx = |fi: usize, lag: i32| fi * n_lag + (lag + d.jz) as usize;
const MLAG: i32 = 10;
let mut red = vec![0.0f32; n_freq];
#[cfg(feature = "parallel")]
red.par_iter_mut().enumerate().for_each(|(fi, r)| {
*r = (-d.jz..=d.jz)
.map(|lag| sync2d[idx(fi, lag)])
.fold(0.0f32, f32::max);
});
#[cfg(not(feature = "parallel"))]
for fi in 0..n_freq {
red[fi] = (-d.jz..=d.jz)
.map(|lag| sync2d[idx(fi, lag)])
.fold(0.0f32, f32::max);
}
let pct = |xs: &[f32]| {
let mut sorted = xs.to_vec();
sorted.sort_by(f32::total_cmp);
let pct_idx = (0.40 * n_freq as f32) as usize;
sorted[pct_idx.min(n_freq - 1)].max(f32::EPSILON)
};
let global_base = pct(&red);
let sbase: Vec<f32> = vec![global_base; n_freq];
let stage1_norm: Vec<f32> = if P::ID == super::ProtocolId::Fst4 {
let avg_power = s.avg_power_per_bin();
let ccf: Vec<f32> = (0..n_freq)
.map(|fi| {
let i = ia + fi;
(0..ntones)
.map(|k| {
let abs_bin = (i + d.nfos * k).min(usable - 1);
avg_power[(abs_bin - s.freq_offset()).min(avg_power.len() - 1)]
})
.sum()
})
.collect();
let stage1_base = pct(&ccf);
ccf.iter().map(|&c| c / stage1_base).collect()
} else {
Vec::new()
};
let stage1_pass = |fi: usize| stage1_norm.get(fi).copied().unwrap_or(0.0) >= sync_min;
let fi_cands = |fi: usize| -> Vec<SyncCandidate> {
let i = ia + fi;
let freq_hz = grid.hz_of(&d, i);
let local_base = sbase[fi];
let bin_stage1_pass = stage1_pass(fi);
let mut peaks: Vec<(i32, f32)> = (-d.jz..=d.jz)
.filter_map(|lag| {
let raw = sync2d[idx(fi, lag)];
let norm = raw / local_base;
if norm.is_finite() && (norm >= sync_min || bin_stage1_pass) {
Some((lag, norm))
} else {
None
}
})
.collect();
peaks.sort_by(|a, b| b.1.partial_cmp(&a.1).unwrap());
let mut picked: Vec<i32> = Vec::new();
let mut out = Vec::new();
'outer: for (lag, score) in peaks {
for &pl in &picked {
if (lag - pl).abs() <= MLAG {
continue 'outer;
}
}
picked.push(lag);
out.push(SyncCandidate {
freq_hz,
dt_sec: (lag as f32 - 0.5) * d.tstep,
score,
});
if picked.len() >= 8 {
break;
}
}
out
};
#[cfg(feature = "parallel")]
let mut cands: Vec<SyncCandidate> = (0..n_freq)
.into_par_iter()
.flat_map_iter(fi_cands)
.collect();
#[cfg(not(feature = "parallel"))]
let mut cands: Vec<SyncCandidate> = (0..n_freq).flat_map(fi_cands).collect();
let suppressed = dedup_suppress(&mut cands);
let mut keep = suppressed.iter().map(|s| !s);
cands.retain(|c| {
if !keep.next().unwrap_or(true) {
return false;
}
if c.score >= sync_min {
return true;
}
let fi = ((c.freq_hz / d.df).round() as usize).saturating_sub(ia);
stage1_pass(fi)
});
rank_candidates(cands, freq_hint, max_cand)
}
const DEDUP_HZ: f32 = 4.0;
const DEDUP_SEC: f32 = 0.04;
fn dedup_suppress(cands: &mut [SyncCandidate]) -> Vec<bool> {
let mut suppressed = alloc::vec![false; cands.len()];
let mut lo = 0usize;
for i in 1..cands.len() {
debug_assert!(
cands[i].freq_hz >= cands[i - 1].freq_hz,
"dedup window assumes frequency-sorted candidates"
);
while cands[i].freq_hz - cands[lo].freq_hz >= DEDUP_HZ {
lo += 1;
}
for j in lo..i {
let fdiff = (cands[i].freq_hz - cands[j].freq_hz).abs();
let tdiff = (cands[i].dt_sec - cands[j].dt_sec).abs();
if fdiff < DEDUP_HZ && tdiff < DEDUP_SEC {
if cands[i].score >= cands[j].score {
cands[j].score = 0.0;
suppressed[j] = true;
} else {
cands[i].score = 0.0;
suppressed[i] = true;
}
}
}
}
suppressed
}
pub(crate) const FREQ_HINT_NEAR_HZ: f32 = 10.0;
pub(crate) fn rank_candidates(
mut cands: Vec<SyncCandidate>,
freq_hint: Option<f32>,
max_cand: usize,
) -> Vec<SyncCandidate> {
cands.sort_by(|a, b| {
b.score
.partial_cmp(&a.score)
.unwrap_or(core::cmp::Ordering::Equal)
});
let Some(fhint) = freq_hint else {
cands.truncate(max_cand);
return cands;
};
let (near, far): (Vec<SyncCandidate>, Vec<SyncCandidate>) = cands
.into_iter()
.partition(|c| (c.freq_hz - fhint).abs() <= FREQ_HINT_NEAR_HZ);
let reserved = near.len().min(max_cand.div_ceil(2));
let mut out = Vec::with_capacity(max_cand.min(near.len() + far.len()));
out.extend_from_slice(&near[..reserved]);
out.extend(far);
out.extend_from_slice(&near[reserved..]);
out.truncate(max_cand);
out
}
#[cfg(feature = "std")]
type CostasRefCacheEntry = (&'static [u8], usize, Vec<Vec<Complex<f32>>>);
#[cfg(feature = "std")]
std::thread_local! {
static COSTAS_REF_CACHE: core::cell::RefCell<Vec<CostasRefCacheEntry>> =
const { core::cell::RefCell::new(Vec::new()) };
}
#[cfg(feature = "std")]
fn cached_costas_ref(pattern: &'static [u8], ds_spb: usize) -> Vec<Vec<Complex<f32>>> {
COSTAS_REF_CACHE.with_borrow_mut(|cache| {
if let Some((_, _, csync)) = cache.iter().find(|(p, d, _)| *p == pattern && *d == ds_spb) {
return csync.clone();
}
let csync = make_costas_ref(pattern, ds_spb);
if cache.len() >= 2 {
cache.remove(0);
}
cache.push((pattern, ds_spb, csync.clone()));
csync
})
}
#[cfg(not(feature = "std"))]
fn cached_costas_ref(pattern: &'static [u8], ds_spb: usize) -> Vec<Vec<Complex<f32>>> {
make_costas_ref(pattern, ds_spb)
}
pub fn make_costas_ref(pattern: &[u8], ds_spb: usize) -> Vec<Vec<Complex<f32>>> {
pattern
.iter()
.map(|&tone| {
let dphi = 2.0 * PI * tone as f32 / ds_spb as f32;
let mut waves = vec![Complex::new(0.0f32, 0.0); ds_spb];
let mut phi = 0.0f32;
for w in waves.iter_mut() {
*w = Complex::new(phi.cos(), phi.sin());
phi = (phi + dphi) % (2.0 * PI);
}
waves
})
.collect()
}
pub fn score_costas_block(
cd0: &[Complex<f32>],
csync: &[Vec<Complex<f32>>],
ds_spb: usize,
array_start: i32,
) -> f32 {
let np2 = cd0.len() as i32;
csync
.iter()
.enumerate()
.map(|(k, ref_tone)| {
let start = array_start + (k * ds_spb) as i32;
if start >= 0 && start + ds_spb as i32 <= np2 {
let s0 = start as usize;
cd0[s0..s0 + ds_spb]
.iter()
.zip(ref_tone.iter())
.map(|(&s, &r)| s * r.conj())
.sum::<Complex<f32>>()
.norm_sqr()
} else {
0.0
}
})
.sum()
}
pub fn fine_sync_power<P: Protocol>(cd0: &[Complex<f32>], i0: i32) -> f32 {
fine_sync_power_per_block::<P>(cd0, i0).into_iter().sum()
}
pub fn fine_sync_power_per_block<P: Protocol>(cd0: &[Complex<f32>], i0: i32) -> Vec<f32> {
type CachedCsync = (&'static [u8], Vec<Vec<Complex<f32>>>);
let d = SyncDims::of::<P>(12_000.0);
let blocks = P::SYNC_MODE.blocks();
let mut out = Vec::with_capacity(blocks.len());
let mut last: Option<CachedCsync> = None;
for block in blocks {
let csync = match &last {
Some((p, c)) if *p == block.pattern => c,
_ => {
last = Some((block.pattern, cached_costas_ref(block.pattern, d.ds_spb)));
&last.as_ref().unwrap().1
}
};
let start = i0 + (block.start_symbol as usize * d.ds_spb) as i32;
out.push(score_costas_block(cd0, csync, d.ds_spb, start));
}
out
}
pub fn parabolic_peak(y_neg: f32, y_0: f32, y_pos: f32) -> (f32, f32) {
let denom = y_neg - 2.0 * y_0 + y_pos;
if denom.abs() < f32::EPSILON {
return (0.0, y_0);
}
let offset = 0.5 * (y_neg - y_pos) / denom;
let peak = y_0 - 0.25 * (y_neg - y_pos) * offset;
(offset.clamp(-0.5, 0.5), peak)
}
pub fn refine_freq_hz_log_power(
base_bin: usize,
bin_count: usize,
df: f32,
power_at: impl Fn(usize) -> f32,
) -> f32 {
if base_bin == 0 || base_bin + 1 >= bin_count {
return base_bin as f32 * df;
}
let y_lo = power_at(base_bin - 1).max(1e-12).ln();
let y_mid = power_at(base_bin).max(1e-12).ln();
let y_hi = power_at(base_bin + 1).max(1e-12).ln();
let denom = y_lo - 2.0 * y_mid + y_hi;
let delta = if denom < -1e-9 {
(0.5 * (y_lo - y_hi) / denom).clamp(-0.5, 0.5)
} else {
0.0
};
(base_bin as f32 + delta) * df
}
pub fn refine_candidate<P: Protocol>(
cd0: &[Complex<f32>],
candidate: &SyncCandidate,
search_steps: i32,
) -> SyncCandidate {
let d = SyncDims::of::<P>(12_000.0);
let nominal_i0 = ((candidate.dt_sec + P::TX_START_OFFSET_S) * d.ds_rate).round() as i32;
let (best_i0, best_score) = (-search_steps..=search_steps)
.map(|delta| {
let i0 = nominal_i0 + delta;
let score = fine_sync_power::<P>(cd0, i0);
(i0, score)
})
.max_by(|a, b| a.1.partial_cmp(&b.1).unwrap())
.unwrap_or((nominal_i0, 0.0));
let y_neg = fine_sync_power::<P>(cd0, best_i0 - 1);
let y_pos = fine_sync_power::<P>(cd0, best_i0 + 1);
let (frac, _) = parabolic_peak(y_neg, best_score, y_pos);
SyncCandidate {
freq_hz: candidate.freq_hz,
dt_sec: (best_i0 as f32 + frac) / d.ds_rate - P::TX_START_OFFSET_S,
score: best_score,
}
}
#[cfg(all(test, feature = "fst4", feature = "ft4"))]
mod tests {
use super::{
AudioSource, DEDUP_HZ, DEDUP_SEC, PI, Protocol, RxGrid, SyncCandidate, SyncDims,
compute_spectra, dedup_suppress,
};
use crate::engine::protocol::ModulationParams;
use crate::fst4::{Fst4s15, Fst4s30, Fst4s60, Fst4s120, Fst4s300};
use crate::ft4::Ft4;
use alloc::vec::Vec;
#[test]
fn sync_dims_of_matches_nsps_at_12khz() {
fn check<P: Protocol + ModulationParams>(name: &str) {
let d = SyncDims::of::<P>(12_000.0);
assert_eq!(
d.nsps,
P::NSPS as usize,
"{name}: SyncDims::of(12_000.0).nsps != P::NSPS"
);
}
check::<Fst4s15>("Fst4s15");
check::<Fst4s30>("Fst4s30");
check::<Fst4s60>("Fst4s60");
check::<Fst4s120>("Fst4s120");
check::<Fst4s300>("Fst4s300");
check::<Ft4>("Ft4");
}
#[test]
fn rx_grid_real_matches_pre_rxgrid_bin_math() {
let d = SyncDims::of::<Fst4s60>(12_000.0);
let grid = RxGrid::real(12_000.0);
for hz in [0.0f32, 100.0, 1500.0, 2963.34, 3000.0] {
let want = (hz / d.df).round() as usize;
assert_eq!(grid.bin_of(&d, hz), want, "hz={hz}");
}
assert_eq!(grid.usable_bins(&d), d.nh1);
}
#[test]
fn rx_grid_complex_bin_of_predicts_compute_spectra_peak() {
let sample_rate_hz = 12_000.0f32;
let center_hz = 1500.0f32;
let d = SyncDims::of::<Fst4s60>(sample_rate_hz);
let grid = RxGrid::complex(sample_rate_hz, center_hz);
for offset_hz in [200.0f32, -200.0] {
let f_signal = center_hz + offset_hz;
let f_baseband = offset_hz; let n = d.nmax;
let w = 2.0 * PI * f_baseband / sample_rate_hz;
let audio_i: Vec<f32> = (0..n).map(|k| (w * k as f32).cos()).collect();
let audio_q: Vec<f32> = (0..n).map(|k| (w * k as f32).sin()).collect();
let s = compute_spectra::<Fst4s60>(
AudioSource::Complex(&audio_i, &audio_q),
0,
d.nfft1 - 1,
grid,
);
let avg = s.avg_power_per_bin();
let (peak_bin, _) = avg
.iter()
.enumerate()
.max_by(|a, b| a.1.partial_cmp(b.1).unwrap())
.unwrap();
let want = grid.bin_of(&d, f_signal);
assert!(
peak_bin.abs_diff(want) <= 1,
"offset={offset_hz}: peak_bin={peak_bin}, RxGrid predicted {want}"
);
}
}
#[test]
fn dedup_suppress_matches_all_pairs() {
fn reference(cands: &mut [SyncCandidate]) -> Vec<bool> {
let mut suppressed = alloc::vec![false; cands.len()];
for i in 1..cands.len() {
for j in 0..i {
let fdiff = (cands[i].freq_hz - cands[j].freq_hz).abs();
let tdiff = (cands[i].dt_sec - cands[j].dt_sec).abs();
if fdiff < DEDUP_HZ && tdiff < DEDUP_SEC {
if cands[i].score >= cands[j].score {
cands[j].score = 0.0;
suppressed[j] = true;
} else {
cands[i].score = 0.0;
suppressed[i] = true;
}
}
}
}
suppressed
}
let mut state = 0x1234_5678u32;
let mut next = move || {
state = state.wrapping_mul(1_103_515_245).wrapping_add(12_345);
(state >> 16) as f32 / 65_536.0
};
for df in [0.5f32, 1.54, 4.0, 13.0] {
let mut cands: Vec<SyncCandidate> = Vec::new();
for bin in 0..40 {
let freq_hz = 100.0 + bin as f32 * df;
for _ in 0..(1 + (next() * 8.0) as usize) {
cands.push(SyncCandidate {
freq_hz,
dt_sec: (next() * 0.4 - 0.2 + (next() * 4.0).floor() * 0.01),
score: (next() * 5.0).floor(),
});
}
}
let mut want = cands.clone();
let want_flags = reference(&mut want);
let mut got = cands.clone();
let got_flags = dedup_suppress(&mut got);
assert_eq!(want_flags, got_flags, "df={df}: suppression flags differ");
for (i, (w, g)) in want.iter().zip(got.iter()).enumerate() {
assert_eq!(w.score, g.score, "df={df}: candidate {i} score differs");
}
}
}
}