Sensor Messages

use horus::prelude::*;

// Create a new laser scan
let mut scan = LaserScan::new();

// Set scan parameters
scan.angle_min = -std::f32::consts::PI;  // -180 degrees
scan.angle_max = std::f32::consts::PI;   // +180 degrees
scan.range_min = 0.1;                     // 10cm minimum
scan.range_max = 30.0;                    // 30m maximum

// Fill in range data (360 readings)
for i in 0..360 {
    scan.ranges[i] = 2.5;  // 2.5m reading at all angles
}

// Get angle for a specific reading
let angle = scan.angle_at(90);  // 90th reading
println!("Angle at index 90: {:.2} rad", angle);

// Check if a reading is valid
if scan.is_range_valid(45) {
    println!("Reading at 45 is valid: {:.2}m", scan.ranges[45]);
}

// Get statistics
let valid = scan.valid_count();
let min_dist = scan.min_range();
println!("Valid readings: {}, Min distance: {:?}", valid, min_dist);

use horus::prelude::*;

// Create new IMU message
let mut imu = Imu::new();

// Set orientation from Euler angles (roll, pitch, yaw)
imu.set_orientation_from_euler(0.0, 0.05, 1.57);  // Slight pitch, 90° yaw

// Set angular velocity [x, y, z] in rad/s
imu.angular_velocity = [0.0, 0.0, 0.5];  // Rotating around Z-axis

// Set linear acceleration [x, y, z] in m/s²
imu.linear_acceleration = [0.0, 0.0, 9.81];  // Gravity pointing up

// Check data availability
if imu.has_orientation() {
    println!("Orientation: {:?}", imu.orientation);
}

// Get as Vector3 for calculations
let angular_vel = imu.angular_velocity_vec();
let linear_acc = imu.linear_acceleration_vec();
println!("Angular velocity magnitude: {:.2} rad/s", angular_vel.magnitude());

// Validate data
assert!(imu.is_valid());

use horus::prelude::*;

// Create odometry message
let mut odom = Odometry::new();

// Set coordinate frames
odom.set_frames("odom", "base_link");

// Update with current pose and velocity
let pose = Pose2D::new(5.0, 3.0, 0.785);  // x, y, theta
let twist = Twist::new_2d(0.5, 0.1);       // linear, angular
odom.update(pose, twist);

// Access pose and velocity
println!("Position: ({:.2}, {:.2})", odom.pose.x, odom.pose.y);
println!("Velocity: {:.2} m/s", odom.twist.linear[0]);

// Validate
assert!(odom.is_valid());

use horus::prelude::*;

// Create ultrasonic range reading
let ultrasonic = RangeSensor::new(RangeSensor::ULTRASONIC, 1.5);  // 1.5m reading

// Create infrared range reading
let ir = RangeSensor::new(RangeSensor::INFRARED, 0.3);  // 30cm reading

// Check if reading is valid (within sensor limits)
if ultrasonic.is_valid() {
    println!("Distance: {:.2}m", ultrasonic.range);
}

// Access sensor parameters
println!("FOV: {:.2} rad", ultrasonic.field_of_view);
println!("Range: {:.2} - {:.2}m", ultrasonic.min_range, ultrasonic.max_range);

use horus::prelude::*;

// Create GPS fix from coordinates
let fix = NavSatFix::from_coordinates(
    37.7749,    // Latitude (positive = North)
    -122.4194,  // Longitude (positive = East)
    10.5        // Altitude in meters
);

// Check fix status
if fix.has_fix() {
    println!("GPS Fix acquired!");
    println!("Position: {:.6}°N, {:.6}°E", fix.latitude, fix.longitude);
    println!("Altitude: {:.1}m", fix.altitude);
    println!("Satellites: {}", fix.satellites_visible);
}

// Get accuracy estimate
let accuracy = fix.horizontal_accuracy();
println!("Horizontal accuracy: ±{:.1}m", accuracy);

// Calculate distance to another position
let destination = NavSatFix::from_coordinates(37.8044, -122.2712, 0.0);
let distance = fix.distance_to(&destination);
println!("Distance to destination: {:.0}m", distance);

// Validate coordinates
assert!(fix.is_valid());

use horus::prelude::*;

// Create battery state
let mut battery = BatteryState::new(12.6, 85.0);  // 12.6V, 85%

// Set additional fields
battery.current = -2.5;  // Discharging at 2.5A
battery.temperature = 28.0;
battery.power_supply_status = BatteryState::STATUS_DISCHARGING;

// Check battery level
if battery.is_low(20.0) {
    println!("Battery low!");
}
if battery.is_critical() {
    println!("Battery critical (below 10%)!");
}

// Estimate remaining time
if let Some(remaining) = battery.time_remaining() {
    println!("Estimated time remaining: {:.0} seconds", remaining);
}

println!("Voltage: {:.2}V", battery.voltage);
println!("Charge: {:.0}%", battery.percentage);
println!("Temperature: {:.1}°C", battery.temperature);

use horus::prelude::*;

struct SensorFusionNode {
    imu_sub: Topic<Imu>,
    odom_sub: Topic<Odometry>,
    gps_sub: Topic<NavSatFix>,
    fused_pose_pub: Topic<Pose2D>,
    // Extended Kalman Filter state
    ekf_state: [f64; 6],  // [x, y, theta, vx, vy, omega]
}

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

    fn tick(&mut self) {
        // Process IMU at highest rate
        if let Some(imu) = self.imu_sub.recv() {
            if imu.is_valid() {
                // Use angular velocity for heading prediction
                let omega = imu.angular_velocity[2];
                self.predict_state(omega);
            }
        }

        // Process odometry
        if let Some(odom) = self.odom_sub.recv() {
            if odom.is_valid() {
                // Update with wheel odometry
                self.update_odometry(&odom);
            }
        }

        // Process GPS (lower rate, absolute position)
        if let Some(gps) = self.gps_sub.recv() {
            if gps.has_fix() && gps.is_valid() {
                // Update with GPS (when available)
                self.update_gps(&gps);
            }
        }

        // Publish fused pose
        let pose = Pose2D::new(
            self.ekf_state[0],
            self.ekf_state[1],
            self.ekf_state[2]
        );
        self.fused_pose_pub.send(pose);
    }
}

impl SensorFusionNode {
    fn predict_state(&mut self, omega: f64) {
        // EKF prediction step using IMU
        let dt = 0.01;  // 100Hz
        self.ekf_state[2] += omega * dt;
    }

    fn update_odometry(&mut self, odom: &Odometry) {
        // EKF update with odometry measurement
        // ... implementation
    }

    fn update_gps(&mut self, gps: &NavSatFix) {
        // EKF update with GPS measurement
        // ... implementation
    }
}