phytrace_sdk/models/domains/

common.rs

//! Common types shared across UDM domains.
//!
//! These types are used by multiple domains and represent shared concepts
//! like object references, poses, dimensions, and coordinates.

use serde::{Deserialize, Serialize};
use validator::Validate;

/// Reference to an object in the world or payload.
///
/// Used for consistent object identification across domains (perception,
/// manipulation, payload, etc.). Supports multiple ID schemes for different
/// use cases.
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct ObjectRef {
    /// Session-stable object identifier (assigned by perception system)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub object_id: Option<String>,

    /// External business system ID (WMS, ERP, etc.)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub item_id: Option<String>,

    /// Object type classification (package, pallet, tool, person, etc.)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub object_type: Option<String>,

    /// Specific object class (e.g., "cardboard_box", "metal_pallet")
    #[serde(skip_serializing_if = "Option::is_none")]
    pub object_class: Option<String>,

    /// Perception system tracking ID
    #[serde(skip_serializing_if = "Option::is_none")]
    pub tracking_id: Option<String>,

    /// Object dimensions
    #[serde(skip_serializing_if = "Option::is_none")]
    pub dimensions_m: Option<Dimensions>,

    /// Object mass in kilograms
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = 0.0))]
    pub mass_kg: Option<f64>,

    /// Object pose (position and orientation)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub pose: Option<Pose>,

    /// Reference frame for the pose
    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,
}

/// 3D dimensions of an object.
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct Dimensions {
    /// Length in meters (x-axis)
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = 0.0))]
    pub length_m: Option<f64>,

    /// Width in meters (y-axis)
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = 0.0))]
    pub width_m: Option<f64>,

    /// Height in meters (z-axis)
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = 0.0))]
    pub height_m: Option<f64>,
}

impl Dimensions {
    /// Create new dimensions with all values.
    pub fn new(length_m: f64, width_m: f64, height_m: f64) -> Self {
        Self {
            length_m: Some(length_m),
            width_m: Some(width_m),
            height_m: Some(height_m),
        }
    }

    /// Calculate volume in cubic meters.
    pub fn volume_m3(&self) -> Option<f64> {
        match (self.length_m, self.width_m, self.height_m) {
            (Some(l), Some(w), Some(h)) => Some(l * w * h),
            _ => None,
        }
    }
}

/// 6-DOF pose (position + orientation).
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct Pose {
    /// Position component
    #[serde(skip_serializing_if = "Option::is_none")]
    pub position: Option<Position3D>,

    /// Orientation as quaternion
    #[serde(skip_serializing_if = "Option::is_none")]
    pub orientation: Option<Quaternion>,

    /// Orientation as Euler angles (alternative to quaternion)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub euler_deg: Option<EulerAngles>,
}

impl Pose {
    /// Create a pose from position only (identity orientation).
    pub fn from_position(x: f64, y: f64, z: f64) -> Self {
        Self {
            position: Some(Position3D::new(x, y, z)),
            orientation: None,
            euler_deg: None,
        }
    }

    /// Create a pose from position and Euler angles.
    pub fn from_position_euler(x: f64, y: f64, z: f64, roll: f64, pitch: f64, yaw: f64) -> Self {
        Self {
            position: Some(Position3D::new(x, y, z)),
            orientation: None,
            euler_deg: Some(EulerAngles::new(roll, pitch, yaw)),
        }
    }
}

/// 3D position in Cartesian coordinates.
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct Position3D {
    /// X coordinate in meters
    #[serde(skip_serializing_if = "Option::is_none")]
    pub x_m: Option<f64>,

    /// Y coordinate in meters
    #[serde(skip_serializing_if = "Option::is_none")]
    pub y_m: Option<f64>,

    /// Z coordinate in meters
    #[serde(skip_serializing_if = "Option::is_none")]
    pub z_m: Option<f64>,
}

impl Position3D {
    /// Create a new 3D position.
    pub fn new(x: f64, y: f64, z: f64) -> Self {
        Self {
            x_m: Some(x),
            y_m: Some(y),
            z_m: Some(z),
        }
    }

    /// Calculate distance to another position.
    pub fn distance_to(&self, other: &Position3D) -> Option<f64> {
        match (
            self.x_m, self.y_m, self.z_m, other.x_m, other.y_m, other.z_m,
        ) {
            (Some(x1), Some(y1), Some(z1), Some(x2), Some(y2), Some(z2)) => {
                let dx = x2 - x1;
                let dy = y2 - y1;
                let dz = z2 - z1;
                Some((dx * dx + dy * dy + dz * dz).sqrt())
            }
            _ => None,
        }
    }
}

