use crate::math::{ceil_log2, floor, pow2};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Imapb {
pub min: f64,
pub max: f64,
}
impl Imapb {
pub const fn new(min: f64, max: f64) -> Self {
Imapb { min, max }
}
fn scales(&self, len: usize) -> Option<(f64, f64, f64)> {
if !(1..=8).contains(&len)
|| self.max.partial_cmp(&self.min) != Some(core::cmp::Ordering::Greater)
|| !self.min.is_finite()
|| !self.max.is_finite()
{
return None;
}
let b_pow = ceil_log2(self.max - self.min);
let d_pow = 8 * len as i32 - 1;
let sf = pow2(d_pow - b_pow);
let sr = pow2(b_pow - d_pow);
let z = if self.min < 0.0 && self.max > 0.0 {
let s = sf * self.min;
s - floor(s)
} else {
0.0
};
Some((sf, sr, z))
}
pub fn decode(&self, bytes: &[u8]) -> Option<f64> {
let (_, sr, z) = self.scales(bytes.len())?;
let y = crate::klv::be_uint(bytes)?;
let bits = 8 * bytes.len() as u32;
if (y >> (bits - 2)) & 0b11 == 0b11 {
return None;
}
let x = sr * (y as f64 - z) + self.min;
if x > self.max + sr {
return None;
}
Some(x)
}
pub fn encode(&self, x: f64, out: &mut [u8]) -> bool {
let Some((sf, _, z)) = self.scales(out.len()) else {
return false;
};
if !x.is_finite() || x < self.min || x > self.max {
return false;
}
let y = floor(sf * (x - self.min) + z) as u64;
let bytes = y.to_be_bytes();
out.copy_from_slice(&bytes[8 - out.len()..]);
true
}
pub fn resolution(&self, len: usize) -> Option<f64> {
self.scales(len).map(|(_, sr, _)| sr)
}
}
pub fn decode(min: f64, max: f64, bytes: &[u8]) -> Option<f64> {
Imapb::new(min, max).decode(bytes)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn st0903_fov_examples() {
let m = Imapb::new(0.0, 180.0);
let mut b = [0u8; 2];
assert!(m.encode(10.0, &mut b));
assert_eq!(b, [0x05, 0x00]);
assert!(m.encode(90.0, &mut b));
assert_eq!(b, [0x2D, 0x00]);
assert_eq!(m.decode(&[0x2D, 0x00]), Some(90.0));
assert!(m.encode(12.5, &mut b));
assert!((m.decode(&b).unwrap() - 12.5).abs() < 1e-2);
}
#[test]
fn straddling_range_round_trips() {
let m = Imapb::new(-900.0, 19000.0);
let mut b = [0u8; 3];
for x in [-900.0, -1.5, 0.0, 10.0, 1234.567, 18999.9] {
assert!(m.encode(x, &mut b));
let back = m.decode(&b).unwrap();
assert!(
(back - x).abs() <= m.resolution(3).unwrap(),
"{x} -> {back}"
);
}
assert!(!m.encode(19000.5, &mut b));
assert!(!m.encode(f64::NAN, &mut b));
}
#[test]
fn specials_decode_to_none() {
let m = Imapb::new(0.0, 180.0);
assert_eq!(m.decode(&[0xC8, 0x00]), None); assert_eq!(m.decode(&[0xE8, 0x00]), None); assert_eq!(m.decode(&[0x7F, 0xFF]), None); assert_eq!(m.decode(&[]), None);
assert_eq!(m.decode(&[0; 9]), None);
}
}