mecha10-core 0.8.2

use serde::{Deserialize, Serialize};

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Image {
    pub timestamp: u64,

    pub width: u32,

    pub height: u32,

    pub encoding: String,

    pub data: Vec<u8>,

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

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct CompressedImage {
    pub timestamp: u64,

    pub width: u32,

    pub height: u32,

    pub format: String,

    pub data: Vec<u8>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub quality: Option<u8>,

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

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct LaserScan {
    pub timestamp: u64,

    pub angle_min: f32,

    pub angle_max: f32,

    pub angle_increment: f32,

    pub time_increment: f32,

    pub scan_time: f32,

    pub range_min: f32,

    pub range_max: f32,

    pub ranges: Vec<f32>,

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

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

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Odometry {
    pub timestamp: u64,

    pub position: [f32; 3],

    pub orientation: [f32; 4],

    pub linear_velocity: [f32; 3],

    pub angular_velocity: [f32; 3],

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

    #[serde(skip_serializing_if = "Option::is_none")]
    pub child_frame_id: Option<String>,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Imu {
    pub timestamp: u64,

    pub orientation: [f32; 4],

    #[serde(skip_serializing_if = "Option::is_none")]
    pub orientation_covariance: Option<[f32; 9]>,

    pub angular_velocity: [f32; 3],

    #[serde(skip_serializing_if = "Option::is_none")]
    pub angular_velocity_covariance: Option<[f32; 9]>,

    pub linear_acceleration: [f32; 3],

    #[serde(skip_serializing_if = "Option::is_none")]
    pub linear_acceleration_covariance: Option<[f32; 9]>,

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

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct GpsData {
    pub timestamp: u64,

    pub latitude: f64,

    pub longitude: f64,

    pub altitude: f64,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub horizontal_accuracy: Option<f32>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub vertical_accuracy: Option<f32>,

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

    #[serde(skip_serializing_if = "Option::is_none")]
    pub fix_type: Option<u8>,
}

impl Image {
    /// Creates a new RGB8 image
    pub fn new_rgb8(width: u32, height: u32, data: Vec<u8>) -> Self {
        Self::with_encoding(width, height, "rgb8", data)
    }

    /// Creates a new depth image (16-bit unsigned)
    pub fn new_depth(width: u32, height: u32, data: Vec<u8>) -> Self {
        Self::with_encoding(width, height, "16UC1", data)
    }

    /// Shared constructor behind `new_rgb8`/`new_depth`, which only differ in encoding.
    fn with_encoding(width: u32, height: u32, encoding: &str, data: Vec<u8>) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            width,
            height,
            encoding: encoding.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,
        };
        (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()
    }
}

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()
    }
}

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],
            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 {
        super::geometry3::distance_3d(self.position, other.position)
    }

    /// Calculates 2D distance to another position in meters
    pub fn distance_2d_to(&self, other: &Odometry) -> f32 {
        super::geometry3::distance_2d(self.position, other.position)
    }
}

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

impl Imu {
    /// Returns the magnitude of angular velocity in rad/s
    pub fn angular_speed(&self) -> f32 {
        super::geometry3::magnitude_3d(self.angular_velocity)
    }

    /// Normalizes the orientation quaternion
    pub fn normalize_orientation(&mut self) {
        super::geometry3::normalize_quaternion(&mut self.orientation);
    }
}

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