use crate::bytes::{Reader, Writer};
use crate::error::{Error, Result};
#[cfg(feature = "net")]
pub mod net;
pub const MAGIC: u32 = 0x4947_5541;
pub const PREFIX_LEN: usize = 12;
pub const KIND_HELLO: u16 = 1;
pub const KIND_CAMERA_DESC: u16 = 2;
pub const KIND_IMAGE_FRAME: u16 = 3;
pub const KIND_HEARTBEAT: u16 = 4;
pub const KIND_BYE: u16 = 5;
pub const HELLO_HEADER_LEN: usize = 41;
pub const CAMERA_DESC_HEADER_LEN: usize = 72;
pub const IMAGE_FRAME_HEADER_LEN: usize = 40;
pub const PROTO_MAJOR: u16 = 1;
pub const PROTO_MINOR: u16 = 0;
pub const MODE_LISTEN: u8 = 0;
pub const MODE_CONNECT: u8 = 1;
pub const FLAG_MID_EXPOSURE_VALID: u8 = 0x01;
pub const ENCODING_NONE: u8 = 0;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
#[non_exhaustive]
pub enum ImageEncoding {
Mono8,
Jpeg,
Other(u8),
}
impl ImageEncoding {
#[must_use]
pub const fn from_wire(value: u8) -> Self {
match value {
1 => Self::Mono8,
2 => Self::Jpeg,
other => Self::Other(other),
}
}
#[must_use]
pub const fn to_wire(self) -> u8 {
match self {
Self::Mono8 => 1,
Self::Jpeg => 2,
Self::Other(value) => value,
}
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct Hello {
pub proto_major: u16,
pub proto_minor: u16,
pub clock_hz: u64,
pub qpf: u64,
pub natnet_major: u16,
pub natnet_minor: u16,
pub motive_build: u32,
pub host_wall_unix_secs: u64,
pub mode: u8,
pub camera_count: u32,
pub bridge_id: String,
pub profile: String,
pub calibration: String,
}
impl Hello {
pub fn to_record(&self) -> Result<Vec<u8>> {
let mut h = Writer::new();
h.u16(self.proto_major);
h.u16(self.proto_minor);
h.u64(self.clock_hz);
h.u64(self.qpf);
h.u16(self.natnet_major);
h.u16(self.natnet_minor);
h.u32(self.motive_build);
h.u64(self.host_wall_unix_secs);
h.u8(self.mode);
h.u32(self.camera_count);
let mut p = Writer::new();
write_lp_str(&mut p, &self.bridge_id, "Hello::bridge_id")?;
write_lp_str(&mut p, &self.profile, "Hello::profile")?;
write_lp_str(&mut p, &self.calibration, "Hello::calibration")?;
encode_record(KIND_HELLO, &h.into_vec(), &p.into_vec())
}
fn parse(header: &[u8], payload: &[u8]) -> Result<Self> {
let mut r = Reader::new(header);
let proto_major = r.u16()?;
let proto_minor = r.u16()?;
let clock_hz = r.u64()?;
let qpf = r.u64()?;
let natnet_major = r.u16()?;
let natnet_minor = r.u16()?;
let motive_build = r.u32()?;
let host_wall_unix_secs = r.u64()?;
let mode = r.u8()?;
let camera_count = r.u32()?;
let mut p = Reader::new(payload);
let bridge_id = read_lp_str(&mut p)?;
let profile = read_lp_str(&mut p)?;
let calibration = read_lp_str(&mut p)?;
p.finish()?;
Ok(Self {
proto_major,
proto_minor,
clock_hz,
qpf,
natnet_major,
natnet_minor,
motive_build,
host_wall_unix_secs,
mode,
camera_count,
bridge_id,
profile,
calibration,
})
}
}
#[derive(Debug, Clone, PartialEq)]
pub struct CameraDesc<'a> {
pub camera_index: u32,
pub camera_id: u32,
pub serial: u32,
pub video_mode: u8,
pub encoding: ImageEncoding,
pub contributes_to_solve: bool,
pub frame_rate_divisor: u8,
pub width: u16,
pub height: u16,
pub position: [f32; 3],
pub orientation: [f32; 9],
pub ray_grid_w: u16,
pub ray_grid_h: u16,
pub rays: &'a [u8],
pub name: &'a str,
}
const RAY_BYTES: usize = 24;
fn ray_grid_bytes(ray_grid_w: u16, ray_grid_h: u16, available: usize) -> Result<usize> {
usize::from(ray_grid_w)
.checked_mul(usize::from(ray_grid_h))
.and_then(|cells| cells.checked_mul(RAY_BYTES))
.ok_or(Error::ImplausibleCount {
count: usize::from(ray_grid_w).saturating_mul(usize::from(ray_grid_h)),
available,
})
}
impl<'a> CameraDesc<'a> {
#[must_use]
pub fn ray(&self, x: usize, y: usize) -> Option<[f32; 6]> {
if x >= usize::from(self.ray_grid_w) || y >= usize::from(self.ray_grid_h) {
return None;
}
let start = (y * usize::from(self.ray_grid_w) + x).checked_mul(RAY_BYTES)?;
let chunk = self.rays.get(start..start.checked_add(RAY_BYTES)?)?;
let mut r = Reader::new(chunk);
let mut out = [0.0f32; 6];
for slot in &mut out {
*slot = r.f32().ok()?;
}
Some(out)
}
pub fn to_record(&self) -> Result<Vec<u8>> {
let declared = match ray_grid_bytes(self.ray_grid_w, self.ray_grid_h, self.rays.len()) {
Ok(declared) => declared,
Err(Error::ImplausibleCount { count, .. }) => {
return Err(Error::LengthOverflow {
field: "CameraDesc::rays",
length: count.saturating_mul(RAY_BYTES),
max: usize::MAX,
});
}
Err(error) => return Err(error),
};
if declared != self.rays.len() {
return Err(Error::SectionSizeMismatch {
declared,
consumed: self.rays.len(),
});
}
if let Some(offset) = self.name.bytes().position(|b| b == 0) {
return Err(Error::InteriorNul {
field: "CameraDesc::name",
offset,
});
}
let mut h = Writer::new();
h.u32(self.camera_index);
h.u32(self.camera_id);
h.u32(self.serial);
h.u8(self.video_mode);
h.u8(self.encoding.to_wire());
h.u8(u8::from(self.contributes_to_solve));
h.u8(self.frame_rate_divisor);
h.u16(self.width);
h.u16(self.height);
h.vec3(self.position);
for value in self.orientation {
h.f32(value);
}
h.u16(self.ray_grid_w);
h.u16(self.ray_grid_h);
let mut p = Writer::new();
p.bytes(self.rays);
p.cstr(self.name);
encode_record(KIND_CAMERA_DESC, &h.into_vec(), &p.into_vec())
}
fn parse(header: &[u8], payload: &'a [u8]) -> Result<Self> {
let mut r = Reader::new(header);
let camera_index = r.u32()?;
let camera_id = r.u32()?;
let serial = r.u32()?;
let video_mode = r.u8()?;
let encoding = ImageEncoding::from_wire(r.u8()?);
let contributes_to_solve = r.u8()? != 0;
let frame_rate_divisor = r.u8()?;
let width = r.u16()?;
let height = r.u16()?;
let position = r.vec3()?;
let mut orientation = [0.0f32; 9];
for slot in &mut orientation {
*slot = r.f32()?;
}
let ray_grid_w = r.u16()?;
let ray_grid_h = r.u16()?;
let cells = ray_grid_bytes(ray_grid_w, ray_grid_h, payload.len())?;
let rays = payload.get(..cells).ok_or(Error::UnexpectedEof {
offset: 0,
needed: cells,
available: payload.len(),
})?;
let rest = payload.get(cells..).unwrap_or_default();
let end = rest
.iter()
.position(|&b| b == 0)
.ok_or(Error::UnterminatedString { offset: cells })?;
if end + 1 != rest.len() {
return Err(Error::TrailingBytes {
remaining: rest.len() - end - 1,
});
}
let name = utf8_prefix(rest.get(..end).unwrap_or_default());
Ok(Self {
camera_index,
camera_id,
serial,
video_mode,
encoding,
contributes_to_solve,
frame_rate_divisor,
width,
height,
position,
orientation,
ray_grid_w,
ray_grid_h,
rays,
name,
})
}
}
#[derive(Debug, Clone, PartialEq)]
pub struct ImageFrame<'a> {
pub frame_number: u32,
pub camera_index: u32,
pub camera_mid_exposure: Option<u64>,
pub grab_ticks: u64,
pub api_timestamp_secs: f64,
pub width: u16,
pub height: u16,
pub encoding: ImageEncoding,
pub payload: &'a [u8],
}
impl<'a> ImageFrame<'a> {
#[must_use]
pub const fn natnet_frame_number(&self) -> i32 {
self.frame_number as i32
}
#[must_use]
pub const fn matches_natnet_frame(&self, natnet_frame_number: i32) -> bool {
self.natnet_frame_number() == natnet_frame_number
}
pub fn to_record_head(&self) -> Result<Vec<u8>> {
let mut h = Writer::new();
h.u32(self.frame_number);
h.u32(self.camera_index);
h.u64(self.camera_mid_exposure.unwrap_or(0));
h.u64(self.grab_ticks);
h.f64(self.api_timestamp_secs);
h.u16(self.width);
h.u16(self.height);
h.u8(self.encoding.to_wire());
h.u8(if self.camera_mid_exposure.is_some() {
FLAG_MID_EXPOSURE_VALID
} else {
0
});
h.u16(0); let header = h.into_vec();
let (header_len, payload_len) = wire_lens(header.len(), self.payload.len())?;
let mut out = Writer::new();
write_prefix(&mut out, KIND_IMAGE_FRAME, header_len, payload_len);
out.bytes(&header);
Ok(out.into_vec())
}
pub fn to_record(&self) -> Result<Vec<u8>> {
let mut out = self.to_record_head()?;
out.extend_from_slice(self.payload);
Ok(out)
}
fn parse(header: &[u8], payload: &'a [u8]) -> Result<Self> {
let mut r = Reader::new(header);
let frame_number = r.u32()?;
let camera_index = r.u32()?;
let ticks = r.u64()?;
let grab_ticks = r.u64()?;
let api_timestamp_secs = r.f64()?;
let width = r.u16()?;
let height = r.u16()?;
let encoding = ImageEncoding::from_wire(r.u8()?);
let flags = r.u8()?;
let _reserved = r.u16()?;
Ok(Self {
frame_number,
camera_index,
camera_mid_exposure: (flags & FLAG_MID_EXPOSURE_VALID != 0).then_some(ticks),
grab_ticks,
api_timestamp_secs,
width,
height,
encoding,
payload,
})
}
}
#[derive(Debug, Clone, PartialEq)]
#[non_exhaustive]
pub enum Record<'a> {
Hello(Hello),
CameraDesc(CameraDesc<'a>),
ImageFrame(ImageFrame<'a>),
Heartbeat,
Bye,
Other {
kind: u16,
header: &'a [u8],
payload: &'a [u8],
},
}
pub fn record_len(prefix: &[u8]) -> Result<Option<usize>> {
if prefix.len() < PREFIX_LEN {
return Ok(None);
}
let mut r = Reader::new(prefix);
let magic = r.u32()?;
if magic != MAGIC {
return Err(Error::InvalidMagic {
found: magic,
expected: MAGIC,
});
}
let _kind = r.u16()?;
let header_len = usize::from(r.u16()?);
let payload_len = r.u32()? as usize;
let total = PREFIX_LEN
.checked_add(header_len)
.and_then(|n| n.checked_add(payload_len))
.ok_or(Error::ImplausibleCount {
count: payload_len,
available: prefix.len(),
})?;
Ok(Some(total))
}
pub fn parse_record(bytes: &[u8]) -> Result<Record<'_>> {
let Some(total) = record_len(bytes)? else {
return Err(Error::UnexpectedEof {
offset: 0,
needed: PREFIX_LEN,
available: bytes.len(),
});
};
if bytes.len() < total {
return Err(Error::UnexpectedEof {
offset: 0,
needed: total,
available: bytes.len(),
});
}
let mut r = Reader::new(bytes);
let _magic = r.u32()?;
let kind = r.u16()?;
let header_len = usize::from(r.u16()?);
let payload_len = r.u32()? as usize;
let header = r.take(header_len)?;
let payload = r.take(payload_len)?;
match kind {
KIND_HELLO => Ok(Record::Hello(Hello::parse(header, payload)?)),
KIND_CAMERA_DESC => Ok(Record::CameraDesc(CameraDesc::parse(header, payload)?)),
KIND_IMAGE_FRAME => Ok(Record::ImageFrame(ImageFrame::parse(header, payload)?)),
KIND_HEARTBEAT => Ok(Record::Heartbeat),
KIND_BYE => Ok(Record::Bye),
kind => Ok(Record::Other {
kind,
header,
payload,
}),
}
}
pub fn encode_record(kind: u16, header: &[u8], payload: &[u8]) -> Result<Vec<u8>> {
let (header_len, payload_len) = wire_lens(header.len(), payload.len())?;
let mut w = Writer::new();
write_prefix(&mut w, kind, header_len, payload_len);
w.bytes(header);
w.bytes(payload);
Ok(w.into_vec())
}
#[must_use]
pub fn heartbeat_record() -> Vec<u8> {
empty_record(KIND_HEARTBEAT)
}
#[must_use]
pub fn bye_record() -> Vec<u8> {
empty_record(KIND_BYE)
}
fn empty_record(kind: u16) -> Vec<u8> {
let mut w = Writer::new();
write_prefix(&mut w, kind, 0, 0);
w.into_vec()
}
fn wire_lens(header_len: usize, payload_len: usize) -> Result<(u16, u32)> {
let header = u16::try_from(header_len).map_err(|_| Error::LengthOverflow {
field: "header",
length: header_len,
max: usize::from(u16::MAX),
})?;
let payload = u32::try_from(payload_len).map_err(|_| Error::LengthOverflow {
field: "payload",
length: payload_len,
max: usize::try_from(u32::MAX).unwrap_or(usize::MAX),
})?;
Ok((header, payload))
}
fn write_prefix(w: &mut Writer, kind: u16, header_len: u16, payload_len: u32) {
w.u32(MAGIC);
w.u16(kind);
w.u16(header_len);
w.u32(payload_len);
}
fn write_lp_str(w: &mut Writer, s: &str, field: &'static str) -> Result<()> {
let len = u16::try_from(s.len()).map_err(|_| Error::LengthOverflow {
field,
length: s.len(),
max: usize::from(u16::MAX),
})?;
w.u16(len);
w.bytes(s.as_bytes());
Ok(())
}
fn read_lp_str(r: &mut Reader<'_>) -> Result<String> {
let len = usize::from(r.u16()?);
Ok(String::from_utf8_lossy(r.take(len)?).into_owned())
}
fn utf8_prefix(bytes: &[u8]) -> &str {
match core::str::from_utf8(bytes) {
Ok(s) => s,
Err(e) => bytes
.get(..e.valid_up_to())
.and_then(|b| core::str::from_utf8(b).ok())
.unwrap_or(""),
}
}