use crate::filter::filter_iterated;
use crate::frame::FlatFrame;
use crate::halo::horizontal_halo_keep_mask;
use crate::params::{FilterParams, HaloParams};
pub fn in_window_mask(scans: &[u32], intervals: &[(u32, u32)], scan_pad: u32) -> Vec<bool> {
scans
.iter()
.map(|&s| {
let i = intervals.partition_point(|&(_, end)| end.saturating_add(scan_pad) <= s);
i < intervals.len() && s >= intervals[i].0.saturating_sub(scan_pad)
})
.collect()
}
pub fn filter_per_window(
frame: &FlatFrame,
intervals: &[(u32, u32)],
params: &FilterParams,
halo: Option<&HaloParams>,
) -> Vec<bool> {
let mut keep = vec![false; frame.len()];
for &(begin, end) in intervals {
if end <= begin {
continue;
}
let local: Vec<usize> = (0..frame.len())
.filter(|&i| frame.scan[i] >= begin && frame.scan[i] < end)
.collect();
if local.is_empty() {
continue;
}
let num_scans = (end - begin) as usize;
let sub = FlatFrame {
frame_id: frame.frame_id,
num_scans,
scan: local.iter().map(|&i| frame.scan[i] - begin).collect(),
tof: local.iter().map(|&i| frame.tof[i]).collect(),
intensity: local.iter().map(|&i| frame.intensity[i]).collect(),
};
let mut mask = filter_iterated(&sub, params);
if let Some(hp) = halo {
apply_halo(&sub, hp, &mut mask);
}
for (j, &orig) in local.iter().enumerate() {
if mask[j] {
keep[orig] = true;
}
}
}
keep
}
fn apply_halo(frame: &FlatFrame, hp: &HaloParams, mask: &mut [bool]) {
let idx: Vec<usize> = (0..frame.len()).filter(|&i| mask[i]).collect();
if idx.is_empty() {
return;
}
let scan: Vec<u32> = idx.iter().map(|&i| frame.scan[i]).collect();
let tof: Vec<u32> = idx.iter().map(|&i| frame.tof[i]).collect();
let inten: Vec<u32> = idx.iter().map(|&i| frame.intensity[i]).collect();
let hmask = horizontal_halo_keep_mask(&scan, &tof, &inten, frame.num_scans, hp);
for (k, &i) in idx.iter().enumerate() {
if !hmask[k] {
mask[i] = false;
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn in_window_mask_basic() {
let scans = [5, 10, 15, 19, 20, 25, 30, 39, 40];
let iv = [(10u32, 20u32), (30, 40)];
let keep = in_window_mask(&scans, &iv, 0);
assert_eq!(
keep,
vec![false, true, true, true, false, false, true, true, false]
);
}
#[test]
fn in_window_mask_pad_extends_edges() {
let scans = [8, 9, 20, 21];
let iv = [(10u32, 20u32)];
assert_eq!(in_window_mask(&scans, &iv, 2), vec![true, true, true, true]);
assert_eq!(
in_window_mask(&scans, &iv, 0),
vec![false, false, false, false]
);
}
#[test]
fn empty_intervals_keep_nothing() {
assert_eq!(
in_window_mask(&[1, 2, 3], &[], 0),
vec![false, false, false]
);
}
#[test]
fn per_window_does_not_link_across_boundary() {
let scan: Vec<u32> = (2..8).collect(); let frame = FlatFrame {
frame_id: 1,
num_scans: 10,
scan: scan.clone(),
tof: vec![1000; scan.len()],
intensity: vec![100; scan.len()],
};
let params = FilterParams {
min_feature_length: 4,
max_internal_gap: 0,
num_iterations: 1,
..FilterParams::default()
};
let iv = [(0u32, 5u32), (5, 10)];
let keep = filter_per_window(&frame, &iv, ¶ms, None);
assert!(
keep.iter().all(|&k| !k),
"boundary-straddling run must split"
);
let whole = filter_iterated(&frame, ¶ms);
assert!(whole.iter().all(|&k| k));
}
#[test]
fn per_window_keeps_self_contained_feature() {
let scan: Vec<u32> = (6..11).collect();
let frame = FlatFrame {
frame_id: 1,
num_scans: 20,
scan: scan.clone(),
tof: vec![1000; scan.len()],
intensity: vec![100; scan.len()],
};
let params = FilterParams {
min_feature_length: 4,
max_internal_gap: 0,
num_iterations: 1,
..FilterParams::default()
};
let keep = filter_per_window(&frame, &[(5, 12)], ¶ms, None);
assert!(keep.iter().all(|&k| k));
}
}