mecha10-core 0.6.2

// Sensor Message Types

use serde::{Deserialize, Serialize};

/// Image data from a camera
///
/// Supports various encodings (rgb8, bgr8, mono8, depth, etc.)
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Image {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Image width in pixels
    pub width: u32,

    /// Image height in pixels
    pub height: u32,

    /// Encoding format (e.g., "rgb8", "bgr8", "mono8", "16UC1" for depth)
    pub encoding: String,

    /// Raw image data
    pub data: Vec<u8>,

    /// Optional camera frame ID
    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,
}

/// Compressed image data for network-efficient transmission
///
/// Used for streaming over network to remote nodes or dashboards.
/// Supports JPEG, PNG, H.264, and other compressed formats.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct CompressedImage {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Original image width in pixels
    pub width: u32,

    /// Original image height in pixels
    pub height: u32,

    /// Compression format (e.g., "jpeg", "png", "h264")
    pub format: String,

    /// Compressed image data
    pub data: Vec<u8>,

    /// Optional JPEG quality (1-100) or compression settings
    #[serde(skip_serializing_if = "Option::is_none")]
    pub quality: Option<u8>,

    /// Optional camera frame ID
    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,
}

/// 2D laser scan data
///
/// Represents a planar laser scan (e.g., from RPLidar, Hokuyo)
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct LaserScan {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Start angle of the scan in radians
    pub angle_min: f32,

    /// End angle of the scan in radians
    pub angle_max: f32,

    /// Angular distance between measurements in radians
    pub angle_increment: f32,

    /// Time between measurements in seconds
    pub time_increment: f32,

    /// Time between scans in seconds
    pub scan_time: f32,

    /// Minimum range value in meters
    pub range_min: f32,

    /// Maximum range value in meters
    pub range_max: f32,

    /// Range data in meters (one per angle)
    pub ranges: Vec<f32>,

    /// Intensity data (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub intensities: Option<Vec<f32>>,

    /// Optional frame ID
    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,
}

/// Odometry data (position and velocity)
///
/// Represents the robot's estimated position and velocity
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Odometry {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Position (x, y, z) in meters
    pub position: [f32; 3],

    /// Orientation as quaternion (x, y, z, w)
    pub orientation: [f32; 4],

    /// Linear velocity (x, y, z) in m/s
    pub linear_velocity: [f32; 3],

    /// Angular velocity (x, y, z) in rad/s
    pub angular_velocity: [f32; 3],

    /// Optional frame ID
    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,

    /// Optional child frame ID
    #[serde(skip_serializing_if = "Option::is_none")]
    pub child_frame_id: Option<String>,
}

/// IMU (Inertial Measurement Unit) data
///
/// Provides orientation, angular velocity, and linear acceleration
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Imu {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Orientation as quaternion (x, y, z, w)
    pub orientation: [f32; 4],

    /// Orientation covariance (9 elements, row-major)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub orientation_covariance: Option<[f32; 9]>,

    /// Angular velocity (x, y, z) in rad/s
    pub angular_velocity: [f32; 3],

    /// Angular velocity covariance (9 elements, row-major)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub angular_velocity_covariance: Option<[f32; 9]>,

    /// Linear acceleration (x, y, z) in m/s²
    pub linear_acceleration: [f32; 3],

    /// Linear acceleration covariance (9 elements, row-major)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub linear_acceleration_covariance: Option<[f32; 9]>,

    /// Optional frame ID
    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,
}

/// GPS data
///
/// Global positioning data with optional accuracy estimates
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct GpsData {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Latitude in degrees
    pub latitude: f64,

    /// Longitude in degrees
    pub longitude: f64,

    /// Altitude above mean sea level in meters
    pub altitude: f64,

    /// Horizontal position accuracy in meters (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub horizontal_accuracy: Option<f32>,

    /// Vertical position accuracy in meters (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub vertical_accuracy: Option<f32>,

    /// Number of satellites used
    #[serde(skip_serializing_if = "Option::is_none")]
    pub satellites: Option<u8>,

    /// GPS fix type (0=no fix, 1=GPS, 2=DGPS, 3=PPS, 4=RTK, 5=Float RTK)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub fix_type: Option<u8>,
}

