use alloc::vec;
use alloc::vec::Vec;
use num_complex::Complex;
#[cfg(not(feature = "std"))]
use num_traits::Float;
use super::Protocol;
use crate::engine::fft::default_planner;
use crate::engine::scalar::{Cmplx, ComplexSpec, LlrScalar, SpecScalar};
#[derive(Clone)]
pub struct LlrSet<T: LlrScalar = f32> {
pub llra: Vec<T>,
pub llrb: Vec<T>,
pub llrc: Vec<T>,
pub llrd: Vec<T>,
pub llre: Vec<T>,
}
pub const LLR_SCALE: f32 = 2.83;
#[inline]
pub fn descramble_info<P: super::Protocol>(info: &mut [u8]) {
if let Some(rvec) = <P as super::ModulationParams>::INFO_SCRAMBLE_RVEC {
let n = rvec.len().min(info.len());
for (b, &r) in info[..n].iter_mut().zip(rvec.iter()) {
*b = (*b ^ r) & 1;
}
}
}
pub fn symbol_spectra<P: Protocol>(cd0: &[Complex<f32>], i_start: i32) -> Vec<Cmplx<f32>> {
let ntones = P::NTONES as usize;
let n_sym = P::N_SYMBOLS as usize;
let ds_spb = (P::NSPS / P::NDOWN) as usize;
let mut planner = default_planner();
let fft = planner.plan_forward(ds_spb);
let mut cs: Vec<Cmplx<f32>> = vec![Cmplx::new(0.0f32, 0.0); n_sym * ntones];
let mut buf = vec![Complex::new(0.0f32, 0.0); ds_spb];
let np2 = cd0.len() as i32;
for k in 0..n_sym {
let i1 = i_start + (k * ds_spb) as i32;
if i1 >= 0 && i1 + ds_spb as i32 <= np2 {
for (j, b) in buf.iter_mut().enumerate() {
*b = cd0[(i1 as usize) + j];
}
} else {
for b in buf.iter_mut() {
*b = Complex::new(0.0, 0.0);
}
}
fft.process(&mut buf);
for (t, bin) in buf.iter().take(ntones).enumerate() {
cs[k * ntones + t] = *bin / 1000.0;
}
}
cs
}
use crate::engine::tx::data_chunks;
#[inline]
fn normalize_bmet(bmet: &mut [f32]) {
let n = bmet.len() as f32;
let mean = bmet.iter().sum::<f32>() / n;
let mean_sq = bmet.iter().map(|&x| x * x).sum::<f32>() / n;
let var = mean_sq - mean * mean;
let sig = if var > 0.0 {
var.sqrt()
} else {
mean_sq.sqrt()
};
if sig > 0.0 {
bmet.iter_mut().for_each(|x| *x /= sig);
}
}
pub fn compute_llr<P: Protocol, T: LlrScalar>(cs: &[Cmplx<f32>]) -> LlrSet<T> {
let mut set = compute_llr_generic::<P, f32, T>(cs, P::LLR_NSYM_MAX as usize);
if let Some(mid) = P::LLR_NSYM_MID {
let mut bmete = vec![0.0f32; codeword_bit_len::<P>()];
fill_bmet_for_nsym::<P, f32>(cs, mid as usize, &mut bmete, None);
set.llre = scale_bmet::<T>(bmete, P::LLR_SCALE);
}
set
}
pub fn compute_llr_fast<P: Protocol, T: LlrScalar>(cs: &[Cmplx<f32>]) -> LlrSet<T> {
compute_llr_generic::<P, f32, T>(cs, 1)
}
const MAX_NSYM: usize = 16;
const MAX_IBMAX_PLUS_1: usize = 32;
fn build_group_amplitudes<S: SpecScalar>(
cs: &[Cmplx<S>],
ks: usize,
span: usize,
ntones: usize,
gray_map: &[u8],
) -> Vec<Complex<f32>> {
if span == 1 {
return (0..ntones)
.map(|t| {
let entry = cs[ks * ntones + gray_map[t] as usize];
Complex::new(entry.re.to_f32(), entry.im.to_f32())
})
.collect();
}
let hi = span / 2;
let lo = span - hi;
let half_hi = build_group_amplitudes::<S>(cs, ks, hi, ntones, gray_map);
let half_lo = build_group_amplitudes::<S>(cs, ks + hi, lo, ntones, gray_map);
let mut out = Vec::with_capacity(half_hi.len() * half_lo.len());
for &a in &half_hi {
for &b in &half_lo {
out.push(a + b);
}
}
out
}
fn fill_bmet_for_nsym<P: Protocol, S: SpecScalar>(
cs: &[Cmplx<S>],
nsym: usize,
bmet_primary: &mut [f32],
bmet_norm: Option<&mut [f32]>,
) {
let ntones = P::NTONES as usize;
let bps = P::BITS_PER_SYMBOL as usize;
let gray_map = P::GRAY_MAP;
let chunks = data_chunks::<P>();
let codeword_len = bmet_primary.len();
let nt = ntones.pow(nsym as u32);
let ibmax = bps * nsym - 1;
let mut s2 = vec![0.0f32; nt];
let mut bmet_norm_holder = bmet_norm;
let mut process_group =
|ks: usize,
i_bit_base: usize,
bmet_primary: &mut [f32],
bmet_norm_holder: &mut Option<&mut [f32]>| {
let table = build_group_amplitudes::<S>(cs, ks, nsym, ntones, gray_map);
for (s2_i, entry) in s2.iter_mut().zip(&table) {
*s2_i = (entry.re * entry.re + entry.im * entry.im).sqrt();
}
let mut max_one = [f32::NEG_INFINITY; MAX_IBMAX_PLUS_1];
let mut max_zero = [f32::NEG_INFINITY; MAX_IBMAX_PLUS_1];
for bit_sel in 0..=ibmax {
let mut mo = f32::NEG_INFINITY;
let mut mz = f32::NEG_INFINITY;
for (i, &v) in s2.iter().enumerate() {
let bit_is_one = (i >> bit_sel) & 1 == 1;
let v_for_one = if bit_is_one { v } else { f32::NEG_INFINITY };
let v_for_zero = if bit_is_one { f32::NEG_INFINITY } else { v };
mo = mo.max(v_for_one);
mz = mz.max(v_for_zero);
}
max_one[bit_sel] = mo;
max_zero[bit_sel] = mz;
}
for ib in 0..=ibmax {
let bit_idx = i_bit_base + ib;
if bit_idx >= codeword_len {
break;
}
let bit_sel = ibmax - ib;
let (max_one, max_zero) = (max_one[bit_sel], max_zero[bit_sel]);
let bm = max_one - max_zero;
bmet_primary[bit_idx] = bm;
if let Some(b) = bmet_norm_holder.as_deref_mut() {
let den = max_one.max(max_zero);
b[bit_idx] = if den > 0.0 { bm / den } else { 0.0 };
}
}
};
let mut chunk_bit_base = 0usize;
for &(chunk_start_sym, chunk_len) in &chunks {
let mut k = 0usize;
while k + nsym <= chunk_len {
let ks = chunk_start_sym + k;
let i_bit_base = chunk_bit_base + k * bps;
process_group(ks, i_bit_base, bmet_primary, &mut bmet_norm_holder);
k += nsym;
}
if k < chunk_len && chunk_len >= nsym {
let tail_k = chunk_len - nsym;
let ks = chunk_start_sym + tail_k;
let i_bit_base = chunk_bit_base + tail_k * bps;
process_group(ks, i_bit_base, bmet_primary, &mut bmet_norm_holder);
}
chunk_bit_base += chunk_len * bps;
}
}
#[inline]
fn scale_bmet<T: LlrScalar>(mut v: Vec<f32>, scale: f32) -> Vec<T> {
normalize_bmet(&mut v);
v.into_iter().map(|x| T::from_f32(x * scale)).collect()
}
#[inline]
fn codeword_bit_len<P: Protocol>() -> usize {
let bps = P::BITS_PER_SYMBOL as usize;
data_chunks::<P>().iter().map(|&(_, l)| l).sum::<usize>() * bps
}
pub fn compute_llr_generic<P: Protocol, S: SpecScalar, T: LlrScalar>(
cs: &[Cmplx<S>],
max_nsym: usize,
) -> LlrSet<T> {
let codeword_len = codeword_bit_len::<P>();
let mut bmeta = vec![0.0f32; codeword_len];
let mut bmetb = vec![0.0f32; codeword_len];
let mut bmetc = vec![0.0f32; codeword_len];
let mut bmetd = vec![0.0f32; codeword_len];
for nsym in 1usize..=max_nsym {
if nsym > 2 && nsym != max_nsym {
continue; }
let primary: &mut [f32] = match nsym {
1 => &mut bmeta,
2 => &mut bmetb,
n if n == max_nsym => &mut bmetc,
_ => unreachable!(),
};
if nsym == 1 {
let (bmeta_slice, bmetd_slice) = (&mut bmeta[..], &mut bmetd[..]);
fill_bmet_for_nsym::<P, S>(cs, 1, bmeta_slice, Some(bmetd_slice));
} else {
fill_bmet_for_nsym::<P, S>(cs, nsym, primary, None);
}
}
let s = P::LLR_SCALE;
LlrSet {
llra: scale_bmet::<T>(bmeta, s),
llrb: scale_bmet::<T>(bmetb, s),
llrc: scale_bmet::<T>(bmetc, s),
llrd: scale_bmet::<T>(bmetd, s),
llre: Vec::new(),
}
}
pub fn compute_llr_partial<P: Protocol, S: SpecScalar, T: LlrScalar>(
cs: &[Cmplx<S>],
nsym: usize,
) -> Vec<T> {
debug_assert!((1..=MAX_NSYM).contains(&nsym));
let codeword_len = codeword_bit_len::<P>();
let mut bmet = vec![0.0f32; codeword_len];
fill_bmet_for_nsym::<P, S>(cs, nsym, &mut bmet, None);
scale_bmet::<T>(bmet, P::LLR_SCALE)
}
pub fn compute_snr_db<P: Protocol>(cs: &[Cmplx<f32>], itone: &[u8]) -> f32 {
compute_snr_db_generic::<P, f32>(cs, itone)
}
pub fn compute_snr_db_generic<P: Protocol, S: SpecScalar>(cs: &[Cmplx<S>], itone: &[u8]) -> f32 {
let ntones = P::NTONES as usize;
let n_sym = P::N_SYMBOLS as usize;
let mut xsig = 0.0f32;
let mut xnoi = 0.0f32;
let offset = ntones / 2;
for k in 0..n_sym.min(itone.len()) {
let t = itone[k] as usize % ntones;
xsig += cs[k * ntones + t].norm_sqr_f32();
xnoi += cs[k * ntones + (t + offset) % ntones].norm_sqr_f32();
}
if xnoi < f32::EPSILON {
return -24.0;
}
let ratio = xsig / xnoi - 1.0;
if ratio <= 0.001 {
return -24.0;
}
(10.0 * ratio.log10() - 27.0_f32).max(-24.0)
}
pub fn sync_quality<P: Protocol>(cs: &[Cmplx<f32>]) -> u32 {
sync_quality_generic::<P, f32>(cs)
}
pub fn sync_quality_generic<P: Protocol, S: SpecScalar>(cs: &[Cmplx<S>]) -> u32
where
S::Wide: PartialOrd,
{
let ntones = P::NTONES as usize;
let mut count = 0u32;
for block in P::SYNC_MODE.blocks() {
let start = block.start_symbol as usize;
for (t, &expected) in block.pattern.iter().enumerate() {
let sym = start + t;
let mut best = 0usize;
let mut best_val = cs[sym * ntones].norm_sqr_wide();
for a in 1..ntones {
let v = cs[sym * ntones + a].norm_sqr_wide();
if v > best_val {
best_val = v;
best = a;
}
}
if best == expected as usize {
count += 1;
}
}
}
count
}