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 Pose3D {
    pub position: Point3,       // Position in 3D space
    pub orientation: Quaternion, // Orientation as quaternion
    pub timestamp_ns: u64,
}

let pose = Pose3D::new(Point3::new(1.0, 2.0, 0.5), Quaternion::identity());
let pose = Pose3D::identity();
let pose = Pose3D::from_pose_2d(&pose_2d);  // Lift 2D pose into 3D

pub struct PoseStamped {
    pub pose: Pose3D,
    pub frame_id: [u8; 32],    // Coordinate frame (null-terminated, max 31 chars)
    pub timestamp_ns: u64,
}

let stamped = PoseStamped::new();
let stamped = PoseStamped::with_frame_id(pose, "map");

pub struct Accel {
    pub linear: [f64; 3],      // [x, y, z] in m/s²
    pub angular: [f64; 3],     // [roll, pitch, yaw] in rad/s²
    pub timestamp_ns: u64,
}

let accel = Accel::new([0.0, 0.0, 9.81], [0.0, 0.0, 0.0]);

pub struct AccelStamped {
    pub accel: Accel,
    pub frame_id: [u8; 32],    // Coordinate frame (null-terminated, max 31 chars)
    pub timestamp_ns: u64,
}

let stamped = AccelStamped::new();
let stamped = AccelStamped::with_frame_id(accel, "imu_link");

pub struct PoseWithCovariance {
    pub pose: Pose3D,
    pub covariance: [f64; 36],  // 6x6 covariance (row-major): x, y, z, roll, pitch, yaw
    pub frame_id: [u8; 32],
    pub timestamp_ns: u64,
}

let pose_cov = PoseWithCovariance::new();
let pose_cov = PoseWithCovariance::with_frame_id(pose, covariance, "map");

// EKF localization output with uncertainty
let mut pose_cov = PoseWithCovariance::new();
pose_cov.pose = estimated_pose;
// Diagonal: position uncertainty 0.1m, orientation 0.05rad
pose_cov.covariance[0] = 0.01;   // x variance
pose_cov.covariance[7] = 0.01;   // y variance
pose_cov.covariance[14] = 0.04;  // z variance
pose_cov.covariance[21] = 0.0025; // roll variance
pose_cov.covariance[28] = 0.0025; // pitch variance
pose_cov.covariance[35] = 0.0025; // yaw variance
topic.send(pose_cov);

pub struct TwistWithCovariance {
    pub twist: Twist,
    pub covariance: [f64; 36],  // 6x6 covariance (row-major): vx, vy, vz, wx, wy, wz
    pub frame_id: [u8; 32],
    pub timestamp_ns: u64,
}

let twist_cov = TwistWithCovariance::new();
let twist_cov = TwistWithCovariance::with_frame_id(twist, covariance, "base_link");

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,
}

pub struct JointState {
    pub names: [[u8; 32]; 16],  // Joint names (null-terminated, max 31 chars each)
    pub joint_count: u8,        // Number of valid joints
    pub positions: [f64; 16],   // Radians (revolute) or meters (prismatic)
    pub velocities: [f64; 16],  // rad/s or m/s
    pub efforts: [f64; 16],     // Torque (Nm) or force (N)
    pub timestamp_ns: u64,
}

let mut joints = JointState::new();
joints.add_joint("shoulder", 0.5, 0.0, 1.2);
joints.add_joint("elbow", -0.8, 0.1, 0.5);
joints.add_joint("wrist", 0.2, 0.0, 0.1);
joint_topic.send(joints);

pub struct Temperature {
    pub temperature: f64,    // Degrees Celsius
    pub variance: f64,       // Variance (0 if exact)
    pub frame_id: [u8; 32],
    pub timestamp_ns: u64,
}

let temp = Temperature::new();
let temp = Temperature::with_frame_id(22.5, 0.1, "motor_0");

pub struct FluidPressure {
    pub fluid_pressure: f64, // Pressure in Pascals
    pub variance: f64,       // Variance (0 if exact)
    pub frame_id: [u8; 32],
    pub timestamp_ns: u64,
}

let pressure = FluidPressure::new();
let pressure = FluidPressure::with_frame_id(101325.0, 10.0, "barometer");

pub struct Illuminance {
    pub illuminance: f64,    // Illuminance in Lux
    pub variance: f64,       // Variance (0 if exact)
    pub frame_id: [u8; 32],
    pub timestamp_ns: u64,
}

let lux = Illuminance::new();
let lux = Illuminance::with_frame_id(500.0, 5.0, "ambient_sensor");

