Perception Messages

use horus::prelude::*;

// Create XYZ point cloud (10000 points, 3 floats per point)
let mut cloud = PointCloud::new(10000, 3, TensorDtype::F32)?;

// Copy point data
let point_data: Vec<u8> = vec![0; 10000 * 3 * 4]; // 10000 points * 3 floats * 4 bytes
cloud.copy_from(&point_data);

// Set metadata (method chaining)
cloud.set_frame_id("lidar_front").set_timestamp_ns(1234567890);

// Access properties
println!("Points: {}", cloud.point_count());
println!("Fields per point: {}", cloud.fields_per_point());
println!("Is XYZ: {}", cloud.is_xyz());

// Zero-copy data access
let data: &[u8] = cloud.data();

// Extract XYZ coordinates as Vec<[f32; 3]>
if let Some(points) = cloud.extract_xyz() {
    for p in &points[..3] {
        println!("({:.2}, {:.2}, {:.2})", p[0], p[1], p[2]);
    }
}

// Access individual point
if let Some(point_bytes) = cloud.point_at(0) {
    println!("Point 0: {} bytes", point_bytes.len());
}

use horus::prelude::*;

// Create custom field
let intensity = PointField::new("intensity", 12, TensorDtype::F32, 1);
println!("Field: {}, size: {} bytes", intensity.name_str(), intensity.field_size());

use horus::prelude::*;

let point = PointXYZ::new(1.0, 2.0, 3.0);

println!("Distance from origin: {:.2}m", point.distance());

let other = PointXYZ::new(4.0, 6.0, 3.0);
println!("Distance between: {:.2}m", point.distance_to(&other));

use horus::prelude::*;

let point = PointXYZRGB::new(1.0, 2.0, 3.0, 255, 0, 0); // Red point

// Convert from PointXYZ (defaults to white)
let xyz = PointXYZ::new(1.0, 2.0, 3.0);
let colored = PointXYZRGB::from_xyz(xyz);

// Get packed RGB as u32 (0xRRGGBBAA)
let packed = point.rgb_packed();

// Convert back to PointXYZ (drop color)
let xyz_only = point.xyz();

use horus::prelude::*;

let point = PointXYZI::new(1.0, 2.0, 3.0, 128.0);

// Convert from PointXYZ (zero intensity)
let xyz = PointXYZ::new(1.0, 2.0, 3.0);
let with_intensity = PointXYZI::from_xyz(xyz);

use horus::prelude::*;

// Create header for different point types
let header = PointCloudHeader::xyz(10000)
    .with_frame_id("lidar_front")
    .with_timestamp(timestamp_ns);

// Or for colored points
let header = PointCloudHeader::xyzrgb(5000);

// Calculate total data size
println!("Data size: {} bytes", header.data_size());

use horus::prelude::*;

// Create F32 depth image (meters)
let mut depth = DepthImage::new(640, 480, TensorDtype::F32)?;

// Or U16 depth image (millimeters)
let mut depth_u16 = DepthImage::new(640, 480, TensorDtype::U16)?;

// Set metadata (method chaining)
depth.set_frame_id("depth_camera").set_timestamp_ns(1234567890);

// Access depth at pixel (always returns meters as f32)
if let Some(d) = depth.get_depth(320, 240) {
    println!("Depth at center: {:.3}m", d);
}

// Set depth at pixel (value in meters)
depth.set_depth(100, 100, 1.5);

// Get raw U16 value for millimeter-format images
if let Some(mm) = depth_u16.get_depth_u16(320, 240) {
    println!("Raw depth: {}mm", mm);
}

// Get statistics (min, max, mean in meters)
if let Some((min, max, mean)) = depth.depth_statistics() {
    println!("Depth range: {:.2}-{:.2}m, mean: {:.2}m", min, max, mean);
}

// Zero-copy data access
let data: &[u8] = depth.data();
println!("Image: {}x{}, {} bytes", depth.width(), depth.height(), data.len());

use horus::prelude::*;

// Create plane detection (floor plane)
let coefficients = [0.0, 0.0, 1.0, 0.0];  // ax + by + cz + d = 0
let center = Point3::new(0.0, 0.0, 0.0);
let normal = Vector3::new(0.0, 0.0, 1.0);