// Helper methods for Image
impl Image {
    /// Creates a new RGB8 image
    pub fn new_rgb8(width: u32, height: u32, data: Vec<u8>) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            width,
            height,
            encoding: "rgb8".to_string(),
            data,
            frame_id: None,
        }
    }

    /// Creates a new depth image (16-bit unsigned)
    pub fn new_depth(width: u32, height: u32, data: Vec<u8>) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            width,
            height,
            encoding: "16UC1".to_string(),
            data,
            frame_id: None,
        }
    }

    /// Returns the expected data size in bytes
    pub fn expected_size(&self) -> usize {
        let bytes_per_pixel = match self.encoding.as_str() {
            "rgb8" | "bgr8" => 3,
            "rgba8" | "bgra8" => 4,
            "mono8" => 1,
            "mono16" | "16UC1" => 2,
            _ => 1, // default
        };
        (self.width * self.height) as usize * bytes_per_pixel
    }

    /// Checks if the image data size is valid
    pub fn is_valid(&self) -> bool {
        self.data.len() == self.expected_size()
    }
}

// Helper methods for LaserScan
impl LaserScan {
    /// Returns the number of scan points
    pub fn num_points(&self) -> usize {
        self.ranges.len()
    }

    /// Converts scan point to (x, y) coordinates in meters
    pub fn point_at(&self, index: usize) -> Option<(f32, f32)> {
        if index >= self.ranges.len() {
            return None;
        }

        let range = self.ranges[index];
        if !self.is_valid_range(range) {
            return None;
        }

        let angle = self.angle_min + (index as f32) * self.angle_increment;
        Some((range * angle.cos(), range * angle.sin()))
    }

    /// Checks if a range value is valid
    pub fn is_valid_range(&self, range: f32) -> bool {
        range.is_finite() && range >= self.range_min && range <= self.range_max
    }

    /// Returns all valid points as (x, y) coordinates
    pub fn valid_points(&self) -> Vec<(f32, f32)> {
        (0..self.ranges.len()).filter_map(|i| self.point_at(i)).collect()
    }
}

// Helper methods for Odometry
impl Odometry {
    /// Creates a new odometry message at the origin
    pub fn new() -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            position: [0.0, 0.0, 0.0],
            orientation: [0.0, 0.0, 0.0, 1.0], // Identity quaternion
            linear_velocity: [0.0, 0.0, 0.0],
            angular_velocity: [0.0, 0.0, 0.0],
            frame_id: None,
            child_frame_id: None,
        }
    }

    /// Returns the 2D linear velocity (vx, vy)
    pub fn velocity_2d(&self) -> (f32, f32) {
        (self.linear_velocity[0], self.linear_velocity[1])
    }

    /// Returns the speed (magnitude of velocity) in m/s
    pub fn speed(&self) -> f32 {
        let vx = self.linear_velocity[0];
        let vy = self.linear_velocity[1];
        let vz = self.linear_velocity[2];
        (vx * vx + vy * vy + vz * vz).sqrt()
    }

    /// Returns the yaw angle in radians (rotation around z-axis)
    pub fn yaw(&self) -> f32 {
        let q = self.orientation;
        (2.0 * (q[3] * q[2] + q[0] * q[1])).atan2(1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]))
    }

    /// Calculates distance to another position in meters
    pub fn distance_to(&self, other: &Odometry) -> f32 {
        let dx = self.position[0] - other.position[0];
        let dy = self.position[1] - other.position[1];
        let dz = self.position[2] - other.position[2];
        (dx * dx + dy * dy + dz * dz).sqrt()
    }

    /// Calculates 2D distance to another position in meters
    pub fn distance_2d_to(&self, other: &Odometry) -> f32 {
        let dx = self.position[0] - other.position[0];
        let dy = self.position[1] - other.position[1];
        (dx * dx + dy * dy).sqrt()
    }
}

impl Default for Odometry {
    fn default() -> Self {
        Self::new()
    }
}

// Helper methods for Imu
impl Imu {
    /// Returns the magnitude of angular velocity in rad/s
    pub fn angular_speed(&self) -> f32 {
        let w = self.angular_velocity;
        (w[0] * w[0] + w[1] * w[1] + w[2] * w[2]).sqrt()
    }

    /// Normalizes the orientation quaternion
    pub fn normalize_orientation(&mut self) {
        let q = self.orientation;
        let mag = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
        if mag > 0.0 {
            self.orientation = [q[0] / mag, q[1] / mag, q[2] / mag, q[3] / mag];
        }
    }
}

// Helper methods for GpsData
impl GpsData {
    /// Calculates distance to another GPS point using Haversine formula (in meters)
    pub fn distance_to(&self, other: &GpsData) -> f64 {
        const EARTH_RADIUS_M: f64 = 6371000.0;

        let lat1 = self.latitude.to_radians();
        let lat2 = other.latitude.to_radians();
        let dlat = (other.latitude - self.latitude).to_radians();
        let dlon = (other.longitude - self.longitude).to_radians();

        let a = (dlat / 2.0).sin().powi(2) + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
        let c = 2.0 * a.sqrt().atan2((1.0 - a).sqrt());

        EARTH_RADIUS_M * c
    }
}