use crate::imapb::Imapb;
use crate::klv::{self, be_uint, read_ber_length, read_ber_oid, Items};
use crate::{Error, GeoPoint};
pub const KEY: [u8; 16] = [
0x06, 0x0E, 0x2B, 0x34, 0x02, 0x0B, 0x01, 0x01, 0x0E, 0x01, 0x03, 0x03, 0x06, 0x00, 0x00, 0x00,
];
pub const VERSION: u64 = 6;
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
#[non_exhaustive]
#[repr(u8)]
pub enum Tag {
Checksum = 1,
PrecisionTimeStamp = 2,
SystemName = 3,
Version = 4,
TotalTargetsDetected = 5,
TargetsReported = 6,
FrameNumber = 7,
FrameWidth = 8,
FrameHeight = 9,
SourceSensor = 10,
HorizontalFov = 11,
VerticalFov = 12,
MiisId = 13,
VTargetSeries = 101,
AlgorithmSeries = 102,
OntologySeries = 103,
}
const FOV: Imapb = Imapb::new(0.0, 180.0);
const OFFSET: Imapb = Imapb::new(-19.2, 19.2);
const HEIGHT: Imapb = Imapb::new(-900.0, 19000.0);
const LAT: Imapb = Imapb::new(-90.0, 90.0);
const LON: Imapb = Imapb::new(-180.0, 180.0);
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct VmtiSet<'a> {
body: &'a [u8],
}
impl<'a> VmtiSet<'a> {
pub fn new(body: &'a [u8]) -> Self {
VmtiSet { body }
}
pub fn parse(buf: &'a [u8]) -> Result<(VmtiSet<'a>, usize), Error> {
let (body, used) = klv::parse_checked(buf, &KEY, true)?;
Ok((VmtiSet { body }, used))
}
pub fn body(&self) -> &'a [u8] {
self.body
}
pub fn items(&self) -> Items<'a> {
Items::new(self.body)
}
pub fn raw(&self, tag: Tag) -> Option<&'a [u8]> {
klv::find(self.body, tag as u32)
}
fn uint(&self, tag: Tag) -> Option<u64> {
self.raw(tag).and_then(be_uint)
}
pub fn timestamp(&self) -> Option<u64> {
self.raw(Tag::PrecisionTimeStamp)
.filter(|r| r.len() == 8)
.and_then(be_uint)
}
pub fn system_name(&self) -> Option<&'a str> {
core::str::from_utf8(self.raw(Tag::SystemName)?).ok()
}
pub fn source_sensor(&self) -> Option<&'a str> {
core::str::from_utf8(self.raw(Tag::SourceSensor)?).ok()
}
pub fn version(&self) -> Option<u64> {
self.uint(Tag::Version)
}
pub fn total_targets_detected(&self) -> Option<u64> {
self.uint(Tag::TotalTargetsDetected)
}
pub fn targets_reported(&self) -> Option<u64> {
self.uint(Tag::TargetsReported)
}
pub fn frame_number(&self) -> Option<u64> {
self.uint(Tag::FrameNumber)
}
pub fn frame_width(&self) -> Option<u64> {
self.uint(Tag::FrameWidth)
}
pub fn frame_height(&self) -> Option<u64> {
self.uint(Tag::FrameHeight)
}
pub fn horizontal_fov(&self) -> Option<f64> {
FOV.decode(self.raw(Tag::HorizontalFov)?)
}
pub fn vertical_fov(&self) -> Option<f64> {
FOV.decode(self.raw(Tag::VerticalFov)?)
}
pub fn targets(&self) -> Targets<'a> {
Targets {
rest: self.raw(Tag::VTargetSeries).unwrap_or(&[]),
failed: false,
}
}
}
#[derive(Clone, Debug)]
pub struct Targets<'a> {
rest: &'a [u8],
failed: bool,
}
impl<'a> Targets<'a> {
fn step(&mut self) -> Result<VTarget<'a>, Error> {
let (len, n) = read_ber_length(self.rest)?;
let end = n.checked_add(len).ok_or(Error::BadLength)?;
let pack = self.rest.get(n..end).ok_or(Error::Truncated)?;
self.rest = &self.rest[end..];
let (id, m) = read_ber_oid(pack)?;
Ok(VTarget {
id,
body: &pack[m..],
})
}
}
impl<'a> Iterator for Targets<'a> {
type Item = Result<VTarget<'a>, Error>;
fn next(&mut self) -> Option<Self::Item> {
if self.rest.is_empty() || self.failed {
return None;
}
let r = self.step();
self.failed = r.is_err();
Some(r)
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
#[non_exhaustive]
#[repr(u8)]
pub enum TargetTag {
Centroid = 1,
BoundaryTopLeft = 2,
BoundaryBottomRight = 3,
Priority = 4,
Confidence = 5,
History = 6,
PercentagePixels = 7,
Color = 8,
Intensity = 9,
LocationOffsetLat = 10,
LocationOffsetLon = 11,
Height = 12,
BoxTopLeftLatOffset = 13,
BoxTopLeftLonOffset = 14,
BoxBottomRightLatOffset = 15,
BoxBottomRightLonOffset = 16,
Location = 17,
BoundarySeries = 18,
CentroidRow = 19,
CentroidColumn = 20,
FpaIndex = 21,
AlgorithmId = 22,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct VTarget<'a> {
pub id: u64,
body: &'a [u8],
}
impl<'a> VTarget<'a> {
pub fn items(&self) -> Items<'a> {
Items::new(self.body)
}
pub fn raw(&self, tag: TargetTag) -> Option<&'a [u8]> {
klv::find(self.body, tag as u32)
}
fn uint(&self, tag: TargetTag) -> Option<u64> {
self.raw(tag).and_then(be_uint)
}
pub fn centroid_pixel(&self) -> Option<u64> {
self.uint(TargetTag::Centroid)
}
pub fn centroid_row_col(&self, frame_width: u64) -> Option<(u64, u64)> {
if let (Some(r), Some(c)) = (
self.uint(TargetTag::CentroidRow),
self.uint(TargetTag::CentroidColumn),
) {
return Some((r, c));
}
pixel_to_row_col(self.centroid_pixel()?, frame_width)
}
pub fn bounding_box(&self) -> Option<(u64, u64)> {
Some((
self.uint(TargetTag::BoundaryTopLeft)?,
self.uint(TargetTag::BoundaryBottomRight)?,
))
}
pub fn priority(&self) -> Option<u8> {
self.uint(TargetTag::Priority).map(|v| v as u8)
}
pub fn confidence(&self) -> Option<u8> {
self.uint(TargetTag::Confidence).map(|v| v as u8)
}
pub fn history(&self) -> Option<u64> {
self.uint(TargetTag::History)
}
pub fn color(&self) -> Option<u32> {
self.uint(TargetTag::Color).map(|v| v as u32)
}
pub fn location_offset(&self) -> Option<(f64, f64)> {
Some((
OFFSET.decode(self.raw(TargetTag::LocationOffsetLat)?)?,
OFFSET.decode(self.raw(TargetTag::LocationOffsetLon)?)?,
))
}
pub fn height(&self) -> Option<f64> {
HEIGHT.decode(self.raw(TargetTag::Height)?)
}
pub fn location(&self) -> Option<GeoPoint> {
let p = self.raw(TargetTag::Location)?;
if p.len() < 10 {
return None;
}
Some(GeoPoint {
lat: LAT.decode(&p[0..4])?,
lon: LON.decode(&p[4..8])?,
height: HEIGHT.decode(&p[8..10]),
})
}
pub fn ground_position(&self, frame_center: Option<GeoPoint>) -> Option<GeoPoint> {
if let Some(p) = self.location() {
return Some(p);
}
let fc = frame_center?;
let (dlat, dlon) = self.location_offset()?;
Some(GeoPoint {
lat: fc.lat + dlat,
lon: fc.lon + dlon,
height: self.height(),
})
}
pub fn algorithm_id(&self) -> Option<u64> {
self.uint(TargetTag::AlgorithmId)
}
}
pub fn pixel_to_row_col(pixel: u64, frame_width: u64) -> Option<(u64, u64)> {
if pixel == 0 || frame_width == 0 {
return None;
}
Some(((pixel - 1) / frame_width + 1, (pixel - 1) % frame_width + 1))
}
#[cfg(feature = "alloc")]
pub use builder::{Builder, TargetBuilder};
#[cfg(feature = "alloc")]
mod builder {
use super::*;
use crate::klv::enc;
use alloc::vec::Vec;
#[derive(Clone, Debug)]
pub struct TargetBuilder {
id: u64,
body: Vec<u8>,
}
impl TargetBuilder {
pub fn new(id: u64) -> Self {
TargetBuilder {
id,
body: Vec::new(),
}
}
pub fn raw(&mut self, tag: u32, value: &[u8]) -> Result<&mut Self, Error> {
enc::push_item(&mut self.body, tag, value)?;
Ok(self)
}
pub fn centroid_pixel(&mut self, pixel: u64) -> Result<&mut Self, Error> {
self.raw(1, &enc::min_uint(pixel))
}
pub fn bounding_box(
&mut self,
top_left: u64,
bottom_right: u64,
) -> Result<&mut Self, Error> {
self.raw(2, &enc::min_uint(top_left))?
.raw(3, &enc::min_uint(bottom_right))
}
pub fn priority(&mut self, p: u8) -> Result<&mut Self, Error> {
self.raw(4, &[p])
}
pub fn confidence(&mut self, c: u8) -> Result<&mut Self, Error> {
if c > 100 {
return Err(Error::OutOfRange { tag: 5 });
}
self.raw(5, &[c])
}
pub fn history(&mut self, frames: u64) -> Result<&mut Self, Error> {
self.raw(6, &enc::min_uint(frames))
}
pub fn location(&mut self, lat: f64, lon: f64, hae: f64) -> Result<&mut Self, Error> {
let mut p = [0u8; 10];
if !(LAT.encode(lat, &mut p[0..4])
&& LON.encode(lon, &mut p[4..8])
&& HEIGHT.encode(hae, &mut p[8..10]))
{
return Err(Error::OutOfRange { tag: 17 });
}
self.raw(17, &p)
}
pub fn location_offset(&mut self, dlat: f64, dlon: f64) -> Result<&mut Self, Error> {
let mut a = [0u8; 3];
let mut b = [0u8; 3];
if !(OFFSET.encode(dlat, &mut a) && OFFSET.encode(dlon, &mut b)) {
return Err(Error::OutOfRange { tag: 10 });
}
self.raw(10, &a)?.raw(11, &b)
}
pub fn centroid_row_col(&mut self, row: u64, col: u64) -> Result<&mut Self, Error> {
self.raw(19, &enc::min_uint(row))?
.raw(20, &enc::min_uint(col))
}
pub fn algorithm_id(&mut self, id: u64) -> Result<&mut Self, Error> {
self.raw(22, &enc::min_uint(id))
}
fn pack(&self) -> Result<Vec<u8>, Error> {
let mut b = [0u8; 10];
let n = klv::write_ber_oid(self.id, &mut b)?;
let mut v = b[..n].to_vec();
v.extend_from_slice(&self.body);
Ok(v)
}
}
#[derive(Clone, Debug)]
pub struct Builder {
header: Vec<u8>,
series: Vec<u8>,
targets: u64,
}
impl Builder {
pub fn new(timestamp_us: u64) -> Self {
let mut header = Vec::new();
enc::push_item(&mut header, 2, ×tamp_us.to_be_bytes()).expect("small item");
Builder {
header,
series: Vec::new(),
targets: 0,
}
}
pub fn raw(&mut self, tag: u32, value: &[u8]) -> Result<&mut Self, Error> {
enc::push_item(&mut self.header, tag, value)?;
Ok(self)
}
pub fn system_name(&mut self, name: &str) -> Result<&mut Self, Error> {
if name.len() > 32 {
return Err(Error::BadValue { tag: 3 });
}
self.raw(3, name.as_bytes())
}
pub fn source_sensor(&mut self, name: &str) -> Result<&mut Self, Error> {
if name.len() > 32 {
return Err(Error::BadValue { tag: 10 });
}
self.raw(10, name.as_bytes())
}
pub fn frame_size(&mut self, width: u64, height: u64) -> Result<&mut Self, Error> {
self.raw(8, &enc::min_uint(width))?
.raw(9, &enc::min_uint(height))
}
pub fn frame_number(&mut self, n: u64) -> Result<&mut Self, Error> {
self.raw(7, &enc::min_uint(n))
}
pub fn fov(&mut self, horizontal: f64, vertical: f64) -> Result<&mut Self, Error> {
let mut h = [0u8; 2];
let mut v = [0u8; 2];
if !(FOV.encode(horizontal, &mut h) && FOV.encode(vertical, &mut v)) {
return Err(Error::OutOfRange { tag: 11 });
}
self.raw(11, &h)?.raw(12, &v)
}
pub fn target(&mut self, t: &TargetBuilder) -> Result<&mut Self, Error> {
let pack = t.pack()?;
enc::push_ber_length(&mut self.series, pack.len())?;
self.series.extend_from_slice(&pack);
self.targets += 1;
Ok(self)
}
pub fn body(&self) -> Vec<u8> {
let mut b = self.header.clone();
enc::push_item(&mut b, 4, &enc::min_uint(VERSION)).expect("small item");
enc::push_item(&mut b, 5, &enc::min_uint(self.targets)).expect("small item");
enc::push_item(&mut b, 6, &enc::min_uint(self.targets)).expect("small item");
if !self.series.is_empty() {
enc::push_item(&mut b, 101, &self.series).expect("series under 4 GiB");
}
b
}
pub fn finish(&self) -> Vec<u8> {
enc::finish_with_checksum(&KEY, &self.body()).expect("packet under 4 GiB")
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn pixel_numbers() {
assert_eq!(pixel_to_row_col(1, 1280), Some((1, 1)));
assert_eq!(pixel_to_row_col(1280, 1280), Some((1, 1280)));
assert_eq!(pixel_to_row_col(1281, 1280), Some((2, 1)));
assert_eq!(pixel_to_row_col(0, 1280), None);
}
#[test]
fn hostile_series_does_not_panic() {
let body = [101u8, 4, 0x84, 0xFF, 0xFF, 0xFF];
let set = VmtiSet::new(&body);
let mut t = set.targets();
assert!(t.next().unwrap().is_err());
assert!(t.next().is_none());
}
#[cfg(feature = "alloc")]
#[test]
fn standalone_round_trip() {
let mut b = Builder::new(42);
b.system_name("unit")
.unwrap()
.frame_size(640, 480)
.unwrap()
.fov(30.0, 22.5)
.unwrap()
.frame_number(9)
.unwrap();
let mut t = TargetBuilder::new(300);
t.centroid_pixel(640 * 10 + 5)
.unwrap()
.bounding_box(1, 640 * 20)
.unwrap()
.priority(1)
.unwrap();
t.location_offset(0.001, -0.002)
.unwrap()
.history(12)
.unwrap();
b.target(&t).unwrap();
b.target(&TargetBuilder::new(5)).unwrap();
let bytes = b.finish();
let (set, used) = VmtiSet::parse(&bytes).unwrap();
assert_eq!(used, bytes.len());
assert_eq!(set.timestamp(), Some(42));
assert_eq!(set.frame_width(), Some(640));
assert_eq!(set.targets_reported(), Some(2));
assert_eq!(set.version(), Some(VERSION));
assert!((set.horizontal_fov().unwrap() - 30.0).abs() < 0.01);
let ts: Result<alloc::vec::Vec<_>, _> = set.targets().collect();
let ts = ts.unwrap();
assert_eq!(ts.len(), 2);
assert_eq!(ts[0].id, 300);
assert_eq!(ts[0].centroid_row_col(640), Some((11, 5)));
let fc = GeoPoint {
lat: 10.0,
lon: 20.0,
height: None,
};
let g = ts[0].ground_position(Some(fc)).unwrap();
assert!((g.lat - 10.001).abs() < 1e-5 && (g.lon - 19.998).abs() < 1e-5);
assert_eq!(ts[1].id, 5);
assert_eq!(ts[1].centroid_pixel(), None);
}
}