/// 2D position (for floor-plane navigation).
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct Position2D {
    /// X coordinate in meters
    #[serde(skip_serializing_if = "Option::is_none")]
    pub x_m: Option<f64>,

    /// Y coordinate in meters
    #[serde(skip_serializing_if = "Option::is_none")]
    pub y_m: Option<f64>,
}

impl Position2D {
    /// Create a new 2D position.
    pub fn new(x: f64, y: f64) -> Self {
        Self {
            x_m: Some(x),
            y_m: Some(y),
        }
    }
}

/// Quaternion for orientation representation.
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct Quaternion {
    /// W component (scalar)
    pub w: f64,
    /// X component
    pub x: f64,
    /// Y component
    pub y: f64,
    /// Z component
    pub z: f64,
}

impl Quaternion {
    /// Create a new quaternion.
    pub fn new(w: f64, x: f64, y: f64, z: f64) -> Self {
        Self { w, x, y, z }
    }

    /// Identity quaternion (no rotation).
    pub fn identity() -> Self {
        Self {
            w: 1.0,
            x: 0.0,
            y: 0.0,
            z: 0.0,
        }
    }

    /// Normalize the quaternion.
    pub fn normalize(&self) -> Self {
        let mag = (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z).sqrt();
        if mag > 0.0 {
            Self {
                w: self.w / mag,
                x: self.x / mag,
                y: self.y / mag,
                z: self.z / mag,
            }
        } else {
            Self::identity()
        }
    }
}

/// Euler angles for orientation (roll, pitch, yaw).
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct EulerAngles {
    /// Roll angle in degrees (rotation about x-axis)
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = -180.0, max = 180.0))]
    pub roll_deg: Option<f64>,

    /// Pitch angle in degrees (rotation about y-axis)
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = -90.0, max = 90.0))]
    pub pitch_deg: Option<f64>,

    /// Yaw angle in degrees (rotation about z-axis)
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = -180.0, max = 180.0))]
    pub yaw_deg: Option<f64>,
}

impl EulerAngles {
    /// Create new Euler angles.
    pub fn new(roll: f64, pitch: f64, yaw: f64) -> Self {
        Self {
            roll_deg: Some(roll),
            pitch_deg: Some(pitch),
            yaw_deg: Some(yaw),
        }
    }
}

/// 3D vector for velocities, accelerations, etc.
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct Vector3D {
    /// X component
    #[serde(skip_serializing_if = "Option::is_none")]
    pub x: Option<f64>,

    /// Y component
    #[serde(skip_serializing_if = "Option::is_none")]
    pub y: Option<f64>,

    /// Z component
    #[serde(skip_serializing_if = "Option::is_none")]
    pub z: Option<f64>,
}

impl Vector3D {
    /// Create a new 3D vector.
    pub fn new(x: f64, y: f64, z: f64) -> Self {
        Self {
            x: Some(x),
            y: Some(y),
            z: Some(z),
        }
    }

    /// Calculate magnitude of the vector.
    pub fn magnitude(&self) -> Option<f64> {
        match (self.x, self.y, self.z) {
            (Some(x), Some(y), Some(z)) => Some((x * x + y * y + z * z).sqrt()),
            _ => None,
        }
    }
}

/// Covariance matrix (6x6 for pose, 3x3 for position).
#[derive(Debug, Clone, Default, Serialize, Deserialize)]
pub struct CovarianceMatrix {
    /// Row-major covariance values
    pub values: Vec<f64>,

    /// Matrix dimension (3 for 3x3, 6 for 6x6)
    pub dimension: u8,
}

impl CovarianceMatrix {
    /// Create a 3x3 covariance matrix for position uncertainty.
    pub fn position_3x3(values: [f64; 9]) -> Self {
        Self {
            values: values.to_vec(),
            dimension: 3,
        }
    }

    /// Create a 6x6 covariance matrix for pose uncertainty.
    pub fn pose_6x6(values: [f64; 36]) -> Self {
        Self {
            values: values.to_vec(),
            dimension: 6,
        }
    }
}

