plozone 0.1.2

//! LiDAR pose math and scan ingestion (feature `lidar`).
//!
//! This module covers the dependency-free, fully-testable part of LiDAR
//! support: a unit-quaternion [`Pose`], a [`ScanFrame`], and [`ingest_scan`],
//! which transforms sensor-frame points into world ENU and feeds them to the
//! octree. The actual RPLidar serial driver (which needs `serialport` and real
//! hardware) is a later milestone.

use crate::octree::OctreeNode;

/// A unit quaternion `(w, x, y, z)` representing a 3D rotation.
///
/// Small and self-contained so the `lidar` feature pulls in no extra crates.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Quat {
    pub w: f64,
    pub x: f64,
    pub y: f64,
    pub z: f64,
}

impl Quat {
    /// The identity rotation.
    pub const IDENTITY: Quat = Quat { w: 1.0, x: 0.0, y: 0.0, z: 0.0 };

    /// Rotation of `angle_rad` about a (not necessarily normalized) `axis`.
    /// A zero-length axis yields the identity.
    pub fn from_axis_angle(axis: [f64; 3], angle_rad: f64) -> Self {
        let len = (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]).sqrt();
        if len == 0.0 {
            return Self::IDENTITY;
        }
        let (s, c) = (angle_rad * 0.5).sin_cos();
        let k = s / len;
        Self { w: c, x: axis[0] * k, y: axis[1] * k, z: axis[2] * k }
    }

    /// Return this quaternion scaled to unit length (identity if degenerate).
    pub fn normalized(self) -> Self {
        let n = (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z).sqrt();
        if n == 0.0 {
            return Self::IDENTITY;
        }
        Self { w: self.w / n, x: self.x / n, y: self.y / n, z: self.z / n }
    }

    /// Rotate a vector by this quaternion (assumes unit length).
    pub fn rotate(self, v: [f64; 3]) -> [f64; 3] {
        // v' = v + 2w(q × v) + 2(q × (q × v)), computed via the standard
        // t = 2 (q.xyz × v); v' = v + w t + q.xyz × t identity.
        let q = [self.x, self.y, self.z];
        let t = cross(q, v).map(|c| c * 2.0);
        let qt = cross(q, t);
        [
            v[0] + self.w * t[0] + qt[0],
            v[1] + self.w * t[1] + qt[1],
            v[2] + self.w * t[2] + qt[2],
        ]
    }
}

/// Hamilton product `self * rhs` (apply `rhs` first, then `self`).
impl std::ops::Mul for Quat {
    type Output = Quat;
    fn mul(self, r: Quat) -> Quat {
        Quat {
            w: self.w * r.w - self.x * r.x - self.y * r.y - self.z * r.z,
            x: self.w * r.x + self.x * r.w + self.y * r.z - self.z * r.y,
            y: self.w * r.y - self.x * r.z + self.y * r.w + self.z * r.x,
            z: self.w * r.z + self.x * r.y - self.y * r.x + self.z * r.w,
        }
    }
}

fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0],
    ]
}

/// A sensor pose: world position plus orientation.
#[derive(Debug, Clone, Copy)]
pub struct Pose {
    pub position: [f64; 3],
    pub orientation: Quat,
}

impl Pose {
    /// A level (un-rotated) pose at `position`.
    pub fn level(position: [f64; 3]) -> Self {
        Self { position, orientation: Quat::IDENTITY }
    }

    /// Transform a sensor-frame point (`f32`) into world coordinates (`f64`).
    pub fn transform(&self, p: [f32; 3]) -> [f64; 3] {
        let local = [p[0] as f64, p[1] as f64, p[2] as f64];
        let r = self.orientation.rotate(local);
        [
            self.position[0] + r[0],
            self.position[1] + r[1],
            self.position[2] + r[2],
        ]
    }
}

/// A single LiDAR scan: points in the sensor frame, the pose they were taken
/// at, and a nanosecond timestamp.
pub struct ScanFrame {
    pub points: Vec<[f32; 3]>,
    pub pose: Pose,
    pub ts_ns: u64,
}

/// Transform every point of `frame` into world coordinates and insert it into
/// the octree.
pub fn ingest_scan(octree: &mut OctreeNode, frame: &ScanFrame, threshold: usize) {
    let world: Vec<[f64; 3]> = frame.points.iter().map(|&p| frame.pose.transform(p)).collect();
    octree.insert_batch(&world, threshold);
}

#[cfg(test)]
mod tests {
    use super::*;
    use std::f64::consts::FRAC_PI_2;

    fn approx(a: [f64; 3], b: [f64; 3]) {
        for i in 0..3 {
            assert!((a[i] - b[i]).abs() < 1e-9, "axis {i}: {a:?} vs {b:?}");
        }
    }

    #[test]
    fn identity_pose_just_translates() {
        let pose = Pose::level([10.0, 20.0, 30.0]);
        approx(pose.transform([1.0, 2.0, 3.0]), [11.0, 22.0, 33.0]);
    }

    #[test]
    fn yaw_90_maps_x_to_y() {
        // +90° about Z: x-axis → y-axis.
        let q = Quat::from_axis_angle([0.0, 0.0, 1.0], FRAC_PI_2);
        approx(q.rotate([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0]);
        approx(q.rotate([0.0, 1.0, 0.0]), [-1.0, 0.0, 0.0]);
        // Rotation preserves the axis it turns about.
        approx(q.rotate([0.0, 0.0, 1.0]), [0.0, 0.0, 1.0]);
    }

    #[test]
    fn pose_rotates_then_translates() {
        let pose = Pose {
            position: [5.0, 0.0, 0.0],
            orientation: Quat::from_axis_angle([0.0, 0.0, 1.0], FRAC_PI_2),
        };
        // Sensor point on +x → rotated to +y → translated by position.
        approx(pose.transform([2.0, 0.0, 0.0]), [5.0, 2.0, 0.0]);
    }

    #[test]
    fn composition_matches_sequential_rotation() {
        let a = Quat::from_axis_angle([0.0, 0.0, 1.0], FRAC_PI_2);
        let combined = a * a; // 180° about Z
        approx(combined.rotate([1.0, 0.0, 0.0]), [-1.0, 0.0, 0.0]);
    }

    #[test]
    fn normalize_recovers_unit_length() {
        let q = Quat { w: 2.0, x: 0.0, y: 0.0, z: 0.0 }.normalized();
        assert!((q.w - 1.0).abs() < 1e-12);
        // A degenerate (zero) quaternion falls back to identity.
        assert_eq!(Quat { w: 0.0, x: 0.0, y: 0.0, z: 0.0 }.normalized(), Quat::IDENTITY);
    }

    #[test]
    fn ingest_scan_places_points_in_world() {
        let mut octree = OctreeNode::new([0.0; 3], 100.0);
        let frame = ScanFrame {
            points: vec![[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]],
            pose: Pose {
                position: [10.0, 10.0, 0.0],
                orientation: Quat::from_axis_angle([0.0, 0.0, 1.0], FRAC_PI_2),
            },
            ts_ns: 42,
        };
        ingest_scan(&mut octree, &frame, 8);
        // [1,0,0] → rotate to [0,1,0] → +pos = [10,11,0]; [0,1,0] → [9,10,0].
        let found = octree.range_query([8.0, 9.0, -1.0], [11.0, 12.0, 1.0]);
        assert_eq!(found.len(), 2, "both transformed points landed in the box");
    }
}