let plane = PlaneDetection::new(coefficients, center, normal)
    .with_type("floor");

// Check distance from point to plane
let test_point = Point3::new(1.0, 2.0, 0.1);
let distance = plane.distance_to_point(&test_point);

// Check if point is on plane (within tolerance)
if plane.contains_point(&test_point, 0.05) {
    println!("Point is on the plane");
}

println!("Plane type: {}", plane.plane_type_str());

use horus::prelude::*;

// Create landmark with visibility
let nose = Landmark::new(320.0, 240.0, 0.95, 0);  // (x, y, visibility, index)

// Create visible landmark (visibility = 1.0)
let eye = Landmark::visible(300.0, 220.0, 1);  // left_eye

// Check visibility
if nose.is_visible(0.5) {
    println!("Nose detected at ({}, {})", nose.x, nose.y);
}

// Distance between landmarks
let distance = nose.distance_to(&eye);

use horus::prelude::*;

let landmark = Landmark3D::new(0.5, 0.3, 0.8, 0.95, 0);

// Project to 2D (drops Z)
let landmark_2d = landmark.to_2d();

// 3D distance
let other = Landmark3D::visible(0.6, 0.4, 0.9, 1);
let dist = landmark.distance_to(&other);

use horus::prelude::*;

// Standard pose estimation formats
let coco = LandmarkArray::coco_pose();           // 17 2D landmarks
let mp_pose = LandmarkArray::mediapipe_pose();    // 33 3D landmarks
let mp_hand = LandmarkArray::mediapipe_hand();    // 21 3D landmarks
let mp_face = LandmarkArray::mediapipe_face();    // 468 3D landmarks

// Custom array with metadata
let header = LandmarkArray::new_2d(17)
    .with_confidence(0.92)
    .with_bbox(100.0, 50.0, 200.0, 300.0);

println!("Data size: {} bytes", header.data_size());

use horus::prelude::*;

// Create tracked object
let bbox = TrackingBBox::new(100.0, 100.0, 50.0, 50.0);
let mut track = TrackedObject::new(1, bbox, 0, 0.95);
track.set_class_name("person");

// Track lifecycle
assert!(track.is_tentative());  // New tracks start tentative

track.confirm();                 // Promote to confirmed
assert!(track.is_confirmed());

// Update with new detection
track.update(TrackingBBox::new(110.0, 105.0, 50.0, 50.0), 0.93);
println!("Velocity: ({}, {})", track.velocity_x, track.velocity_y);
println!("Speed: {:.1}", track.speed());
println!("Heading: {:.2} rad", track.heading());

// Handle missed detection
track.mark_missed();
println!("Predicted position: ({}, {})",
    track.predicted_bbox.x, track.predicted_bbox.y);

use horus::prelude::*;

// Semantic segmentation (class ID per pixel)
let mask = SegmentationMask::semantic(1920, 1080, 80)
    .with_frame_id("camera_front")
    .with_timestamp(timestamp_ns);

println!("Data size: {} bytes", mask.data_size());  // 1920*1080

// Instance segmentation
let mask = SegmentationMask::instance(640, 480);

// Panoptic segmentation
let mask = SegmentationMask::panoptic(640, 480, 80);

// Check type
if mask.is_panoptic() {
    println!("Panoptic mask, u16 data: {} bytes", mask.data_size_u16());
}

use horus::prelude::*;

struct PerceptionNode {
    depth_sub: Topic<DepthImage>,
    cloud_pub: Topic<PointCloud>,
    fx: f32,
    fy: f32,
    cx: f32,
    cy: f32,
}

impl Node for PerceptionNode {
    fn name(&self) -> &str { "PerceptionNode" }

    fn tick(&mut self) {
        if let Some(depth) = self.depth_sub.recv() {
            // Read depth values and build point cloud
            let w = depth.width();
            let h = depth.height();
            let num_points = (w * h) as u32;

            if let Ok(mut cloud) = PointCloud::new(num_points, 3, TensorDtype::F32) {
                // Depth-to-pointcloud conversion would fill cloud data here
                // using camera intrinsics (fx, fy, cx, cy)
                self.cloud_pub.send(cloud);
            }
        }
    }
}