use crate::error::DecodeError;
pub type DecodedFrame = (usize, Vec<(u32, u32, u32)>);
const HEADER_BYTES: usize = 8;
pub fn encode_frame_type2(num_scans: usize, points: &[(u32, u32, u32)]) -> Vec<u8> {
let peak_count = points.len();
let mut counts = vec![0u32; num_scans];
for &(scan, _, _) in points {
counts[scan as usize] += 1;
}
let mut offsets = vec![0usize; num_scans + 1];
for s in 0..num_scans {
offsets[s + 1] = offsets[s] + counts[s] as usize;
}
let mut flat: Vec<(u32, u32)> = vec![(0, 0); peak_count];
let mut cursor = offsets[..num_scans].to_vec(); for &(scan, tof, intensity) in points {
let s = scan as usize;
flat[cursor[s]] = (tof, intensity);
cursor[s] += 1;
}
for s in 0..num_scans {
flat[offsets[s]..offsets[s + 1]].sort_unstable_by_key(|&(tof, _)| tof);
}
let n = num_scans + 2 * peak_count;
let mut values = vec![0u32; n];
values[0] = num_scans as u32;
for s in 0..num_scans.saturating_sub(1) {
values[s + 1] = counts[s] * 2;
}
let mut p = 0usize;
for s in 0..num_scans {
let mut current_sum: u32 = 0;
for &(tof, intensity) in &flat[offsets[s]..offsets[s + 1]] {
let delta = (tof + 1) - current_sum; current_sum = tof + 1;
values[num_scans + 2 * p] = delta;
values[num_scans + 1 + 2 * p] = intensity;
p += 1;
}
}
let mut transposed = vec![0u8; n * 4];
for (i, &v) in values.iter().enumerate() {
transposed[i] = (v & 0xFF) as u8;
transposed[i + n] = ((v >> 8) & 0xFF) as u8;
transposed[i + 2 * n] = ((v >> 16) & 0xFF) as u8;
transposed[i + 3 * n] = ((v >> 24) & 0xFF) as u8;
}
let compressed = zstd::bulk::compress(&transposed, 1).expect("zstd encode is infallible here");
let total_byte_count = (HEADER_BYTES + compressed.len()) as u32;
let mut record = Vec::with_capacity(total_byte_count as usize);
record.extend_from_slice(&total_byte_count.to_le_bytes());
record.extend_from_slice(&(num_scans as u32).to_le_bytes());
record.extend_from_slice(&compressed);
record
}
pub fn encode_empty_frame_type2(num_scans: usize) -> Vec<u8> {
let mut record = Vec::with_capacity(HEADER_BYTES);
record.extend_from_slice(&(HEADER_BYTES as u32).to_le_bytes());
record.extend_from_slice(&(num_scans as u32).to_le_bytes());
record
}
pub fn decode_frame_type2(record: &[u8]) -> Result<DecodedFrame, DecodeError> {
if record.len() < HEADER_BYTES {
return Err(DecodeError::ShortRecord);
}
let total_byte_count = u32::from_le_bytes(record[0..4].try_into().unwrap()) as usize;
if total_byte_count > record.len() || total_byte_count < HEADER_BYTES {
return Err(DecodeError::InvalidByteCount(total_byte_count));
}
let payload = &record[HEADER_BYTES..total_byte_count];
let bytes = zstd::decode_all(payload).map_err(DecodeError::Zstd)?;
if bytes.len() % 4 != 0 {
return Err(DecodeError::Misaligned);
}
let n = bytes.len() / 4;
let get = |i: usize| -> u32 {
bytes[i] as u32
| (bytes[i + n] as u32) << 8
| (bytes[i + 2 * n] as u32) << 16
| (bytes[i + 3 * n] as u32) << 24
};
let scan_count = get(0) as usize;
if scan_count > n {
return Err(DecodeError::ScanCountOverflow { scan_count, len: n });
}
let peak_count = (n - scan_count) / 2;
let mut scan_offsets = Vec::with_capacity(scan_count + 1);
scan_offsets.push(0usize);
for scan_index in 0..scan_count.saturating_sub(1) {
let size = (get(scan_index + 1) / 2) as usize;
scan_offsets.push(scan_offsets[scan_index] + size);
}
scan_offsets.push(peak_count);
let mut points = Vec::with_capacity(peak_count);
for scan_index in 0..scan_count {
let start = scan_offsets[scan_index];
let end = scan_offsets[scan_index + 1];
let mut current_sum: u32 = 0;
for peak_index in start..end {
current_sum += get(scan_count + 2 * peak_index);
let tof = current_sum - 1;
let intensity = get(scan_count + 1 + 2 * peak_index);
points.push((scan_index as u32, tof, intensity));
}
}
Ok((scan_count, points))
}