pub struct MagneticField {
    pub magnetic_field: [f64; 3],             // [x, y, z] in Tesla
    pub magnetic_field_covariance: [f64; 9],  // 3x3 covariance (row-major). [0] = -1 means unknown
    pub frame_id: [u8; 32],
    pub timestamp_ns: u64,
}

let mag = MagneticField::new();
let mag = MagneticField::with_frame_id([25e-6, 0.0, -45e-6], "imu_link");

// 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 struct RegionOfInterest {
    pub x_offset: u32,
    pub y_offset: u32,
    pub width: u32,
    pub height: u32,
    pub do_rectify: bool,
}

pub struct CompressedImage {
    pub format: [u8; 8],        // "jpeg", "png", "webp"
    pub data: Vec<u8>,          // Compressed image bytes
    pub width: u32,
    pub height: u32,
    pub frame_id: [u8; 32],
    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 BoundingBox2D {
    pub x: f32,       // Top-left X (pixels)
    pub y: f32,       // Top-left Y (pixels)
    pub width: f32,   // Width (pixels)
    pub height: f32,  // Height (pixels)
}

pub struct BoundingBox3D {
    pub cx: f32,      // Center X (meters)
    pub cy: f32,      // Center Y (meters)
    pub cz: f32,      // Center Z (meters)
    pub length: f32,  // Along X axis (meters)
    pub width: f32,   // Along Y axis (meters)
    pub height: f32,  // Along Z axis (meters)
    pub roll: f32,    // Rotation around X (radians)
    pub pitch: f32,   // Rotation around Y (radians)
    pub yaw: f32,     // Rotation around Z (radians)
}

pub struct Detection3D {
    pub bbox: BoundingBox3D,
    pub confidence: f32,
    pub class_id: u32,
    pub class_name: [u8; 32],
    pub velocity_x: f32,      // m/s (for tracking)
    pub velocity_y: f32,
    pub velocity_z: f32,
    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 CmdVel {
    pub timestamp_ns: u64,
    pub linear: f32,   // Forward velocity in m/s
    pub angular: f32,  // Turning velocity in rad/s
}

let cmd = CmdVel::new(0.5, 0.2);  // 0.5 m/s forward, 0.2 rad/s left turn
let cmd = CmdVel::zero();          // Stop

let twist: Twist = cmd.into();
let cmd = CmdVel::from(twist);

pub struct DifferentialDriveCommand {
    pub left_velocity: f64,      // Left wheel in rad/s
    pub right_velocity: f64,     // Right wheel in rad/s
    pub max_acceleration: f64,   // Maximum acceleration in rad/s²
    pub enable: u8,              // Enable motors
    pub timestamp_ns: u64,
}

let cmd = DifferentialDriveCommand::new(5.0, 5.0);  // Both wheels forward
let cmd = DifferentialDriveCommand::stop();           // Emergency stop
let cmd = DifferentialDriveCommand::from_twist(&twist, wheel_separation);

pub struct TrajectoryPoint {
    pub position: [f64; 3],          // [x, y, z]
    pub velocity: [f64; 3],          // [vx, vy, vz]
    pub acceleration: [f64; 3],      // [ax, ay, az]
    pub orientation: [f64; 4],       // Quaternion [x, y, z, w]
    pub angular_velocity: [f64; 3],  // [wx, wy, wz]
    pub time_from_start: f64,        // Seconds from trajectory start
}

let point = TrajectoryPoint::new_2d(1.0, 2.0, 0.5);  // x, y, theta
let point = TrajectoryPoint::stationary([1.0, 2.0, 0.0]);

pub struct GenericMessage {
    pub inline_data: [u8; 256],    // First 256 bytes (inline)
    pub overflow_data: [u8; 3840], // Overflow up to 3840 more bytes
    pub 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"]);
}

pub struct Landmark {
    pub x: f32,           // X coordinate (pixels or normalized 0-1)
    pub y: f32,           // Y coordinate (pixels or normalized 0-1)
    pub visibility: f32,  // Confidence (0.0 - 1.0)
    pub index: u32,       // Landmark ID (e.g., 0=nose, 1=left_eye)
}

pub struct Landmark3D {
    pub x: f32,           // X (meters or normalized)
    pub y: f32,           // Y (meters or normalized)
    pub z: f32,           // Z (depth)
    pub visibility: f32,  // Confidence (0.0 - 1.0)
    pub index: u32,       // Landmark ID
}

pub struct LandmarkArray {
    pub num_landmarks: u32,
    pub dimension: u32,      // 2 for 2D, 3 for 3D
    pub instance_id: u32,    // Person/detection ID
    pub confidence: f32,     // Overall pose confidence (0.0 - 1.0)
    pub timestamp_ns: u64,
    pub bbox_x: f32,         // Bounding box X (pixels)
    pub bbox_y: f32,         // Bounding box Y (pixels)
    pub bbox_width: f32,
    pub bbox_height: f32,
}

let pose = LandmarkArray::coco_pose();       // 17 keypoints (COCO format)
let pose = LandmarkArray::mediapipe_pose();   // 33 keypoints
let hand = LandmarkArray::mediapipe_hand();   // 21 keypoints
let face = LandmarkArray::mediapipe_face();   // 468 keypoints

pub struct PlaneDetection {
    pub coefficients: [f64; 4],  // [a, b, c, d] where ax + by + cz + d = 0
    pub center: Point3,          // Center of the plane
    pub normal: Vector3,         // Normal vector
    pub size: [f64; 2],          // [width, height] if bounded
    pub inlier_count: u32,       // Number of inlier points
    pub confidence: f32,         // Detection confidence (0.0 - 1.0)
    pub plane_type: [u8; 16],    // "floor", "wall", "table", etc.
    pub timestamp_ns: u64,
}

pub struct SegmentationMask {
    pub width: u32,
    pub height: u32,
    pub num_classes: u32,  // Number of semantic classes
    pub mask_type: u32,    // 0=semantic, 1=instance, 2=panoptic
    pub timestamp_ns: u64,
    pub seq: u64,
    pub frame_id: [u8; 32],
}

let mask = SegmentationMask::semantic(640, 480, 21);   // 21 classes
let mask = SegmentationMask::instance(640, 480, 80);   // 80 instance classes
let mask = SegmentationMask::panoptic(640, 480, 133);  // Panoptic segmentation

pub struct NavGoal {
    pub target_pose: Pose2D,
    pub tolerance_position: f64,  // Position tolerance in meters
    pub tolerance_angle: f64,     // Orientation tolerance in radians
    pub timeout_seconds: f64,     // Max time to reach goal (0 = no limit)
    pub priority: u8,             // Goal priority (0 = highest)
    pub goal_id: u32,             // Unique goal identifier
    pub timestamp_ns: u64,
}

let goal = NavGoal::new(Pose2D::new(5.0, 3.0, 0.0));
let goal = NavGoal::new(target).with_timeout(30.0).with_priority(1);

let goal = NavGoal::new(Pose2D::new(5.0, 3.0, 1.57));
goal_topic.send(goal);

// In planner node
if let Some(goal) = goal_topic.recv() {
    if goal.is_reached(&current_pose) {
        hlog!(info, "Goal {} reached!", goal.goal_id);
    }
}

pub struct NavPath {
    pub waypoints: [Waypoint; 256],  // Waypoint = {pose, velocity, time_from_start, curvature, stop_required}
    pub waypoint_count: u16,
    pub total_length: f64,           // Total path length in meters
    pub duration_seconds: f64,       // Estimated completion time
    pub frame_id: [u8; 32],
    pub algorithm: [u8; 32],         // e.g., "a_star", "rrt"
    pub timestamp_ns: u64,
}

pub struct DiagnosticValue {
    pub key: [u8; 32],       // Key name (null-terminated)
    pub value: [u8; 64],     // Value as string (null-terminated)
    pub value_type: u8,      // 0=string, 1=int, 2=float, 3=bool
}

let val = DiagnosticValue::string("firmware", "v2.1.0");
let val = DiagnosticValue::int("error_count", 3);
let val = DiagnosticValue::float("temperature", 45.2);
let val = DiagnosticValue::bool("calibrated", true);

pub struct DiagnosticReport {
    pub component: [u8; 32],
    pub values: [DiagnosticValue; 16],
    pub value_count: u8,
    pub level: u8,            // Overall status level
    pub timestamp_ns: u64,
}

let mut report = DiagnosticReport::new("left_motor");
report.add_float("temperature", 42.5);
report.add_int("error_count", 0);
report.add_bool("calibrated", true);
report.add_string("firmware", "v2.1.0");
report.set_level(0); // OK
diag_topic.send(report);

pub struct NodeHeartbeat {
    pub state: u8,                 // Node execution state
    pub health: u8,                // Health status
    pub tick_count: u64,
    pub target_rate_hz: u32,
    pub actual_rate_hz: u32,
    pub error_count: u32,
    pub last_tick_timestamp: u64,  // Unix epoch seconds
    pub heartbeat_timestamp: u64,  // Unix epoch seconds
}