Standard Messages

use horus::prelude::*;

pub struct Twist {
    pub linear: [f64; 3],       // [x, y, z] in m/s
    pub angular: [f64; 3],      // [roll, pitch, yaw] in rad/s
    pub timestamp_ns: u64,      // Timestamp in nanoseconds since epoch
}

// Full 3D velocity
let twist = Twist::new([1.0, 0.0, 0.0], [0.0, 0.0, 0.5]);

// 2D velocity (forward + rotation)
let twist = Twist::new_2d(1.0, 0.5);  // 1 m/s forward, 0.5 rad/s rotation

// Stop command
let twist = Twist::stop();

pub struct Pose2D {
    pub x: f64,            // X position in meters
    pub y: f64,            // Y position in meters
    pub theta: f64,        // Orientation in radians
    pub timestamp_ns: u64, // Timestamp in nanoseconds since epoch
}

let pose = Pose2D::new(1.0, 2.0, 0.5);  // x=1m, y=2m, theta=0.5rad
let pose = Pose2D::origin();             // (0, 0, 0)

pub struct TransformStamped {
    pub translation: [f64; 3],  // [x, y, z] in meters
    pub rotation: [f64; 4],     // Quaternion [x, y, z, w]
    pub timestamp_ns: u64,
}

let tf = TransformStamped::identity();
let tf = TransformStamped::new([1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]);
let tf = TransformStamped::from_pose_2d(&pose);

pub struct Point3 {
    pub x: f64,
    pub y: f64,
    pub z: f64,
}

let p1 = Point3::new(1.0, 2.0, 3.0);
let p2 = Point3::origin();
let dist = p1.distance_to(&p2);

pub struct Vector3 {
    pub x: f64,
    pub y: f64,
    pub z: f64,
}

pub struct Quaternion {
    pub x: f64,
    pub y: f64,
    pub z: f64,
    pub w: f64,
}

let q = Quaternion::identity();
let q = Quaternion::new(0.0, 0.0, 0.0, 1.0);
let q = Quaternion::from_euler(roll, pitch, yaw);

pub struct LaserScan {
    pub ranges: [f32; 360],      // Range measurements in meters
    pub angle_min: f32,          // Start angle in radians
    pub angle_max: f32,          // End angle in radians
    pub range_min: f32,          // Minimum valid range
    pub range_max: f32,          // Maximum valid range
    pub angle_increment: f32,    // Angular resolution
    pub time_increment: f32,     // Time between measurements
    pub scan_time: f32,          // Full scan time
    pub timestamp_ns: u64,
}

let scan = LaserScan::new();
let scan = LaserScan::default();

if let Some(scan) = scan_sub.recv() {
    if let Some(min_dist) = scan.min_range() {
        if min_dist < 0.5 {
            // Obstacle detected!
        }
    }
}

pub struct Imu {
    pub orientation: [f64; 4],              // Quaternion [x, y, z, w]
    pub orientation_covariance: [f64; 9],   // 3x3 covariance (-1 = no data)
    pub angular_velocity: [f64; 3],         // [x, y, z] in rad/s
    pub angular_velocity_covariance: [f64; 9],
    pub linear_acceleration: [f64; 3],      // [x, y, z] in m/s²
    pub linear_acceleration_covariance: [f64; 9],
    pub timestamp_ns: u64,
}

let imu = Imu::new();

pub struct Odometry {
    pub pose: Pose2D,
    pub twist: Twist,
    pub pose_covariance: [f64; 36],    // 6x6 covariance
    pub twist_covariance: [f64; 36],
    pub frame_id: [u8; 32],            // e.g., "odom"
    pub child_frame_id: [u8; 32],      // e.g., "base_link"
    pub timestamp_ns: u64,
}

pub struct NavSatFix {
    pub latitude: f64,           // Degrees (+ North, - South)
    pub longitude: f64,          // Degrees (+ East, - West)
    pub altitude: f64,           // Meters above WGS84
    pub position_covariance: [f64; 9],
    pub position_covariance_type: u8,
    pub status: u8,              // Fix status
    pub satellites_visible: u16,
    pub hdop: f32,
    pub vdop: f32,
    pub speed: f32,              // m/s
    pub heading: f32,            // degrees
    pub timestamp_ns: u64,
}