/// Bounding box (axis-aligned or oriented).
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct BoundingBox {
    /// Center position
    #[serde(skip_serializing_if = "Option::is_none")]
    pub center: Option<Position3D>,

    /// Dimensions
    #[serde(skip_serializing_if = "Option::is_none")]
    pub dimensions: Option<Dimensions>,

    /// Orientation (for oriented bounding boxes)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub orientation: Option<Quaternion>,

    /// Whether this is axis-aligned
    #[serde(skip_serializing_if = "Option::is_none")]
    pub axis_aligned: Option<bool>,
}

/// 2D bounding box for image detections.
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct BoundingBox2D {
    /// Top-left X coordinate (pixels)
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = 0.0))]
    pub x: Option<f64>,

    /// Top-left Y coordinate (pixels)
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = 0.0))]
    pub y: Option<f64>,

    /// Width in pixels
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = 0.0))]
    pub width: Option<f64>,

    /// Height in pixels
    #[serde(skip_serializing_if = "Option::is_none")]
    #[validate(range(min = 0.0))]
    pub height: Option<f64>,
}

/// Polygon for 2D regions (zones, areas).
#[derive(Debug, Clone, Default, Serialize, Deserialize)]
pub struct Polygon2D {
    /// List of vertices
    pub vertices: Vec<Position2D>,

    /// Whether the polygon is closed
    #[serde(default = "default_true")]
    pub closed: bool,
}

fn default_true() -> bool {
    true
}

impl Polygon2D {
    /// Create a new polygon from vertices.
    pub fn new(vertices: Vec<Position2D>) -> Self {
        Self {
            vertices,
            closed: true,
        }
    }

    /// Check if a point is inside the polygon (2D ray casting).
    pub fn contains(&self, point: &Position2D) -> bool {
        let (px, py) = match (point.x_m, point.y_m) {
            (Some(x), Some(y)) => (x, y),
            _ => return false,
        };

        let n = self.vertices.len();
        if n < 3 {
            return false;
        }

        let mut inside = false;
        let mut j = n - 1;

        for i in 0..n {
            #[expect(
                clippy::indexing_slicing,
                reason = "i and j are bounded by n = self.vertices.len()"
            )]
            let (xi, yi) = match (self.vertices[i].x_m, self.vertices[i].y_m) {
                (Some(x), Some(y)) => (x, y),
                _ => continue,
            };
            #[expect(
                clippy::indexing_slicing,
                reason = "j is bounded by n = self.vertices.len()"
            )]
            let (xj, yj) = match (self.vertices[j].x_m, self.vertices[j].y_m) {
                (Some(x), Some(y)) => (x, y),
                _ => {
                    j = i;
                    continue;
                }
            };

            if ((yi > py) != (yj > py)) && (px < (xj - xi) * (py - yi) / (yj - yi) + xi) {
                inside = !inside;
            }
            j = i;
        }

        inside
    }
}

/// Time range.
#[derive(Debug, Clone, Default, Serialize, Deserialize)]
pub struct TimeRange {
    /// Start time (ISO 8601)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub start: Option<chrono::DateTime<chrono::Utc>>,

    /// End time (ISO 8601)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub end: Option<chrono::DateTime<chrono::Utc>>,
}

/// Key-value metadata pair.
#[derive(Debug, Clone, Default, Serialize, Deserialize)]
pub struct Metadata {
    /// Key
    pub key: String,

    /// Value
    pub value: String,
}

/// Confidence score with optional source.
#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
pub struct Confidence {
    /// Confidence value (0.0 to 1.0)
    #[validate(range(min = 0.0, max = 1.0))]
    pub score: f64,

    /// Source of the confidence (e.g., "model_v2", "sensor_fusion")
    #[serde(skip_serializing_if = "Option::is_none")]
    pub source: Option<String>,
}

impl Confidence {
    /// Create a new confidence score.
    pub fn new(score: f64) -> Self {
        Self {
            score,
            source: None,
        }
    }

