Vision Messages

use horus::prelude::*;

// Create an RGB image (width, height, encoding) — allocates from global pool
let mut image = Image::new(640, 480, ImageEncoding::Rgb8)?;

// Copy pixel data into the image
let pixels: Vec<u8> = vec![128; 480 * 640 * 3];
image.copy_from(&pixels);

// Set metadata (method chaining)
image.set_frame_id("camera_front").set_timestamp_ns(1234567890);

// Access image properties
println!("Image: {}x{}, {:?}", image.width(), image.height(), image.encoding());
println!("Data size: {} bytes", image.data().len());

// Access individual pixel (x, y)
if let Some(pixel) = image.pixel(0, 0) {
    println!("Pixel[0,0]: R={}, G={}, B={}", pixel[0], pixel[1], pixel[2]);
}

// Set a pixel value
image.set_pixel(0, 0, &[255, 0, 0]);

// Extract region of interest (returns raw bytes, not Image)
if let Some(roi_data) = image.roi(0, 0, 100, 100) {
    println!("ROI data: {} bytes", roi_data.len());
}

// Fill entire image with a color
image.fill(&[0, 0, 0]); // Black

use horus::prelude::*;

// Create compressed image from JPEG data
let jpeg_data = std::fs::read("image.jpg").unwrap();
let compressed = CompressedImage::new("jpeg", jpeg_data);

println!("Format: {}", compressed.format_str());
println!("Compressed size: {} bytes", compressed.data.len());

// Optional: set original dimensions if known
let mut img = compressed;
img.width = 640;
img.height = 480;

use horus::prelude::*;

// Create camera info with intrinsics
let camera = CameraInfo::new(
    640, 480,       // width, height
    525.0, 525.0,   // fx, fy
    320.0, 240.0    // cx, cy (principal point)
).with_distortion_model("plumb_bob");

// Access intrinsics
let (fx, fy) = camera.focal_lengths();
let (cx, cy) = camera.principal_point();

println!("Focal length: ({:.1}, {:.1})", fx, fy);
println!("Principal point: ({:.1}, {:.1})", cx, cy);

// Set distortion coefficients
let mut camera = camera;
camera.distortion_coefficients = [
    -0.25,  // k1
    0.12,   // k2
    0.001,  // p1
    -0.001, // p2
    0.0, 0.0, 0.0, 0.0  // k3-k6
];

use horus::prelude::*;

// Create ROI
let roi = RegionOfInterest::new(100, 50, 200, 150);

// Check if point is inside ROI
if roi.contains(150, 100) {
    println!("Point is inside ROI");
}

// Get area
println!("ROI area: {} pixels", roi.area());

// Access properties
println!("ROI: ({}, {}) -> {}x{}",
    roi.x_offset, roi.y_offset, roi.width, roi.height);

use horus_library::messages::vision::StereoInfo;
use horus::prelude::*;

// Create stereo configuration
let left = CameraInfo::new(640, 480, 525.0, 525.0, 320.0, 240.0);
let right = CameraInfo::new(640, 480, 525.0, 525.0, 320.0, 240.0);

let stereo = StereoInfo {
    left_camera: left,
    right_camera: right,
    baseline: 0.12,  // 12cm between cameras
    depth_scale: 1.0,
};

// Calculate depth from disparity
let disparity = 64.0_f32;  // pixels
let depth = stereo.depth_from_disparity(disparity);
println!("Disparity {} -> depth {:.2}m", disparity, depth);

// Calculate disparity from depth
let depth = 2.0_f32;  // meters
let disparity = stereo.disparity_from_depth(depth);
println!("Depth {}m -> disparity {:.1}px", depth, disparity);

use horus::prelude::*;

// Create from top-left corner
let bbox = BoundingBox2D::new(100.0, 50.0, 200.0, 150.0);

// Create from center (YOLO format)
let bbox = BoundingBox2D::from_center(200.0, 125.0, 200.0, 150.0);

// Get properties
println!("Center: ({}, {})", bbox.center_x(), bbox.center_y());
println!("Area: {} px²", bbox.area());

// Calculate IoU between two boxes
let other = BoundingBox2D::new(150.0, 75.0, 200.0, 150.0);
println!("IoU: {:.3}", bbox.iou(&other));

use horus::prelude::*;

// Create detection with class name, confidence, and bounding box coordinates
let det = Detection::new("person", 0.95, 100.0, 50.0, 200.0, 300.0);

println!("Detected: {} ({:.1}% confidence)",
    det.class_name(),
    det.confidence * 100.0);

println!("BBox: ({}, {}) {}x{}",
    det.bbox.x, det.bbox.y, det.bbox.width, det.bbox.height);

// Check confidence threshold
if det.is_confident(0.9) {
    println!("High confidence detection!");
}

// Create with class ID instead of name
let bbox = BoundingBox2D::new(100.0, 50.0, 200.0, 300.0);
let det = Detection::with_class_id(1, 0.88, bbox);

use horus::prelude::*;

// Create 3D bounding box (center, dimensions, yaw)
let bbox = BoundingBox3D::new(
    5.0, 2.0, 0.5,    // center (x, y, z) in meters
    4.5, 2.0, 1.5,    // dimensions (length, width, height)
    0.1                // yaw rotation in radians
);

// Create 3D detection with velocity
let det = Detection3D::new("car", 0.92, bbox)
    .with_velocity(10.0, 5.0, 0.0);  // m/s

println!("Detected: {} at ({}, {}, {})",
    det.class_name(),
    det.bbox.cx, det.bbox.cy, det.bbox.cz);
println!("Volume: {:.1} m³", det.bbox.volume());

use horus::prelude::*;

struct VisionNode {
    image_sub: Topic<Image>,
    camera_info_sub: Topic<CameraInfo>,
    detection_pub: Topic<Detection>,
    camera_info: Option<CameraInfo>,
}

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

    fn tick(&mut self) {
        // Update camera calibration
        if let Some(info) = self.camera_info_sub.recv() {
            self.camera_info = Some(info);
        }

        // Process images
        if let Some(image) = self.image_sub.recv() {
            // Run detection (your ML model here)
            let detection = Detection::new(
                "person", 0.95,
                100.0, 50.0, 200.0, 300.0
            );

            self.detection_pub.send(detection);
        }
    }
}