openigtlink_rust/protocol/
header.rs

1//! OpenIGTLink protocol header implementation
2//!
3//! The header is a fixed 58-byte structure that precedes every OpenIGTLink message.
4
5use bytes::{Buf, BufMut, BytesMut};
6use crate::error::{IgtlError, Result};
7
8/// Type-safe wrapper for message type name (12 bytes, null-padded)
9#[derive(Debug, Clone, PartialEq, Eq)]
10pub struct TypeName([u8; 12]);
11
12impl TypeName {
13    /// Create a new TypeName from a string
14    pub fn new(name: &str) -> Result<Self> {
15        if name.len() > 12 {
16            return Err(IgtlError::InvalidHeader(
17                format!("Type name too long: {} bytes (max: 12)", name.len())
18            ));
19        }
20        let mut bytes = [0u8; 12];
21        bytes[..name.len()].copy_from_slice(name.as_bytes());
22        Ok(TypeName(bytes))
23    }
24
25    /// Get the type name as a string (trimming null bytes)
26    pub fn as_str(&self) -> Result<&str> {
27        let len = self.0.iter().position(|&b| b == 0).unwrap_or(12);
28        std::str::from_utf8(&self.0[..len])
29            .map_err(|_| IgtlError::InvalidHeader("Invalid UTF-8 in type name".to_string()))
30    }
31}
32
33impl From<[u8; 12]> for TypeName {
34    fn from(bytes: [u8; 12]) -> Self {
35        TypeName(bytes)
36    }
37}
38
39/// Type-safe wrapper for device name (20 bytes, null-padded)
40#[derive(Debug, Clone, PartialEq, Eq)]
41pub struct DeviceName([u8; 20]);
42
43impl DeviceName {
44    /// Create a new DeviceName from a string
45    pub fn new(name: &str) -> Result<Self> {
46        if name.len() > 20 {
47            return Err(IgtlError::InvalidHeader(
48                format!("Device name too long: {} bytes (max: 20)", name.len())
49            ));
50        }
51        let mut bytes = [0u8; 20];
52        bytes[..name.len()].copy_from_slice(name.as_bytes());
53        Ok(DeviceName(bytes))
54    }
55
56    /// Get the device name as a string (trimming null bytes)
57    pub fn as_str(&self) -> Result<&str> {
58        let len = self.0.iter().position(|&b| b == 0).unwrap_or(20);
59        std::str::from_utf8(&self.0[..len])
60            .map_err(|_| IgtlError::InvalidHeader("Invalid UTF-8 in device name".to_string()))
61    }
62}
63
64impl From<[u8; 20]> for DeviceName {
65    fn from(bytes: [u8; 20]) -> Self {
66        DeviceName(bytes)
67    }
68}
69
70/// High-precision timestamp for OpenIGTLink messages
71///
72/// The timestamp field in OpenIGTLink is a 64-bit value where:
73/// - Upper 32 bits: seconds since Unix epoch (UTC)
74/// - Lower 32 bits: fractional seconds in nanoseconds / 2^32
75///
76/// This provides nanosecond-level precision, critical for real-time tracking
77/// at 1000 Hz (1ms intervals).
78#[derive(Debug, Clone, Copy, PartialEq, Eq)]
79pub struct Timestamp {
80    /// Seconds since Unix epoch (1970-01-01 00:00:00 UTC)
81    pub seconds: u32,
82    /// Fractional seconds as a 32-bit value (nanoseconds * 2^32 / 1_000_000_000)
83    pub fraction: u32,
84}
85
86impl Timestamp {
87    /// Create a new timestamp from seconds and fraction
88    ///
89    /// # Arguments
90    /// * `seconds` - Seconds since Unix epoch
91    /// * `fraction` - Fractional seconds (0x00000000 to 0xFFFFFFFF represents 0.0 to ~1.0)
92    pub fn new(seconds: u32, fraction: u32) -> Self {
93        Timestamp { seconds, fraction }
94    }
95
96    /// Create a timestamp representing the current time
97    ///
98    /// # Examples
99    ///
100    /// ```
101    /// use openigtlink_rust::protocol::header::Timestamp;
102    ///
103    /// let ts = Timestamp::now();
104    /// assert!(ts.seconds > 0);
105    /// ```
106    pub fn now() -> Self {
107        let now = std::time::SystemTime::now()
108            .duration_since(std::time::UNIX_EPOCH)
109            .unwrap();
110
111        let seconds = now.as_secs() as u32;
112        // Convert nanoseconds to fraction: fraction = (nanoseconds * 2^32) / 10^9
113        let nanos = now.subsec_nanos();
114        let fraction = ((nanos as u64) * 0x1_0000_0000 / 1_000_000_000) as u32;
115
116        Timestamp { seconds, fraction }
117    }
118
119    /// Create a zero timestamp (no timestamp)
120    ///
121    /// # Examples
122    ///
123    /// ```
124    /// use openigtlink_rust::protocol::header::Timestamp;
125    ///
126    /// let ts = Timestamp::zero();
127    /// assert_eq!(ts.to_u64(), 0);
128    /// ```
129    pub fn zero() -> Self {
130        Timestamp {
131            seconds: 0,
132            fraction: 0,
133        }
134    }
135
136    /// Convert to OpenIGTLink wire format (u64)
137    ///
138    /// Upper 32 bits: seconds, Lower 32 bits: fraction
139    pub fn to_u64(self) -> u64 {
140        ((self.seconds as u64) << 32) | (self.fraction as u64)
141    }
142
143    /// Create from OpenIGTLink wire format (u64)
144    ///
145    /// Upper 32 bits: seconds, Lower 32 bits: fraction
146    pub fn from_u64(value: u64) -> Self {
147        Timestamp {
148            seconds: (value >> 32) as u32,
149            fraction: (value & 0xFFFFFFFF) as u32,
150        }
151    }
152
153    /// Convert to nanoseconds since Unix epoch
154    ///
155    /// # Examples
156    ///
157    /// ```
158    /// use openigtlink_rust::protocol::header::Timestamp;
159    ///
160    /// let ts = Timestamp::new(1000, 0x80000000); // 1000.5 seconds
161    /// assert_eq!(ts.to_nanos(), 1_000_500_000_000);
162    /// ```
163    pub fn to_nanos(self) -> u64 {
164        let sec_nanos = (self.seconds as u64) * 1_000_000_000;
165        let frac_nanos = ((self.fraction as u64) * 1_000_000_000) / 0x1_0000_0000;
166        sec_nanos + frac_nanos
167    }
168
169    /// Create from nanoseconds since Unix epoch
170    ///
171    /// # Examples
172    ///
173    /// ```
174    /// use openigtlink_rust::protocol::header::Timestamp;
175    ///
176    /// let ts = Timestamp::from_nanos(1_000_500_000_000); // 1000.5 seconds
177    /// assert_eq!(ts.seconds, 1000);
178    /// // Fraction should be approximately 0x80000000 (0.5)
179    /// assert!((ts.fraction as i64 - 0x80000000_i64).abs() < 100);
180    /// ```
181    pub fn from_nanos(nanos: u64) -> Self {
182        let seconds = (nanos / 1_000_000_000) as u32;
183        let remaining_nanos = (nanos % 1_000_000_000) as u32;
184        let fraction = ((remaining_nanos as u64) * 0x1_0000_0000 / 1_000_000_000) as u32;
185
186        Timestamp { seconds, fraction }
187    }
188
189    /// Convert to floating-point seconds
190    ///
191    /// # Examples
192    ///
193    /// ```
194    /// use openigtlink_rust::protocol::header::Timestamp;
195    ///
196    /// let ts = Timestamp::new(1000, 0x80000000); // 1000.5 seconds
197    /// assert!((ts.to_f64() - 1000.5).abs() < 0.0001);
198    /// ```
199    pub fn to_f64(self) -> f64 {
200        let frac_f64 = (self.fraction as f64) / (u32::MAX as f64 + 1.0);
201        (self.seconds as f64) + frac_f64
202    }
203}
204
205/// OpenIGTLink message header (58 bytes fixed size)
206///
207/// # Header Structure (all numerical values in big-endian)
208/// - Version: u16 (2 bytes)
209/// - Type: char[12] (12 bytes, null-padded)
210/// - Device Name: char[20] (20 bytes, null-padded)
211/// - Timestamp: u64 (8 bytes) - high 32 bits: seconds, low 32 bits: fraction
212/// - Body Size: u64 (8 bytes)
213/// - CRC: u64 (8 bytes)
214#[derive(Debug, Clone)]
215pub struct Header {
216    /// Protocol version number (2 for version 2 and 3)
217    pub version: u16,
218    /// Message type name
219    pub type_name: TypeName,
220    /// Unique device name
221    pub device_name: DeviceName,
222    /// High-precision timestamp (nanosecond resolution)
223    pub timestamp: Timestamp,
224    /// Size of the body in bytes
225    pub body_size: u64,
226    /// 64-bit CRC for body data
227    pub crc: u64,
228}
229
230impl Header {
231    /// Header size in bytes
232    pub const SIZE: usize = 58;
233
234    /// Decode a header from a byte slice
235    ///
236    /// # Arguments
237    /// * `buf` - Byte slice containing at least 58 bytes
238    ///
239    /// # Returns
240    /// Decoded header or error if buffer is too short
241    pub fn decode(buf: &[u8]) -> Result<Self> {
242        if buf.len() < Self::SIZE {
243            return Err(IgtlError::InvalidSize {
244                expected: Self::SIZE,
245                actual: buf.len(),
246            });
247        }
248
249        let mut cursor = std::io::Cursor::new(buf);
250
251        // Read version (2 bytes, big-endian)
252        let version = cursor.get_u16();
253
254        // Read type name (12 bytes)
255        let mut type_bytes = [0u8; 12];
256        cursor.copy_to_slice(&mut type_bytes);
257        let type_name = TypeName::from(type_bytes);
258
259        // Read device name (20 bytes)
260        let mut device_bytes = [0u8; 20];
261        cursor.copy_to_slice(&mut device_bytes);
262        let device_name = DeviceName::from(device_bytes);
263
264        // Read timestamp (8 bytes, big-endian) - convert from u64 to Timestamp
265        let timestamp_u64 = cursor.get_u64();
266        let timestamp = Timestamp::from_u64(timestamp_u64);
267
268        // Read body size (8 bytes, big-endian)
269        let body_size = cursor.get_u64();
270
271        // Read CRC (8 bytes, big-endian)
272        let crc = cursor.get_u64();
273
274        Ok(Header {
275            version,
276            type_name,
277            device_name,
278            timestamp,
279            body_size,
280            crc,
281        })
282    }
283
284    /// Encode the header into a byte vector
285    ///
286    /// # Returns
287    /// 58-byte vector containing the encoded header
288    pub fn encode(&self) -> Vec<u8> {
289        let mut buf = BytesMut::with_capacity(Self::SIZE);
290
291        // Write version (2 bytes, big-endian)
292        buf.put_u16(self.version);
293
294        // Write type name (12 bytes)
295        buf.put_slice(&self.type_name.0);
296
297        // Write device name (20 bytes)
298        buf.put_slice(&self.device_name.0);
299
300        // Write timestamp (8 bytes, big-endian) - convert Timestamp to u64
301        buf.put_u64(self.timestamp.to_u64());
302
303        // Write body size (8 bytes, big-endian)
304        buf.put_u64(self.body_size);
305
306        // Write CRC (8 bytes, big-endian)
307        buf.put_u64(self.crc);
308
309        buf.to_vec()
310    }
311}
312
313#[cfg(test)]
314mod tests {
315    use super::*;
316
317    #[test]
318    fn test_type_name_creation() {
319        let name = TypeName::new("TRANSFORM").unwrap();
320        assert_eq!(name.as_str().unwrap(), "TRANSFORM");
321    }
322
323    #[test]
324    fn test_type_name_too_long() {
325        let result = TypeName::new("VERY_LONG_TYPE_NAME");
326        assert!(result.is_err());
327    }
328
329    #[test]
330    fn test_device_name_creation() {
331        let name = DeviceName::new("TestDevice").unwrap();
332        assert_eq!(name.as_str().unwrap(), "TestDevice");
333    }
334
335    #[test]
336    fn test_header_size() {
337        assert_eq!(Header::SIZE, 58);
338    }
339
340    #[test]
341    fn test_timestamp_now() {
342        let ts = Timestamp::now();
343        assert!(ts.seconds > 0);
344        assert!(ts.to_f64() > 0.0);
345    }
346
347    #[test]
348    fn test_timestamp_zero() {
349        let ts = Timestamp::zero();
350        assert_eq!(ts.seconds, 0);
351        assert_eq!(ts.fraction, 0);
352        assert_eq!(ts.to_u64(), 0);
353    }
354
355    #[test]
356    fn test_timestamp_conversion() {
357        let ts = Timestamp::new(1000, 0x80000000); // 1000.5 seconds
358        assert_eq!(ts.seconds, 1000);
359        assert_eq!(ts.fraction, 0x80000000);
360
361        // Test to_nanos
362        let nanos = ts.to_nanos();
363        assert_eq!(nanos, 1_000_500_000_000);
364
365        // Test from_nanos roundtrip
366        let ts2 = Timestamp::from_nanos(nanos);
367        assert_eq!(ts2.seconds, ts.seconds);
368        // Allow small rounding error in fraction
369        assert!((ts2.fraction as i64 - ts.fraction as i64).abs() < 100);
370
371        // Test to_f64
372        let f = ts.to_f64();
373        assert!((f - 1000.5).abs() < 0.0001);
374    }
375
376    #[test]
377    fn test_timestamp_u64_roundtrip() {
378        let original = Timestamp::new(1234567890, 0xABCDEF12);
379        let u64_val = original.to_u64();
380        let restored = Timestamp::from_u64(u64_val);
381
382        assert_eq!(restored.seconds, original.seconds);
383        assert_eq!(restored.fraction, original.fraction);
384    }
385
386    #[test]
387    fn test_header_roundtrip() {
388        let original = Header {
389            version: 2,
390            type_name: TypeName::new("TRANSFORM").unwrap(),
391            device_name: DeviceName::new("TestDevice").unwrap(),
392            timestamp: Timestamp::new(1234567890, 0x12345678),
393            body_size: 48,
394            crc: 0xDEADBEEFCAFEBABE,
395        };
396
397        let encoded = original.encode();
398        assert_eq!(encoded.len(), Header::SIZE);
399
400        let decoded = Header::decode(&encoded).unwrap();
401        assert_eq!(decoded.version, original.version);
402        assert_eq!(decoded.type_name, original.type_name);
403        assert_eq!(decoded.device_name, original.device_name);
404        assert_eq!(decoded.timestamp, original.timestamp);
405        assert_eq!(decoded.body_size, original.body_size);
406        assert_eq!(decoded.crc, original.crc);
407    }
408
409    #[test]
410    fn test_header_decode_short_buffer() {
411        let short_buf = vec![0u8; 30];
412        let result = Header::decode(&short_buf);
413        assert!(matches!(result, Err(IgtlError::InvalidSize { .. })));
414    }
415
416    #[test]
417    fn test_big_endian_encoding() {
418        let header = Header {
419            version: 0x0102,
420            type_name: TypeName::new("TEST").unwrap(),
421            device_name: DeviceName::new("DEV").unwrap(),
422            timestamp: Timestamp::from_u64(0x0102030405060708),
423            body_size: 0x090A0B0C0D0E0F10,
424            crc: 0x1112131415161718,
425        };
426
427        let encoded = header.encode();
428
429        // Verify big-endian encoding of version
430        assert_eq!(encoded[0], 0x01);
431        assert_eq!(encoded[1], 0x02);
432
433        // Verify big-endian encoding of timestamp (at offset 34)
434        assert_eq!(encoded[34], 0x01);
435        assert_eq!(encoded[35], 0x02);
436        assert_eq!(encoded[36], 0x03);
437        assert_eq!(encoded[37], 0x04);
438    }
439}
openigtlink_rust/protocol/header.rs

openigtlink_rust/protocol/
header.rs