    /// Create a confidence score with source.
    pub fn with_source(score: f64, source: impl Into<String>) -> Self {
        Self {
            score,
            source: Some(source.into()),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_dimensions_volume() {
        let dims = Dimensions::new(2.0, 3.0, 4.0);
        assert_eq!(dims.volume_m3(), Some(24.0));
    }

    #[test]
    fn test_position_distance() {
        let p1 = Position3D::new(0.0, 0.0, 0.0);
        let p2 = Position3D::new(3.0, 4.0, 0.0);
        assert!((p1.distance_to(&p2).unwrap() - 5.0).abs() < 1e-10);
    }

    #[test]
    fn test_quaternion_normalize() {
        let q = Quaternion::new(2.0, 0.0, 0.0, 0.0);
        let normalized = q.normalize();
        assert!((normalized.w - 1.0).abs() < 1e-10);
    }

    #[test]
    fn test_polygon_contains() {
        let polygon = Polygon2D::new(vec![
            Position2D::new(0.0, 0.0),
            Position2D::new(4.0, 0.0),
            Position2D::new(4.0, 4.0),
            Position2D::new(0.0, 4.0),
        ]);

        assert!(polygon.contains(&Position2D::new(2.0, 2.0)));
        assert!(!polygon.contains(&Position2D::new(5.0, 5.0)));
    }

    // ==========================================================================
    // ObjectRef Tests
    // ==========================================================================

    #[test]
    fn test_object_ref_default() {
        let obj = ObjectRef::default();
        assert!(obj.object_id.is_none());
        assert!(obj.item_id.is_none());
        assert!(obj.object_type.is_none());
        assert!(obj.object_class.is_none());
        assert!(obj.tracking_id.is_none());
        assert!(obj.dimensions_m.is_none());
        assert!(obj.mass_kg.is_none());
        assert!(obj.pose.is_none());
        assert!(obj.frame_id.is_none());
    }

    #[test]
    fn test_object_ref_full() {
        let obj = ObjectRef {
            object_id: Some("obj-001".to_string()),
            item_id: Some("SKU12345".to_string()),
            object_type: Some("package".to_string()),
            object_class: Some("cardboard_box".to_string()),
            tracking_id: Some("track-001".to_string()),
            dimensions_m: Some(Dimensions::new(0.5, 0.3, 0.2)),
            mass_kg: Some(2.5),
            pose: Some(Pose::from_position(1.0, 2.0, 0.5)),
            frame_id: Some("base_link".to_string()),
        };

        assert_eq!(obj.object_type, Some("package".to_string()));
        assert_eq!(obj.mass_kg, Some(2.5));
    }

    // ==========================================================================
    // Dimensions Tests
    // ==========================================================================

    #[test]
    fn test_dimensions_new() {
        let dims = Dimensions::new(1.0, 2.0, 3.0);
        assert_eq!(dims.length_m, Some(1.0));
        assert_eq!(dims.width_m, Some(2.0));
        assert_eq!(dims.height_m, Some(3.0));
    }

    #[test]
    fn test_dimensions_volume_partial() {
        let dims = Dimensions {
            length_m: Some(2.0),
            width_m: None,
            height_m: Some(3.0),
        };
        assert!(dims.volume_m3().is_none());
    }

    #[test]
    fn test_dimensions_default() {
        let dims = Dimensions::default();
        assert!(dims.length_m.is_none());
        assert!(dims.width_m.is_none());
        assert!(dims.height_m.is_none());
        assert!(dims.volume_m3().is_none());
    }

    // ==========================================================================
    // Pose Tests
    // ==========================================================================

    #[test]
    fn test_pose_from_position() {
        let pose = Pose::from_position(1.0, 2.0, 3.0);
        assert!(pose.position.is_some());
        let pos = pose.position.unwrap();
        assert_eq!(pos.x_m, Some(1.0));
        assert_eq!(pos.y_m, Some(2.0));
        assert_eq!(pos.z_m, Some(3.0));
    }

    #[test]
    fn test_pose_from_position_euler() {
        let pose = Pose::from_position_euler(1.0, 2.0, 3.0, 10.0, 20.0, 30.0);
        assert!(pose.position.is_some());
        assert!(pose.euler_deg.is_some());
        let euler = pose.euler_deg.unwrap();
        assert_eq!(euler.roll_deg, Some(10.0));
        assert_eq!(euler.pitch_deg, Some(20.0));
        assert_eq!(euler.yaw_deg, Some(30.0));
    }

    #[test]
    fn test_pose_default() {
        let pose = Pose::default();
        assert!(pose.position.is_none());
        assert!(pose.orientation.is_none());
        assert!(pose.euler_deg.is_none());
    }

    // ==========================================================================
    // Position3D Tests
    // ==========================================================================

    #[test]
    fn test_position_3d_new() {
        let pos = Position3D::new(1.5, 2.5, 3.5);
        assert_eq!(pos.x_m, Some(1.5));
        assert_eq!(pos.y_m, Some(2.5));
        assert_eq!(pos.z_m, Some(3.5));
    }

    #[test]
    fn test_position_3d_distance_to_partial() {
        let p1 = Position3D::new(0.0, 0.0, 0.0);
        let p2 = Position3D::default();
        assert!(p1.distance_to(&p2).is_none());
    }

    #[test]
    fn test_position_3d_default() {
        let pos = Position3D::default();
        assert!(pos.x_m.is_none());
        assert!(pos.y_m.is_none());
        assert!(pos.z_m.is_none());
    }

    // ==========================================================================
    // Position2D Tests
    // ==========================================================================

    #[test]
    fn test_position_2d_new() {
        let pos = Position2D::new(10.0, 20.0);
        assert_eq!(pos.x_m, Some(10.0));
        assert_eq!(pos.y_m, Some(20.0));
    }

    #[test]
    fn test_position_2d_default() {
        let pos = Position2D::default();
        assert!(pos.x_m.is_none());
        assert!(pos.y_m.is_none());
    }

    // ==========================================================================
    // Quaternion Tests
    // ==========================================================================

    #[test]
    fn test_quaternion_new() {
        let q = Quaternion::new(1.0, 0.0, 0.0, 0.0);
        assert!((q.w - 1.0).abs() < f64::EPSILON);
        assert!((q.x - 0.0).abs() < f64::EPSILON);
        assert!((q.y - 0.0).abs() < f64::EPSILON);
        assert!((q.z - 0.0).abs() < f64::EPSILON);
    }

    #[test]
    fn test_quaternion_identity() {
        let q = Quaternion::identity();
        assert!((q.w - 1.0).abs() < f64::EPSILON);
        assert!((q.x - 0.0).abs() < f64::EPSILON);
        assert!((q.y - 0.0).abs() < f64::EPSILON);
        assert!((q.z - 0.0).abs() < f64::EPSILON);
    }

    #[test]
    fn test_quaternion_normalize_non_unit() {
        let q = Quaternion::new(0.0, 3.0, 4.0, 0.0);
        let normalized = q.normalize();
        // Verify normalized quaternion has unit length (w^2 + x^2 + y^2 + z^2 = 1)
        let mag_sq = normalized.w * normalized.w
            + normalized.x * normalized.x
            + normalized.y * normalized.y
            + normalized.z * normalized.z;
        assert!((mag_sq - 1.0).abs() < 1e-10);
    }

    #[test]
    fn test_quaternion_default() {
        let q = Quaternion::default();
        assert!((q.w - 0.0).abs() < f64::EPSILON);
        assert!((q.x - 0.0).abs() < f64::EPSILON);
        assert!((q.y - 0.0).abs() < f64::EPSILON);
        assert!((q.z - 0.0).abs() < f64::EPSILON);
    }

    // ==========================================================================
    // EulerAngles Tests
    // ==========================================================================

    #[test]
    fn test_euler_angles_new() {
        let euler = EulerAngles::new(10.0, 20.0, 30.0);
        assert_eq!(euler.roll_deg, Some(10.0));
        assert_eq!(euler.pitch_deg, Some(20.0));
        assert_eq!(euler.yaw_deg, Some(30.0));
    }

    #[test]
    fn test_euler_angles_default() {
        let euler = EulerAngles::default();
        assert!(euler.roll_deg.is_none());
        assert!(euler.pitch_deg.is_none());
        assert!(euler.yaw_deg.is_none());
    }

    // ==========================================================================
    // Vector3D Tests
    // ==========================================================================

    #[test]
    fn test_vector_3d_new() {
        let v = Vector3D::new(1.0, 2.0, 3.0);
        assert_eq!(v.x, Some(1.0));
        assert_eq!(v.y, Some(2.0));
        assert_eq!(v.z, Some(3.0));
    }

    #[test]
    fn test_vector_3d_magnitude() {
        let v = Vector3D::new(3.0, 4.0, 0.0);
        assert!((v.magnitude().unwrap() - 5.0).abs() < 1e-10);
    }

    #[test]
    fn test_vector_3d_magnitude_none() {
        let v = Vector3D::default();
        assert!(v.magnitude().is_none());
    }

    #[test]
    fn test_vector_3d_default() {
        let v = Vector3D::default();
        assert!(v.x.is_none());
        assert!(v.y.is_none());
        assert!(v.z.is_none());
    }

    // ==========================================================================
    // BoundingBox Tests
    // ==========================================================================

    #[test]
    fn test_bounding_box_default() {
        let bb = BoundingBox::default();
        assert!(bb.center.is_none());
        assert!(bb.dimensions.is_none());
        assert!(bb.orientation.is_none());
        assert!(bb.axis_aligned.is_none());
    }

    #[test]
    fn test_bounding_box_full() {
        let bb = BoundingBox {
            center: Some(Position3D::new(1.0, 2.0, 0.5)),
            dimensions: Some(Dimensions::new(1.0, 0.5, 0.8)),
            orientation: Some(Quaternion::identity()),
            axis_aligned: Some(true),
        };

        assert!(bb.center.is_some());
        assert!(bb.dimensions.is_some());
    }

    // ==========================================================================
    // Polygon2D Tests
    // ==========================================================================

    #[test]
    fn test_polygon_2d_new() {
        let vertices = vec![
            Position2D::new(0.0, 0.0),
            Position2D::new(1.0, 0.0),
            Position2D::new(1.0, 1.0),
        ];
        let poly = Polygon2D::new(vertices);
        assert_eq!(poly.vertices.len(), 3);
    }

    #[test]
    fn test_polygon_2d_empty() {
        let poly = Polygon2D::new(vec![]);
        assert!(!poly.contains(&Position2D::new(0.0, 0.0)));
    }

    #[test]
    fn test_polygon_2d_contains_partial_vertices() {
        let poly = Polygon2D::new(vec![
            Position2D::default(),
            Position2D::new(1.0, 0.0),
            Position2D::new(1.0, 1.0),
        ]);
        // Should not crash with partial data
        let _ = poly.contains(&Position2D::new(0.5, 0.5));
    }

    #[test]
    fn test_polygon_2d_contains_point_no_coords() {
        let poly = Polygon2D::new(vec![
            Position2D::new(0.0, 0.0),
            Position2D::new(1.0, 0.0),
            Position2D::new(1.0, 1.0),
        ]);
        assert!(!poly.contains(&Position2D::default()));
    }

    // ==========================================================================
    // Confidence Tests
    // ==========================================================================

    #[test]
    fn test_confidence_new() {
        let conf = Confidence::new(0.95);
        assert!((conf.score - 0.95).abs() < f64::EPSILON);
        assert!(conf.source.is_none());
    }

    #[test]
    fn test_confidence_with_source() {
        let conf = Confidence::with_source(0.87, "model_v2");
        assert!((conf.score - 0.87).abs() < f64::EPSILON);
        assert_eq!(conf.source, Some("model_v2".to_string()));
    }

    #[test]
    fn test_confidence_default() {
        let conf = Confidence::default();
        assert!((conf.score - 0.0).abs() < f64::EPSILON);
        assert!(conf.source.is_none());
    }

    // ==========================================================================
    // TimeRange Tests
    // ==========================================================================

    #[test]
    fn test_time_range_default() {
        let tr = TimeRange::default();
        assert!(tr.start.is_none());
        assert!(tr.end.is_none());
    }

    // ==========================================================================
    // Metadata Tests
    // ==========================================================================

    #[test]
    fn test_metadata_default() {
        let md = Metadata::default();
        assert!(md.key.is_empty());
        assert!(md.value.is_empty());
    }

    #[test]
    fn test_metadata_full() {
        let md = Metadata {
            key: "version".to_string(),
            value: "1.0.0".to_string(),
        };
        assert_eq!(md.key, "version");
        assert_eq!(md.value, "1.0.0");
    }

    // ==========================================================================
    // Serialization Roundtrip Tests
    // ==========================================================================

    #[test]
    fn test_object_ref_serialization() {
        let obj = ObjectRef {
            object_id: Some("obj-001".to_string()),
            mass_kg: Some(5.0),
            ..Default::default()
        };

        let json = serde_json::to_string(&obj).unwrap();
        let deserialized: ObjectRef = serde_json::from_str(&json).unwrap();
        assert_eq!(deserialized.object_id, Some("obj-001".to_string()));
    }

    #[test]
    fn test_pose_serialization() {
        let pose = Pose::from_position_euler(1.0, 2.0, 3.0, 0.0, 0.0, 90.0);
        let json = serde_json::to_string(&pose).unwrap();
        let deserialized: Pose = serde_json::from_str(&json).unwrap();
        assert!(deserialized.position.is_some());
        assert!(deserialized.euler_deg.is_some());
    }
}