NavSatFix::STATUS_NO_FIX    // 0
NavSatFix::STATUS_FIX       // 1
NavSatFix::STATUS_SBAS_FIX  // 2
NavSatFix::STATUS_GBAS_FIX  // 3

pub struct BatteryState {
    pub voltage: f32,            // Volts
    pub current: f32,            // Amperes (negative = discharging)
    pub charge: f32,             // Amp-hours
    pub capacity: f32,           // Amp-hours
    pub percentage: f32,         // 0-100
    pub power_supply_status: u8,
    pub temperature: f32,        // Celsius
    pub cell_voltages: [f32; 16],
    pub cell_count: u8,
    pub timestamp_ns: u64,
}

BatteryState::STATUS_UNKNOWN     // 0
BatteryState::STATUS_CHARGING    // 1
BatteryState::STATUS_DISCHARGING // 2
BatteryState::STATUS_FULL        // 3

pub struct RangeSensor {
    pub sensor_type: u8,     // 0=ultrasonic, 1=infrared
    pub field_of_view: f32,  // radians
    pub min_range: f32,      // meters
    pub max_range: f32,      // meters
    pub range: f32,          // meters
    pub timestamp_ns: u64,
}

// Image is an RAII type, not a plain struct.
// Create with Image::new(width, height, encoding)?
// Access data with .data(), .pixel(), etc.
// See Vision Messages for full API.
let mut img = Image::new(640, 480, ImageEncoding::Rgb8)?;
img.copy_from(&pixel_data);

pub enum ImageEncoding {
    Rgb8,
    Bgr8,
    Rgba8,
    Bgra8,
    Mono8,
    Mono16,
    Yuv422,
    Mono32F,
    Rgb32F,
    BayerRggb8,
    Depth16,
}

pub struct CameraInfo {
    pub width: u32,
    pub height: u32,
    pub distortion_model: [u8; 16],
    pub distortion_coefficients: [f64; 8],  // [k1, k2, p1, p2, k3, k4, k5, k6]
    pub camera_matrix: [f64; 9],            // Intrinsic matrix (3x3)
    pub rectification_matrix: [f64; 9],     // Rectification (3x3)
    pub projection_matrix: [f64; 12],       // Projection (3x4)
    pub frame_id: [u8; 32],                 // Camera identifier
    pub timestamp_ns: u64,
}

pub struct Detection {
    pub bbox: BoundingBox2D,
    pub confidence: f32,
    pub class_id: u32,
    pub class_name: [u8; 32],
    pub instance_id: u32,
}

pub struct MotorCommand {
    pub motor_id: u8,
    pub mode: u8,             // 0=velocity, 1=position, 2=torque, 3=voltage
    pub target: f64,
    pub max_velocity: f64,
    pub max_acceleration: f64,
    pub feed_forward: f64,
    pub enable: u8,
    pub timestamp_ns: u64,
}

pub struct ServoCommand {
    pub servo_id: u8,
    pub position: f32,       // radians
    pub speed: f32,          // 0-1 (0 = max speed)
    pub enable: u8,
    pub timestamp_ns: u64,
}

pub struct PidConfig {
    pub controller_id: u8,
    pub kp: f64,
    pub ki: f64,
    pub kd: f64,
    pub integral_limit: f64,
    pub output_limit: f64,
    pub anti_windup: u8,
    pub timestamp_ns: u64,
}

pub struct GenericMessage {
    inline_data: [u8; 256],    // First 256 bytes (inline)
    overflow_data: [u8; 3840], // Overflow up to 3840 more bytes
    metadata: [u8; 256],       // Optional metadata
    // ... internal length tracking fields
}

// From raw bytes (returns Err if > 4KB)
let msg = GenericMessage::new(data_vec)?;

// From any serializable type (uses MessagePack)
let msg = GenericMessage::from_value(&my_struct)?;

// With metadata string (max 255 chars)
let msg = GenericMessage::with_metadata(data, "my_type".to_string())?;

use std::collections::HashMap;

// Send any serializable data
let mut data = HashMap::new();
data.insert("x", 1.0);
data.insert("y", 2.0);
let msg = GenericMessage::from_value(&data)?;
topic.send(msg);

// Receive and deserialize
if let Some(msg) = topic.recv() {
    let data: HashMap<String, f64> = msg.to_value()?;
    println!("x: {}", data["x"]);
}