use alloc::vec;
use alloc::vec::Vec;
#[cfg(not(feature = "std"))]
use num_traits::Float;
use super::super::params::{COSTAS, COSTAS_POS, NTONES};
use super::spectrogram::{CoarseAcc, Spectrogram};
use super::types::{
NFFT_SPEC, NSSY, NSTEP, SAMPLE_RATE_HZ, TONE_SPACING_HZ, TX_START_OFFSET_S, ratio_eps,
sync_lag_s,
};
use crate::core::sync::SyncCandidate;
pub fn coarse_sync(
spec: &Spectrogram,
freq_min: f32,
freq_max: f32,
sync_min: f32,
max_cand: usize,
) -> Vec<SyncCandidate> {
coarse_sync_inner(spec, freq_min, freq_max, sync_min, max_cand, None)
}
pub fn coarse_sync_with_allsum(
spec: &Spectrogram,
freq_min: f32,
freq_max: f32,
sync_min: f32,
max_cand: usize,
allsum: &[CoarseAcc],
) -> Vec<SyncCandidate> {
coarse_sync_inner(spec, freq_min, freq_max, sync_min, max_cand, Some(allsum))
}
pub fn coarse_allsum_len(
spec_n_freq: usize,
spec_n_time: usize,
freq_min: f32,
freq_max: f32,
) -> usize {
let df = SAMPLE_RATE_HZ / NFFT_SPEC as f32;
let tone_step_bins = TONE_SPACING_HZ / df;
let ia = (freq_min / df).round() as usize;
let max_tone_off = ((NTONES - 1) as f32 * tone_step_bins).ceil() as usize + 1;
let ib_unbounded = (freq_max / df).round() as usize;
let ib = ib_unbounded.min(spec_n_freq.saturating_sub(max_tone_off));
if ib < ia {
return 0;
}
let n_freq = ib - ia + 1;
n_freq * spec_n_time
}
pub fn precompute_coarse_allsum(
spec: &Spectrogram,
freq_min: f32,
freq_max: f32,
) -> Vec<CoarseAcc> {
let mut buf =
vec![CoarseAcc::default(); coarse_allsum_len(spec.n_freq, spec.n_time, freq_min, freq_max)];
if !buf.is_empty() {
precompute_coarse_allsum_into(spec, freq_min, freq_max, &mut buf);
}
buf
}
pub fn precompute_coarse_allsum_into(
spec: &Spectrogram,
freq_min: f32,
freq_max: f32,
dst: &mut [CoarseAcc],
) {
let df = SAMPLE_RATE_HZ / NFFT_SPEC as f32;
let tone_step_bins = TONE_SPACING_HZ / df;
let ia = (freq_min / df).round() as usize;
let max_tone_off = ((NTONES - 1) as f32 * tone_step_bins).ceil() as usize + 1;
let nh1 = spec.n_freq;
let ib_unbounded = (freq_max / df).round() as usize;
let ib = ib_unbounded.min(nh1.saturating_sub(max_tone_off));
if ib < ia {
return;
}
let n_freq = ib - ia + 1;
debug_assert_eq!(
dst.len(),
n_freq * spec.n_time,
"allsum buffer length mismatch"
);
fill_coarse_allsum(spec, ia, ib, n_freq, dst);
}
fn fill_allsum_row(spec: &Spectrogram, ia: usize, n_freq: usize, m: usize, dst: &mut [CoarseAcc]) {
if n_freq == 0 {
return;
}
let nh1 = spec.n_freq;
let upper = nh1 - 1;
let n_time = spec.n_time;
#[cfg(feature = "fixed-point")]
{
let mut prev = [CoarseAcc::default(); 2];
for parity in 0..2usize {
if parity >= n_freq {
break;
}
let i_carrier = ia + parity;
let mut s = CoarseAcc::default();
for k in 0..(NTONES - 1) {
let bin = (i_carrier + 2 * k).min(upper);
s += spec.power_acc(bin, m);
}
dst[parity * n_time + m] = s;
prev[parity] = s;
}
for fi in 2..n_freq {
let i_carrier = ia + fi;
let drop = (i_carrier - 2).min(upper);
let add = (i_carrier + 2 * (NTONES - 2)).min(upper);
let p = fi & 1;
#[allow(clippy::unnecessary_cast)]
let s = prev[p] + spec.power_acc(add, m) - spec.power_acc(drop, m);
dst[fi * n_time + m] = s;
prev[p] = s;
}
}
#[cfg(not(feature = "fixed-point"))]
{
for fi in 0..n_freq {
let i_carrier = ia + fi;
let mut s = CoarseAcc::default();
for k in 0..(NTONES - 1) {
let bin = (i_carrier + 2 * k).min(upper);
s += spec.power_acc(bin, m);
}
dst[fi * n_time + m] = s;
}
}
}
fn fill_coarse_allsum(
spec: &Spectrogram,
ia: usize,
ib: usize,
_n_freq: usize,
dst: &mut [CoarseAcc],
) {
let n_freq = ib - ia + 1;
for m in 0..spec.n_time {
fill_allsum_row(spec, ia, n_freq, m, dst);
}
}
fn coarse_sync_inner(
spec: &Spectrogram,
freq_min: f32,
freq_max: f32,
sync_min: f32,
max_cand: usize,
external_allsum: Option<&[CoarseAcc]>,
) -> Vec<SyncCandidate> {
let df = SAMPLE_RATE_HZ / NFFT_SPEC as f32;
let tstep = NSTEP as f32 / SAMPLE_RATE_HZ;
let jstrt = (TX_START_OFFSET_S / tstep).round() as i32;
let jz = (sync_lag_s() / tstep).round() as i32;
let tone_step_bins = TONE_SPACING_HZ / df;
let ia = (freq_min / df).round() as usize;
let max_tone_off = ((NTONES - 1) as f32 * tone_step_bins).ceil() as usize + 1;
let nh1 = spec.n_freq;
let ib_unbounded = (freq_max / df).round() as usize;
let ib = ib_unbounded.min(nh1.saturating_sub(max_tone_off));
if ib < ia {
return Vec::new();
}
let n_freq = ib - ia + 1;
let n_lag = (2 * jz + 1) as usize;
let mut sync2d = vec![0.0f32; n_freq * n_lag];
let idx = |fi: usize, lag: i32| fi * n_lag + (lag + jz) as usize;
let ratio_eps = ratio_eps();
#[cfg(all(
feature = "profile-coarse",
not(all(target_arch = "wasm32", target_os = "unknown"))
))]
let t_setup = std::time::Instant::now();
let mut tone_bin_lo = [0usize; NTONES];
for k in 0..NTONES {
tone_bin_lo[k] = (k as f32 * tone_step_bins).floor() as usize;
}
let m_base: [[i32; COSTAS.len()]; COSTAS_POS.len()] = {
let mut t = [[0i32; COSTAS.len()]; COSTAS_POS.len()];
for (bk, &start_sym) in COSTAS_POS.iter().enumerate() {
let block_offset = NSSY * start_sym as i32;
for (n, _) in COSTAS.iter().enumerate() {
t[bk][n] = jstrt + block_offset + NSSY * n as i32;
}
}
t
};
let costas_off: [usize; COSTAS.len()] = {
let mut t = [0usize; COSTAS.len()];
for (n, &costas_n) in COSTAS.iter().enumerate() {
t[n] = tone_bin_lo[costas_n];
}
t
};
let needed_m: alloc::vec::Vec<usize> = {
let mut mark = alloc::vec![false; spec.n_time];
for bk in 0..COSTAS_POS.len() {
let lo = m_base[bk][0] - jz;
let hi = m_base[bk][COSTAS.len() - 1] + jz;
let lo_u = lo.max(0) as usize;
let hi_u = (hi.min(spec.n_time as i32 - 1)) as usize;
if lo_u <= hi_u {
#[allow(clippy::needless_range_loop)]
for m in lo_u..=hi_u {
mark[m] = true;
}
}
}
(0..spec.n_time).filter(|&m| mark[m]).collect()
};
let owned_allsum: Vec<CoarseAcc>;
let allsum: &[CoarseAcc] = if let Some(ext) = external_allsum {
debug_assert_eq!(
ext.len(),
n_freq * spec.n_time,
"external allsum length mismatch (expected n_freq * spec.n_time)"
);
ext
} else {
owned_allsum = {
let mut buf = vec![CoarseAcc::default(); n_freq * spec.n_time];
for &m in &needed_m {
fill_allsum_row(spec, ia, n_freq, m, &mut buf);
}
buf
};
&owned_allsum
};
#[cfg(all(
feature = "profile-coarse",
not(all(target_arch = "wasm32", target_os = "unknown"))
))]
let t_allsum = std::time::Instant::now();
debug_assert!(
m_base[2][COSTAS.len() - 1] + jz < spec.n_time as i32,
"n_time too small for SYNC_LAG_S/jstrt"
);
let n_time = spec.n_time;
let mut tbin_lo_arr = [0usize; COSTAS.len()];
for (fi, i_carrier) in (ia..=ib).enumerate() {
for n in 0..COSTAS.len() {
tbin_lo_arr[n] = i_carrier + costas_off[n];
}
let allsum_row = &allsum[fi * n_time..(fi + 1) * n_time];
for lag in -jz..=jz {
let mut t_blocks: [CoarseAcc; 3] = [CoarseAcc::default(); 3];
let mut t0_blocks: [CoarseAcc; 3] = [CoarseAcc::default(); 3];
let bk0_n_start = {
let needed = -jstrt - lag;
if needed <= 0 {
0usize
} else {
((needed + NSSY - 1) / NSSY).min(COSTAS.len() as i32) as usize
}
};
for n in bk0_n_start..COSTAS.len() {
let m_u = (m_base[0][n] + lag) as usize;
let tbin_lo = tbin_lo_arr[n];
t_blocks[0] += spec.power_acc(tbin_lo, m_u);
t0_blocks[0] += allsum_row[m_u];
}
for bk in 1..COSTAS_POS.len() {
for n in 0..COSTAS.len() {
let m_u = (m_base[bk][n] + lag) as usize;
let tbin_lo = tbin_lo_arr[n];
t_blocks[bk] += spec.power_acc(tbin_lo, m_u);
t0_blocks[bk] += allsum_row[m_u];
}
}
let t_all: CoarseAcc = t_blocks[0] + t_blocks[1] + t_blocks[2];
let t0_all: CoarseAcc = t0_blocks[0] + t0_blocks[1] + t0_blocks[2];
#[allow(clippy::unnecessary_cast)]
let t_all_f = t_all as f32;
#[allow(clippy::unnecessary_cast)]
let t0_all_f = t0_all as f32;
let t0_ref = (t0_all_f - t_all_f) / (NTONES as f32 - 2.0);
let sync_all = t_all_f / (t0_ref + ratio_eps);
#[allow(clippy::unnecessary_cast)]
let t_tail_f = (t_blocks[1] + t_blocks[2]) as f32;
#[allow(clippy::unnecessary_cast)]
let t0_tail_f = (t0_blocks[1] + t0_blocks[2]) as f32;
let t0_tail_ref = (t0_tail_f - t_tail_f) / (NTONES as f32 - 2.0);
let sync_tail = t_tail_f / (t0_tail_ref + ratio_eps);
sync2d[idx(fi, lag)] = sync_all.max(sync_tail);
}
}
#[cfg(all(
feature = "profile-coarse",
not(all(target_arch = "wasm32", target_os = "unknown"))
))]
let t_score = std::time::Instant::now();
const MLAG: i32 = 10;
let mut red = vec![0.0f32; n_freq];
for fi in 0..n_freq {
red[fi] = (-jz..=jz)
.map(|lag| sync2d[idx(fi, lag)])
.fold(0.0f32, f32::max);
}
let base = {
let mut sorted = red.clone();
let pct_idx = ((0.40 * n_freq as f32) as usize).min(n_freq - 1);
sorted.select_nth_unstable_by(pct_idx, |a, b| {
a.partial_cmp(b).unwrap_or(core::cmp::Ordering::Equal)
});
sorted[pct_idx].max(f32::EPSILON)
};
let mut cands: Vec<SyncCandidate> = Vec::new();
for fi in 0..n_freq {
let i_carrier = ia + fi;
let freq_hz = i_carrier as f32 * df;
let mut peaks: Vec<(i32, f32)> = (-jz..=jz)
.filter_map(|lag| {
let raw = sync2d[idx(fi, lag)];
let norm = raw / base;
if norm.is_finite() && norm >= sync_min {
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();
for (lag, score) in peaks {
if picked.iter().any(|&pl| (lag - pl).abs() <= MLAG) {
continue;
}
picked.push(lag);
let dt_quanta = if lag > -jz && lag < jz {
let y_lo = sync2d[idx(fi, lag - 1)];
let y_mi = sync2d[idx(fi, lag)];
let y_hi = sync2d[idx(fi, lag + 1)];
let denom = y_lo - 2.0 * y_mi + y_hi;
if denom.abs() > f32::EPSILON {
let off = 0.5 * (y_lo - y_hi) / denom;
off.clamp(-1.0, 1.0)
} else {
0.0
}
} else {
0.0
};
let dt_lag = lag as f32 + dt_quanta;
cands.push(SyncCandidate {
freq_hz,
dt_sec: (dt_lag - 0.5) * tstep,
score,
});
if picked.len() >= 2 {
break;
}
}
}
cands.sort_by(|a, b| b.score.partial_cmp(&a.score).unwrap());
let mut out: Vec<SyncCandidate> = Vec::with_capacity(max_cand);
for c in cands {
if c.score < sync_min {
break;
}
let near = out
.iter()
.any(|k| (c.freq_hz - k.freq_hz).abs() < 4.0 && (c.dt_sec - k.dt_sec).abs() < 0.04);
if near {
continue;
}
out.push(c);
if out.len() >= max_cand {
break;
}
}
let cands = out;
#[cfg(all(
feature = "profile-coarse",
not(all(target_arch = "wasm32", target_os = "unknown"))
))]
{
let t_end = std::time::Instant::now();
let allsum_us = (t_allsum - t_setup).as_micros();
let score_us = (t_score - t_allsum).as_micros();
let post_us = (t_end - t_score).as_micros();
let total_us = (t_end - t_setup).as_micros();
eprintln!(
"[coarse_sync prof] n_freq={n_freq} n_lag={n_lag} allsum={allsum_us} score={score_us} dedupe+sort={post_us} total={total_us} us"
);
}
cands
}