//! CSI frame parsing, signal field generation, feature extraction,
//! classification, vital signs smoothing, and multi-person estimation.
use ruvector_mincut::{DynamicMinCut, MinCutBuilder};
use std::collections::{HashMap, VecDeque};
use wifi_densepose_hardware::PpduType;
use crate::adaptive_classifier;
use crate::types::*;
use crate::vital_signs::VitalSigns;
// ── ESP32 UDP frame parsers ─────────────────────────────────────────────────
/// Parse a 32-byte edge vitals packet (magic 0xC511_0002).
pub fn parse_esp32_vitals(buf: &[u8]) -> Option<Esp32VitalsPacket> {
if buf.len() < 32 {
return None;
}
let magic = u32::from_le_bytes([buf[0], buf[1], buf[2], buf[3]]);
if magic != 0xC511_0002 {
return None;
}
let node_id = buf[4];
let flags = buf[5];
let breathing_raw = u16::from_le_bytes([buf[6], buf[7]]);
let heartrate_raw = u32::from_le_bytes([buf[8], buf[9], buf[10], buf[11]]);
let rssi = buf[12] as i8;
let n_persons = buf[13];
let motion_energy = f32::from_le_bytes([buf[16], buf[17], buf[18], buf[19]]);
let presence_score = f32::from_le_bytes([buf[20], buf[21], buf[22], buf[23]]);
let timestamp_ms = u32::from_le_bytes([buf[24], buf[25], buf[26], buf[27]]);
Some(Esp32VitalsPacket {
node_id,
presence: (flags & 0x01) != 0,
fall_detected: (flags & 0x02) != 0,
motion: (flags & 0x04) != 0,
breathing_rate_bpm: breathing_raw as f64 / 100.0,
heartrate_bpm: heartrate_raw as f64 / 10000.0,
rssi,
n_persons,
motion_energy,
presence_score,
timestamp_ms,
})
}
/// Parse a WASM output packet (magic 0xC511_0007 — reassigned per issue #928;
/// the original 0xC511_0004 collided with ADR-063 fused vitals).
pub fn parse_wasm_output(buf: &[u8]) -> Option<WasmOutputPacket> {
if buf.len() < 8 {
return None;
}
let magic = u32::from_le_bytes([buf[0], buf[1], buf[2], buf[3]]);
if magic != 0xC511_0007 {
return None;
}
let node_id = buf[4];
let module_id = buf[5];
let event_count = u16::from_le_bytes([buf[6], buf[7]]) as usize;
let mut events = Vec::with_capacity(event_count);
let mut offset = 8;
for _ in 0..event_count {
if offset + 5 > buf.len() {
break;
}
let event_type = buf[offset];
let value = f32::from_le_bytes([
buf[offset + 1],
buf[offset + 2],
buf[offset + 3],
buf[offset + 4],
]);
events.push(WasmEvent { event_type, value });
offset += 5;
}
Some(WasmOutputPacket {
node_id,
module_id,
events,
})
}
/// Parse an ADR-018 raw CSI frame (magic 0xC511_0001).
///
/// Header layout (authoritative: firmware `csi_collector.c` / ADR-018):
/// magic u32 LE @0, node_id u8 @4, n_antennas u8 @5, n_subcarriers u16 LE
/// @6-7, freq_mhz u32 LE @8-11, sequence u32 LE @12-15, rssi i8 @16,
/// noise_floor i8 @17, PPDU type u8 @18 (ADR-110), flags u8 @19 (ADR-110),
/// I/Q pairs from @20.
///
/// Until issue #1005 this function read `n_subcarriers` from byte 6 alone
/// (an ESP32-C6 HE-SU frame's 256 = 0x0100 LE decoded as 0 — the frame
/// parsed "successfully" with zero subcarriers) and read sequence/rssi/
/// noise at stale offsets 10/14/15 (rssi landed on sequence bytes ⇒ 0).
pub fn parse_esp32_frame(buf: &[u8]) -> Option<Esp32Frame> {
if buf.len() < 20 {
return None;
}
let magic = u32::from_le_bytes([buf[0], buf[1], buf[2], buf[3]]);
if magic != 0xC511_0001 {
return None;
}
let node_id = buf[4];
let n_antennas = buf[5];
let n_subcarriers = u16::from_le_bytes([buf[6], buf[7]]);
let freq_mhz_u32 = u32::from_le_bytes([buf[8], buf[9], buf[10], buf[11]]);
let freq_mhz = u16::try_from(freq_mhz_u32).unwrap_or(0);
let sequence = u32::from_le_bytes([buf[12], buf[13], buf[14], buf[15]]);
let rssi_raw = buf[16] as i8;
let rssi = if rssi_raw > 0 {
rssi_raw.saturating_neg()
} else {
rssi_raw
};
let noise_floor = buf[17] as i8;
let ppdu_type = PpduType::from_byte(buf[18]);
let iq_start = 20;
let n_pairs = n_antennas as usize * n_subcarriers as usize;
let expected_len = iq_start + n_pairs * 2;
if buf.len() < expected_len {
return None;
}
let mut amplitudes = Vec::with_capacity(n_pairs);
let mut phases = Vec::with_capacity(n_pairs);
for k in 0..n_pairs {
let i_val = buf[iq_start + k * 2] as i8 as f64;
let q_val = buf[iq_start + k * 2 + 1] as i8 as f64;
amplitudes.push((i_val * i_val + q_val * q_val).sqrt());
phases.push(q_val.atan2(i_val));
}
Some(Esp32Frame {
magic,
node_id,
n_antennas,
n_subcarriers,
freq_mhz,
sequence,
rssi,
noise_floor,
ppdu_type,
amplitudes,
phases,
})
}
// ── Signal field generation ─────────────────────────────────────────────────
pub fn generate_signal_field(
_mean_rssi: f64,
motion_score: f64,
breathing_rate_hz: f64,
signal_quality: f64,
subcarrier_variances: &[f64],
) -> SignalField {
let grid = 20usize;
let mut values = vec![0.0f64; grid * grid];
let center = (grid as f64 - 1.0) / 2.0;
let max_var = subcarrier_variances.iter().cloned().fold(0.0f64, f64::max);
let norm_factor = if max_var > 1e-9 { max_var } else { 1.0 };
let n_sub = subcarrier_variances.len().max(1);
for (k, &var) in subcarrier_variances.iter().enumerate() {
let weight = (var / norm_factor) * motion_score;
if weight < 1e-6 {
continue;
}
let angle = (k as f64 / n_sub as f64) * 2.0 * std::f64::consts::PI;
let radius = center * 0.8 * weight.sqrt();
let hx = center + radius * angle.cos();
let hz = center + radius * angle.sin();
for z in 0..grid {
for x in 0..grid {
let dx = x as f64 - hx;
let dz = z as f64 - hz;
let dist2 = dx * dx + dz * dz;
let spread = (0.5 + weight * 2.0).max(0.5);
values[z * grid + x] += weight * (-dist2 / (2.0 * spread * spread)).exp();
}
}
}
for z in 0..grid {
for x in 0..grid {
let dx = x as f64 - center;
let dz = z as f64 - center;
let dist = (dx * dx + dz * dz).sqrt();
let base = signal_quality * (-dist * 0.12).exp();
values[z * grid + x] += base * 0.3;
}
}
if breathing_rate_hz > 0.05 {
let ring_r = center * 0.55;
let ring_width = 1.8f64;
for z in 0..grid {
for x in 0..grid {
let dx = x as f64 - center;
let dz = z as f64 - center;
let dist = (dx * dx + dz * dz).sqrt();
let ring_val =
0.08 * (-(dist - ring_r).powi(2) / (2.0 * ring_width * ring_width)).exp();
values[z * grid + x] += ring_val;
}
}
}
let field_max = values.iter().cloned().fold(0.0f64, f64::max);
let scale = if field_max > 1e-9 {
1.0 / field_max
} else {
1.0
};
for v in &mut values {
*v = (*v * scale).clamp(0.0, 1.0);
}
SignalField {
grid_size: [grid, 1, grid],
values,
}
}
// ── Feature extraction ──────────────────────────────────────────────────────
pub fn estimate_breathing_rate_hz(frame_history: &VecDeque<Vec<f64>>, sample_rate_hz: f64) -> f64 {
let n = frame_history.len();
if n < 6 {
return 0.0;
}
let series: Vec<f64> = frame_history
.iter()
.map(|amps| {
if amps.is_empty() {
0.0
} else {
amps.iter().sum::<f64>() / amps.len() as f64
}
})
.collect();
let mean_s = series.iter().sum::<f64>() / n as f64;
let detrended: Vec<f64> = series.iter().map(|x| x - mean_s).collect();
let n_candidates = 9usize;
let f_low = 0.1f64;
let f_high = 0.5f64;
let mut best_freq = 0.0f64;
let mut best_power = 0.0f64;
for i in 0..n_candidates {
let freq = f_low + (f_high - f_low) * i as f64 / (n_candidates - 1).max(1) as f64;
let omega = 2.0 * std::f64::consts::PI * freq / sample_rate_hz;
let coeff = 2.0 * omega.cos();
let (mut s_prev2, mut s_prev1) = (0.0f64, 0.0f64);
for &x in &detrended {
let s = x + coeff * s_prev1 - s_prev2;
s_prev2 = s_prev1;
s_prev1 = s;
}
let power = s_prev2 * s_prev2 + s_prev1 * s_prev1 - coeff * s_prev1 * s_prev2;
if power > best_power {
best_power = power;
best_freq = freq;
}
}
let avg_power = {
let mut total = 0.0f64;
for i in 0..n_candidates {
let freq = f_low + (f_high - f_low) * i as f64 / (n_candidates - 1).max(1) as f64;
let omega = 2.0 * std::f64::consts::PI * freq / sample_rate_hz;
let coeff = 2.0 * omega.cos();
let (mut s_prev2, mut s_prev1) = (0.0f64, 0.0f64);
for &x in &detrended {
let s = x + coeff * s_prev1 - s_prev2;
s_prev2 = s_prev1;
s_prev1 = s;
}
total += s_prev2 * s_prev2 + s_prev1 * s_prev1 - coeff * s_prev1 * s_prev2;
}
total / n_candidates as f64
};
if best_power > avg_power * 3.0 {
best_freq.clamp(f_low, f_high)
} else {
0.0
}
}
pub fn compute_subcarrier_importance_weights(sensitivity: &[f64]) -> Vec<f64> {
let n = sensitivity.len();
if n == 0 {
return vec![];
}
let max_sens = sensitivity
.iter()
.cloned()
.fold(f64::NEG_INFINITY, f64::max)
.max(1e-9);
let mut sorted = sensitivity.to_vec();
sorted.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
let median = if n % 2 == 0 {
(sorted[n / 2 - 1] + sorted[n / 2]) / 2.0
} else {
sorted[n / 2]
};
sensitivity
.iter()
.map(|&s| {
if s >= median {
1.0 + (s / max_sens).min(1.0)
} else {
0.5
}
})
.collect()
}
pub fn compute_subcarrier_variances(frame_history: &VecDeque<Vec<f64>>, n_sub: usize) -> Vec<f64> {
if frame_history.is_empty() || n_sub == 0 {
return vec![0.0; n_sub];
}
let n_frames = frame_history.len() as f64;
let mut means = vec![0.0f64; n_sub];
let mut sq_means = vec![0.0f64; n_sub];
for frame in frame_history.iter() {
for k in 0..n_sub {
let a = if k < frame.len() { frame[k] } else { 0.0 };
means[k] += a;
sq_means[k] += a * a;
}
}
(0..n_sub)
.map(|k| {
let mean = means[k] / n_frames;
let sq_mean = sq_means[k] / n_frames;
(sq_mean - mean * mean).max(0.0)
})
.collect()
}
pub fn extract_features_from_frame(
frame: &Esp32Frame,
frame_history: &VecDeque<Vec<f64>>,
sample_rate_hz: f64,
) -> (FeatureInfo, ClassificationInfo, f64, Vec<f64>, f64) {
let n_sub = frame.amplitudes.len().max(1);
let n = n_sub as f64;
let mean_rssi = frame.rssi as f64;
let sub_sensitivity: Vec<f64> = frame.amplitudes.iter().map(|a| a.abs()).collect();
let importance_weights = compute_subcarrier_importance_weights(&sub_sensitivity);
let weight_sum: f64 = importance_weights.iter().sum::<f64>();
let mean_amp: f64 = if weight_sum > 0.0 {
frame
.amplitudes
.iter()
.zip(importance_weights.iter())
.map(|(a, w)| a * w)
.sum::<f64>()
/ weight_sum
} else {
frame.amplitudes.iter().sum::<f64>() / n
};
let intra_variance: f64 = if weight_sum > 0.0 {
frame
.amplitudes
.iter()
.zip(importance_weights.iter())
.map(|(a, w)| w * (a - mean_amp).powi(2))
.sum::<f64>()
/ weight_sum
} else {
frame
.amplitudes
.iter()
.map(|a| (a - mean_amp).powi(2))
.sum::<f64>()
/ n
};
let sub_variances = compute_subcarrier_variances(frame_history, n_sub);
let temporal_variance: f64 = if sub_variances.is_empty() {
intra_variance
} else {
sub_variances.iter().sum::<f64>() / sub_variances.len() as f64
};
let variance = intra_variance.max(temporal_variance);
let spectral_power: f64 = frame.amplitudes.iter().map(|a| a * a).sum::<f64>() / n;
let half = frame.amplitudes.len() / 2;
let motion_band_power = if half > 0 {
frame.amplitudes[half..]
.iter()
.map(|a| (a - mean_amp).powi(2))
.sum::<f64>()
/ (frame.amplitudes.len() - half) as f64
} else {
0.0
};
let breathing_band_power = if half > 0 {
frame.amplitudes[..half]
.iter()
.map(|a| (a - mean_amp).powi(2))
.sum::<f64>()
/ half as f64
} else {
0.0
};
let peak_idx = frame
.amplitudes
.iter()
.enumerate()
.max_by(|a, b| a.1.partial_cmp(b.1).unwrap_or(std::cmp::Ordering::Equal))
.map(|(i, _)| i)
.unwrap_or(0);
let dominant_freq_hz = peak_idx as f64 * 0.05;
let threshold = mean_amp * 1.2;
let change_points = frame
.amplitudes
.windows(2)
.filter(|w| (w[0] < threshold) != (w[1] < threshold))
.count();
let temporal_motion_score = if let Some(prev_frame) = frame_history.back() {
let n_cmp = n_sub.min(prev_frame.len());
if n_cmp > 0 {
let diff_energy: f64 = (0..n_cmp)
.map(|k| (frame.amplitudes[k] - prev_frame[k]).powi(2))
.sum::<f64>()
/ n_cmp as f64;
let ref_energy = mean_amp * mean_amp + 1e-9;
(diff_energy / ref_energy).sqrt().clamp(0.0, 1.0)
} else {
0.0
}
} else {
(intra_variance / (mean_amp * mean_amp + 1e-9))
.sqrt()
.clamp(0.0, 1.0)
};
let variance_motion = (temporal_variance / 10.0).clamp(0.0, 1.0);
let mbp_motion = (motion_band_power / 25.0).clamp(0.0, 1.0);
let cp_motion = (change_points as f64 / 15.0).clamp(0.0, 1.0);
let motion_score = (temporal_motion_score * 0.4
+ variance_motion * 0.2
+ mbp_motion * 0.25
+ cp_motion * 0.15)
.clamp(0.0, 1.0);
let snr_db = (frame.rssi as f64 - frame.noise_floor as f64).max(0.0);
let snr_quality = (snr_db / 40.0).clamp(0.0, 1.0);
let stability =
(1.0 - (temporal_variance / (mean_amp * mean_amp + 1e-9)).clamp(0.0, 1.0)).max(0.0);
let signal_quality = (snr_quality * 0.6 + stability * 0.4).clamp(0.0, 1.0);
let breathing_rate_hz = estimate_breathing_rate_hz(frame_history, sample_rate_hz);
let features = FeatureInfo {
mean_rssi,
variance,
motion_band_power,
breathing_band_power,
dominant_freq_hz,
change_points,
spectral_power,
};
let raw_classification = ClassificationInfo {
motion_level: raw_classify(motion_score),
presence: motion_score > 0.04,
confidence: (0.4 + signal_quality * 0.3 + motion_score * 0.3).clamp(0.0, 1.0),
};
(
features,
raw_classification,
breathing_rate_hz,
sub_variances,
motion_score,
)
}
// ── Classification ──────────────────────────────────────────────────────────
pub fn raw_classify(score: f64) -> String {
if score > 0.25 {
"active".into()
} else if score > 0.12 {
"present_moving".into()
} else if score > 0.04 {
"present_still".into()
} else {
"absent".into()
}
}
pub fn smooth_and_classify(
state: &mut AppStateInner,
raw: &mut ClassificationInfo,
raw_motion: f64,
) {
state.baseline_frames += 1;
if state.baseline_frames < BASELINE_WARMUP {
state.baseline_motion = state.baseline_motion * 0.9 + raw_motion * 0.1;
} else if raw_motion < state.smoothed_motion + 0.05 {
state.baseline_motion =
state.baseline_motion * (1.0 - BASELINE_EMA_ALPHA) + raw_motion * BASELINE_EMA_ALPHA;
}
let adjusted = (raw_motion - state.baseline_motion * 0.7).max(0.0);
state.smoothed_motion =
state.smoothed_motion * (1.0 - MOTION_EMA_ALPHA) + adjusted * MOTION_EMA_ALPHA;
let sm = state.smoothed_motion;
let candidate = raw_classify(sm);
if candidate == state.current_motion_level {
state.debounce_counter = 0;
state.debounce_candidate = candidate;
} else if candidate == state.debounce_candidate {
state.debounce_counter += 1;
if state.debounce_counter >= DEBOUNCE_FRAMES {
state.current_motion_level = candidate;
state.debounce_counter = 0;
}
} else {
state.debounce_candidate = candidate;
state.debounce_counter = 1;
}
raw.motion_level = state.current_motion_level.clone();
raw.presence = sm > 0.03;
raw.confidence = (0.4 + sm * 0.6).clamp(0.0, 1.0);
}
pub fn smooth_and_classify_node(ns: &mut NodeState, raw: &mut ClassificationInfo, raw_motion: f64) {
ns.baseline_frames += 1;
if ns.baseline_frames < BASELINE_WARMUP {
ns.baseline_motion = ns.baseline_motion * 0.9 + raw_motion * 0.1;
} else if raw_motion < ns.smoothed_motion + 0.05 {
ns.baseline_motion =
ns.baseline_motion * (1.0 - BASELINE_EMA_ALPHA) + raw_motion * BASELINE_EMA_ALPHA;
}
let adjusted = (raw_motion - ns.baseline_motion * 0.7).max(0.0);
ns.smoothed_motion =
ns.smoothed_motion * (1.0 - MOTION_EMA_ALPHA) + adjusted * MOTION_EMA_ALPHA;
let sm = ns.smoothed_motion;
let candidate = raw_classify(sm);
if candidate == ns.current_motion_level {
ns.debounce_counter = 0;
ns.debounce_candidate = candidate;
} else if candidate == ns.debounce_candidate {
ns.debounce_counter += 1;
if ns.debounce_counter >= DEBOUNCE_FRAMES {
ns.current_motion_level = candidate;
ns.debounce_counter = 0;
}
} else {
ns.debounce_candidate = candidate;
ns.debounce_counter = 1;
}
raw.motion_level = ns.current_motion_level.clone();
raw.presence = sm > 0.03;
raw.confidence = (0.4 + sm * 0.6).clamp(0.0, 1.0);
}
pub fn adaptive_override(
state: &AppStateInner,
features: &FeatureInfo,
classification: &mut ClassificationInfo,
) {
if let Some(ref model) = state.adaptive_model {
let amps = state
.frame_history
.back()
.map(|v| v.as_slice())
.unwrap_or(&[]);
let feat_arr = adaptive_classifier::features_from_runtime(
&serde_json::json!({
"variance": features.variance,
"motion_band_power": features.motion_band_power,
"breathing_band_power": features.breathing_band_power,
"spectral_power": features.spectral_power,
"dominant_freq_hz": features.dominant_freq_hz,
"change_points": features.change_points,
"mean_rssi": features.mean_rssi,
}),
amps,
);
let (label, conf) = model.classify(&feat_arr);
classification.motion_level = label.to_string();
classification.presence = label != "absent";
classification.confidence = (conf * 0.7 + classification.confidence * 0.3).clamp(0.0, 1.0);
}
}
// ── Vital signs smoothing ───────────────────────────────────────────────────
fn trimmed_mean(buf: &VecDeque<f64>) -> f64 {
if buf.is_empty() {
return 0.0;
}
let mut sorted: Vec<f64> = buf.iter().copied().collect();
sorted.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
let n = sorted.len();
let trim = n / 4;
let middle = &sorted[trim..n - trim.max(0)];
if middle.is_empty() {
sorted[n / 2]
} else {
middle.iter().sum::<f64>() / middle.len() as f64
}
}
pub fn smooth_vitals(state: &mut AppStateInner, raw: &VitalSigns) -> VitalSigns {
let raw_hr = raw.heart_rate_bpm.unwrap_or(0.0);
let raw_br = raw.breathing_rate_bpm.unwrap_or(0.0);
let hr_ok = state.smoothed_hr < 1.0 || (raw_hr - state.smoothed_hr).abs() < HR_MAX_JUMP;
let br_ok = state.smoothed_br < 1.0 || (raw_br - state.smoothed_br).abs() < BR_MAX_JUMP;
if hr_ok && raw_hr > 0.0 {
state.hr_buffer.push_back(raw_hr);
if state.hr_buffer.len() > VITAL_MEDIAN_WINDOW {
state.hr_buffer.pop_front();
}
}
if br_ok && raw_br > 0.0 {
state.br_buffer.push_back(raw_br);
if state.br_buffer.len() > VITAL_MEDIAN_WINDOW {
state.br_buffer.pop_front();
}
}
let trimmed_hr = trimmed_mean(&state.hr_buffer);
let trimmed_br = trimmed_mean(&state.br_buffer);
if trimmed_hr > 0.0 {
if state.smoothed_hr < 1.0 {
state.smoothed_hr = trimmed_hr;
} else if (trimmed_hr - state.smoothed_hr).abs() > HR_DEAD_BAND {
state.smoothed_hr =
state.smoothed_hr * (1.0 - VITAL_EMA_ALPHA) + trimmed_hr * VITAL_EMA_ALPHA;
}
}
if trimmed_br > 0.0 {
if state.smoothed_br < 1.0 {
state.smoothed_br = trimmed_br;
} else if (trimmed_br - state.smoothed_br).abs() > BR_DEAD_BAND {
state.smoothed_br =
state.smoothed_br * (1.0 - VITAL_EMA_ALPHA) + trimmed_br * VITAL_EMA_ALPHA;
}
}
state.smoothed_hr_conf = state.smoothed_hr_conf * 0.92 + raw.heartbeat_confidence * 0.08;
state.smoothed_br_conf = state.smoothed_br_conf * 0.92 + raw.breathing_confidence * 0.08;
VitalSigns {
breathing_rate_bpm: if state.smoothed_br > 1.0 {
Some(state.smoothed_br)
} else {
None
},
heart_rate_bpm: if state.smoothed_hr > 1.0 {
Some(state.smoothed_hr)
} else {
None
},
breathing_confidence: state.smoothed_br_conf,
heartbeat_confidence: state.smoothed_hr_conf,
signal_quality: raw.signal_quality,
}
}
pub fn smooth_vitals_node(ns: &mut NodeState, raw: &VitalSigns) -> VitalSigns {
let raw_hr = raw.heart_rate_bpm.unwrap_or(0.0);
let raw_br = raw.breathing_rate_bpm.unwrap_or(0.0);
let hr_ok = ns.smoothed_hr < 1.0 || (raw_hr - ns.smoothed_hr).abs() < HR_MAX_JUMP;
let br_ok = ns.smoothed_br < 1.0 || (raw_br - ns.smoothed_br).abs() < BR_MAX_JUMP;
if hr_ok && raw_hr > 0.0 {
ns.hr_buffer.push_back(raw_hr);
if ns.hr_buffer.len() > VITAL_MEDIAN_WINDOW {
ns.hr_buffer.pop_front();
}
}
if br_ok && raw_br > 0.0 {
ns.br_buffer.push_back(raw_br);
if ns.br_buffer.len() > VITAL_MEDIAN_WINDOW {
ns.br_buffer.pop_front();
}
}
let trimmed_hr = trimmed_mean(&ns.hr_buffer);
let trimmed_br = trimmed_mean(&ns.br_buffer);
if trimmed_hr > 0.0 {
if ns.smoothed_hr < 1.0 {
ns.smoothed_hr = trimmed_hr;
} else if (trimmed_hr - ns.smoothed_hr).abs() > HR_DEAD_BAND {
ns.smoothed_hr =
ns.smoothed_hr * (1.0 - VITAL_EMA_ALPHA) + trimmed_hr * VITAL_EMA_ALPHA;
}
}
if trimmed_br > 0.0 {
if ns.smoothed_br < 1.0 {
ns.smoothed_br = trimmed_br;
} else if (trimmed_br - ns.smoothed_br).abs() > BR_DEAD_BAND {
ns.smoothed_br =
ns.smoothed_br * (1.0 - VITAL_EMA_ALPHA) + trimmed_br * VITAL_EMA_ALPHA;
}
}
ns.smoothed_hr_conf = ns.smoothed_hr_conf * 0.92 + raw.heartbeat_confidence * 0.08;
ns.smoothed_br_conf = ns.smoothed_br_conf * 0.92 + raw.breathing_confidence * 0.08;
VitalSigns {
breathing_rate_bpm: if ns.smoothed_br > 1.0 {
Some(ns.smoothed_br)
} else {
None
},
heart_rate_bpm: if ns.smoothed_hr > 1.0 {
Some(ns.smoothed_hr)
} else {
None
},
breathing_confidence: ns.smoothed_br_conf,
heartbeat_confidence: ns.smoothed_hr_conf,
signal_quality: raw.signal_quality,
}
}
// ── Multi-person estimation ─────────────────────────────────────────────────
pub fn fuse_multi_node_features(
current_features: &FeatureInfo,
node_states: &HashMap<u8, NodeState>,
) -> FeatureInfo {
let now = std::time::Instant::now();
let active: Vec<(&FeatureInfo, f64)> = node_states
.values()
.filter(|ns| {
ns.last_frame_time
.is_some_and(|t| now.duration_since(t).as_secs() < 10)
})
.filter_map(|ns| {
let feat = ns.latest_features.as_ref()?;
let rssi = ns.rssi_history.back().copied().unwrap_or(-80.0);
Some((feat, rssi))
})
.collect();
if active.len() <= 1 {
return current_features.clone();
}
let max_rssi = active
.iter()
.map(|(_, r)| *r)
.fold(f64::NEG_INFINITY, f64::max);
let weights: Vec<f64> = active
.iter()
.map(|(_, r)| (1.0 + (r - max_rssi + 20.0) / 20.0).clamp(0.1, 1.0))
.collect();
let w_sum: f64 = weights.iter().sum::<f64>().max(1e-9);
FeatureInfo {
variance: active
.iter()
.zip(&weights)
.map(|((f, _), w)| f.variance * w)
.sum::<f64>()
/ w_sum,
motion_band_power: active
.iter()
.zip(&weights)
.map(|((f, _), w)| f.motion_band_power * w)
.sum::<f64>()
/ w_sum,
breathing_band_power: active
.iter()
.zip(&weights)
.map(|((f, _), w)| f.breathing_band_power * w)
.sum::<f64>()
/ w_sum,
spectral_power: active
.iter()
.zip(&weights)
.map(|((f, _), w)| f.spectral_power * w)
.sum::<f64>()
/ w_sum,
dominant_freq_hz: active
.iter()
.zip(&weights)
.map(|((f, _), w)| f.dominant_freq_hz * w)
.sum::<f64>()
/ w_sum,
change_points: current_features.change_points,
mean_rssi: active
.iter()
.map(|(f, _)| f.mean_rssi)
.fold(f64::NEG_INFINITY, f64::max),
}
}
pub fn compute_person_score(feat: &FeatureInfo) -> f64 {
let var_norm = (feat.variance / 300.0).clamp(0.0, 1.0);
let cp_norm = (feat.change_points as f64 / 30.0).clamp(0.0, 1.0);
let motion_norm = (feat.motion_band_power / 250.0).clamp(0.0, 1.0);
let sp_norm = (feat.spectral_power / 500.0).clamp(0.0, 1.0);
var_norm * 0.40 + cp_norm * 0.20 + motion_norm * 0.25 + sp_norm * 0.15
}
pub fn estimate_persons_from_correlation(frame_history: &VecDeque<Vec<f64>>) -> usize {
let n_frames = frame_history.len();
if n_frames < 10 {
return 1;
}
let window: Vec<&Vec<f64>> = frame_history.iter().rev().take(20).collect();
let n_sub = window[0].len().min(56);
if n_sub < 4 {
return 1;
}
let k = window.len() as f64;
let mut means = vec![0.0f64; n_sub];
let mut variances = vec![0.0f64; n_sub];
for frame in &window {
for sc in 0..n_sub.min(frame.len()) {
means[sc] += frame[sc] / k;
}
}
for frame in &window {
for sc in 0..n_sub.min(frame.len()) {
variances[sc] += (frame[sc] - means[sc]).powi(2) / k;
}
}
let noise_floor = 1.0;
let active: Vec<usize> = (0..n_sub)
.filter(|&sc| variances[sc] > noise_floor)
.collect();
let m = active.len();
if m < 3 {
return if m == 0 { 0 } else { 1 };
}
let mut edges: Vec<(u64, u64, f64)> = Vec::new();
let source = m as u64;
let sink = (m + 1) as u64;
let stds: Vec<f64> = active
.iter()
.map(|&sc| variances[sc].sqrt().max(1e-9))
.collect();
for i in 0..m {
for j in (i + 1)..m {
let mut cov = 0.0f64;
for frame in &window {
let (si, sj) = (active[i], active[j]);
if si < frame.len() && sj < frame.len() {
cov += (frame[si] - means[si]) * (frame[sj] - means[sj]) / k;
}
}
let corr = (cov / (stds[i] * stds[j])).abs();
if corr > 0.1 {
let weight = corr * 10.0;
edges.push((i as u64, j as u64, weight));
edges.push((j as u64, i as u64, weight));
}
}
}
// partial_cmp returns None on NaN; the outer unwrap_or only catches an
// empty iterator, not a comparator panic. Same NaN-panic class as #611.
let (max_var_idx, _) = active
.iter()
.enumerate()
.max_by(|(_, &a), (_, &b)| {
variances[a]
.partial_cmp(&variances[b])
.unwrap_or(std::cmp::Ordering::Equal)
})
.unwrap_or((0, &0));
let (min_var_idx, _) = active
.iter()
.enumerate()
.min_by(|(_, &a), (_, &b)| {
variances[a]
.partial_cmp(&variances[b])
.unwrap_or(std::cmp::Ordering::Equal)
})
.unwrap_or((0, &0));
if max_var_idx == min_var_idx {
return 1;
}
edges.push((source, max_var_idx as u64, 100.0));
edges.push((min_var_idx as u64, sink, 100.0));
let mc: DynamicMinCut = match MinCutBuilder::new()
.exact()
.with_edges(edges.clone())
.build()
{
Ok(mc) => mc,
Err(_) => return 1,
};
let cut_value = mc.min_cut_value();
let total_edge_weight: f64 = edges
.iter()
.filter(|(s, t, _)| *s != source && *s != sink && *t != source && *t != sink)
.map(|(_, _, w)| w)
.sum::<f64>()
/ 2.0;
if total_edge_weight < 1e-9 {
return 1;
}
let cut_ratio = cut_value / total_edge_weight;
if cut_ratio > 0.4 {
1
} else if cut_ratio > 0.15 {
2
} else {
3
}
}
pub fn score_to_person_count(smoothed_score: f64, prev_count: usize) -> usize {
match prev_count {
0 | 1 => {
if smoothed_score > 0.85 {
3
} else if smoothed_score > 0.70 {
2
} else {
1
}
}
2 => {
if smoothed_score > 0.92 {
3
} else if smoothed_score < 0.55 {
1
} else {
2
}
}
_ => {
if smoothed_score < 0.55 {
1
} else if smoothed_score < 0.78 {
2
} else {
3
}
}
}
}
/// Generate a simulated ESP32 frame for testing/demo mode.
pub fn generate_simulated_frame(tick: u64) -> Esp32Frame {
let t = tick as f64 * 0.1;
let n_sub = 56usize;
let mut amplitudes = Vec::with_capacity(n_sub);
let mut phases = Vec::with_capacity(n_sub);
for i in 0..n_sub {
let base = 15.0 + 5.0 * (i as f64 * 0.1 + t * 0.3).sin();
let noise = (i as f64 * 7.3 + t * 13.7).sin() * 2.0;
amplitudes.push((base + noise).max(0.1));
phases.push((i as f64 * 0.2 + t * 0.5).sin() * std::f64::consts::PI);
}
Esp32Frame {
magic: 0xC511_0001,
node_id: 1,
n_antennas: 1,
n_subcarriers: n_sub as u16,
freq_mhz: 2437,
sequence: tick as u32,
rssi: (-40.0 + 5.0 * (t * 0.2).sin()) as i8,
noise_floor: -90,
ppdu_type: PpduType::HtLegacy,
amplitudes,
phases,
}
}
/// Generate a simple timestamp (epoch seconds) for recording IDs.
pub fn chrono_timestamp() -> u64 {
std::time::SystemTime::now()
.duration_since(std::time::UNIX_EPOCH)
.map(|d| d.as_secs())
.unwrap_or(0)
}
// ── ADR-110 / issue #1005 tests: live ESP32-C6 HE-LTF frames ────────────────
#[cfg(test)]
mod adr110_tests {
use super::*;
use crate::types::NodeState;
/// Verbatim 532-byte HE-SU UDP payload captured live 2026-06-11 from an
/// ESP32-C6 (node 12, IDF v5.5): 256 subcarrier bins, byte18=0x01.
const HE_FRAME_HEX: &str = "010011c50c010001800900005a2d0000d8a9011000000000000000000000f70ef70ef50cf30bf209f108f006ef03ee02ee00eefdeffbeff8f0f7f1f4f2f3f4f1f5f0f7eef8edfaecfdecffeb01ea03ea05e908ea0aeb0deb0fec11ee13f015f216f318f519f71afa1bfd1bff1c021c051b071b0a1a0c190f1811161315161218101a0e1b0c1c091d071e041f0120ff20fc20f91ff71ff41ef11def1cec1be919e717e615e413e311e10edf0cde09dd06dc04dc01dcffdcfbdcf9ddf6def3dff0e0ede2eae4e8e6e6e8e4eae2ebe0eedef1dcf4dbf7dafad9fdd900d903d806d909d90cda0fdc12dc14dd17df1ae11ce31ee520e722e924ed25f127f328f629f929fd2900290329062809270c260e26122516061a00001c201c1f1a211722142411250e260c27082804280129fe29fb28f927f627f426f125ef23ec22ea20e81eea20e81e891b53a82951565d4ffafbfebe9abddb10222aa47b3b371fd2c0860cd4d86ea2f35faccd46b0b66f6ff0050f2da27d1c92f7f8e1017cb545afd3e3fe60db6f478dc85a33b3454cf6df9061194a0a0fc3e0eedf76f1d292cb25c8f541dfcc4109f9f1a34955520ad8ffa3694ac395cbf6c19073a4aefb1ebf47c76730458431805d9f18ff2e81955e8752b29757f66e289f72f8e35309a737547c040444cbda1a81d221d950037ec38fd9d1dd0f56c3dc707a7bbfe66ca5a97ab7cc17d68d38ba43a1806f91f5911a5967e2c9f7f07186";
/// Verbatim 148-byte HT payload from the same node seconds later:
/// 64 bins, byte18=0x00.
const HT_FRAME_HEX: &str = "010011c50c01400080090000662d0000b1a900100000000000000000fcfaf909f013f112f213f212f311f410f511f510f610f510f411f410f411f312f213f214f214f212f313f513f512f611f610f80ef90df90c0000010eff11fe13ff11fe1300000000ff01000001010002000200020204000301040103000400040002ff03ff03fe02fe02fe01fd00edfc03fa000000000000";
fn unhex(s: &str) -> Vec<u8> {
(0..s.len())
.step_by(2)
.map(|i| u8::from_str_radix(&s[i..i + 2], 16).unwrap())
.collect()
}
#[test]
fn live_he_su_frame_parses_with_256_subcarriers() {
let buf = unhex(HE_FRAME_HEX);
assert_eq!(buf.len(), 532);
let f = parse_esp32_frame(&buf).expect("532-byte HE frame must parse");
assert_eq!(f.node_id, 12);
assert_eq!(f.n_subcarriers, 256);
assert_eq!(f.amplitudes.len(), 256);
assert_eq!(f.freq_mhz, 2432);
assert_eq!(f.sequence, 11610);
assert_eq!(f.rssi, -40);
assert_eq!(f.noise_floor, -87);
assert_eq!(f.ppdu_type, PpduType::HeSu);
}
#[test]
fn live_ht_frame_parses_with_64_subcarriers() {
let buf = unhex(HT_FRAME_HEX);
assert_eq!(buf.len(), 148);
let f = parse_esp32_frame(&buf).expect("148-byte HT frame must parse");
assert_eq!(f.node_id, 12);
assert_eq!(f.n_subcarriers, 64);
assert_eq!(f.amplitudes.len(), 64);
assert_eq!(f.rssi, -79);
assert_eq!(f.ppdu_type, PpduType::HtLegacy);
}
#[test]
fn grid_gate_never_mixes_ht_and_he_windows() {
let he = parse_esp32_frame(&unhex(HE_FRAME_HEX)).unwrap();
let ht = parse_esp32_frame(&unhex(HT_FRAME_HEX)).unwrap();
let mut ns = NodeState::new();
// First frame locks the grid.
assert!(ns.accept_grid(ht.grid()));
ns.frame_history.push_back(ht.amplitudes.clone());
// HE upgrade: accepted, denser grid wins, history re-keyed.
assert!(ns.accept_grid(he.grid()));
assert!(ns.frame_history.is_empty(), "upgrade must clear HT history");
ns.frame_history.push_back(he.amplitudes.clone());
// Interleaved HT minority frames are rejected from the feature path.
assert!(!ns.accept_grid(ht.grid()));
assert_eq!(ns.frame_history.len(), 1, "HT frame must not touch window");
// Steady-state HE frames keep flowing.
assert!(ns.accept_grid(he.grid()));
}
}