plozone 0.1.2

//! Adaptive-density octree over ENU point clouds.
//!
//! Nodes subdivide only where points accumulate, so empty volume stays cheap
//! while dense regions refine down to the configured [`MAX_DEPTH`].

/// Maximum subdivision depth. At a 1000 m world this bottoms out near ~4 mm
/// voxels — fine enough for RTK-grade precision.
pub const MAX_DEPTH: u8 = 17;

/// A single octree node: a cube centred at `center` with half-edge `half_len`.
pub struct OctreeNode {
    /// Node centre in ENU metres.
    pub center: [f64; 3],
    /// Half the cube's edge length, in metres.
    pub half_len: f64,
    /// Depth below the root (root is 0).
    pub depth: u8,
    /// Points held directly in this node (non-empty only on leaves).
    pub points: Vec<[f64; 3]>,
    /// The eight child octants, present once this node has subdivided.
    pub children: Option<Box<[OctreeNode; 8]>>,
}

impl OctreeNode {
    /// Root node centred at `center` with the given half-edge length.
    pub fn new(center: [f64; 3], half_len: f64) -> Self {
        Self { center, half_len, depth: 0, points: vec![], children: None }
    }

    fn with_depth(center: [f64; 3], half_len: f64, depth: u8) -> Self {
        Self { center, half_len, depth, points: vec![], children: None }
    }

    /// Insert a point, subdividing once a leaf exceeds `threshold` points
    /// (unless [`MAX_DEPTH`] is reached). Points outside this node are ignored.
    pub fn insert(&mut self, p: [f64; 3], threshold: usize) {
        if !self.contains(p) {
            return;
        }
        // Leaf that still has room, or we have hit the depth floor: store here.
        if self.depth >= MAX_DEPTH || (self.children.is_none() && self.points.len() < threshold) {
            self.points.push(p);
            return;
        }
        // Need to split: create children and redistribute existing points.
        if self.children.is_none() {
            self.subdivide();
            let old: Vec<_> = self.points.drain(..).collect();
            for op in old {
                self.insert(op, threshold);
            }
        }
        if let Some(ch) = &mut self.children {
            for c in ch.iter_mut() {
                if c.contains(p) {
                    c.insert(p, threshold);
                    break;
                }
            }
        }
    }

    /// Insert many points.
    pub fn insert_batch(&mut self, pts: &[[f64; 3]], threshold: usize) {
        for &p in pts {
            self.insert(p, threshold);
        }
    }

    /// Half-open containment `[center-h, center+h)` so adjacent cells partition
    /// space without double-counting boundary points.
    fn contains(&self, p: [f64; 3]) -> bool {
        let h = self.half_len;
        (0..3).all(|i| p[i] >= self.center[i] - h && p[i] < self.center[i] + h)
    }

    fn subdivide(&mut self) {
        let h = self.half_len / 2.0;
        let [cx, cy, cz] = self.center;
        let d = self.depth + 1;
        let offs = [
            [-h, -h, -h],
            [h, -h, -h],
            [-h, h, -h],
            [h, h, -h],
            [-h, -h, h],
            [h, -h, h],
            [-h, h, h],
            [h, h, h],
        ];
        self.children = Some(Box::new(std::array::from_fn(|i| {
            let [dx, dy, dz] = offs[i];
            OctreeNode::with_depth([cx + dx, cy + dy, cz + dz], h, d)
        })));
    }

    /// All nodes that exist at the given depth. Note that empty regions never
    /// subdivide, so this only surfaces *materialised* nodes.
    pub fn nodes_at_depth(&self, target: u8) -> Vec<&OctreeNode> {
        if self.depth == target {
            return vec![self];
        }
        if let Some(ch) = &self.children {
            ch.iter().flat_map(|c| c.nodes_at_depth(target)).collect()
        } else {
            vec![]
        }
    }

    /// Points within the inclusive ENU box `[min, max]`.
    pub fn range_query(&self, min: [f64; 3], max: [f64; 3]) -> Vec<&[f64; 3]> {
        let h = self.half_len;
        let overlaps = (0..3).all(|i| self.center[i] + h > min[i] && self.center[i] - h < max[i]);
        if !overlaps {
            return vec![];
        }
        if self.children.is_none() {
            return self
                .points
                .iter()
                .filter(|p| (0..3).all(|i| p[i] >= min[i] && p[i] <= max[i]))
                .collect();
        }
        self.children
            .as_ref()
            .unwrap()
            .iter()
            .flat_map(|c| c.range_query(min, max))
            .collect()
    }
}

/// Depth needed to resolve `max_error_m` in a world of `world_size_m` edge.
pub fn depth_for_accuracy(max_error_m: f64, world_size_m: f64) -> u8 {
    let mut size = world_size_m;
    let mut d = 0u8;
    while size > max_error_m && d < MAX_DEPTH {
        size /= 2.0;
        d += 1;
    }
    d
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn insert_and_range_query() {
        let mut t = OctreeNode::new([0.0; 3], 100.0);
        t.insert([1.0, 1.0, 1.0], 4);
        t.insert([2.0, 2.0, 2.0], 4);
        t.insert([-50.0, -50.0, -50.0], 4);

        let near = t.range_query([0.0, 0.0, 0.0], [5.0, 5.0, 5.0]);
        assert_eq!(near.len(), 2);
        let all = t.range_query([-100.0, -100.0, -100.0], [100.0, 100.0, 100.0]);
        assert_eq!(all.len(), 3);
    }

    #[test]
    fn points_outside_root_are_dropped() {
        let mut t = OctreeNode::new([0.0; 3], 10.0);
        t.insert([100.0, 0.0, 0.0], 4);
        let all = t.range_query([-10.0, -10.0, -10.0], [10.0, 10.0, 10.0]);
        assert!(all.is_empty());
    }

    #[test]
    fn subdivision_preserves_all_points() {
        let mut t = OctreeNode::new([0.0; 3], 100.0);
        // Spread points across octants so the node must split past threshold.
        let pts = [
            [10.0, 10.0, 10.0],
            [-10.0, 10.0, 10.0],
            [10.0, -10.0, 10.0],
            [10.0, 10.0, -10.0],
            [-10.0, -10.0, -10.0],
        ];
        t.insert_batch(&pts, 2);
        assert!(t.children.is_some(), "should have subdivided past threshold");
        let found = t.range_query([-100.0, -100.0, -100.0], [100.0, 100.0, 100.0]);
        assert_eq!(found.len(), pts.len(), "no points lost during subdivision");
    }

    #[test]
    fn depth_for_accuracy_scales() {
        // 1000 m world, want ≤ 1 m: 1000 → 500 → ... → ~0.98 m at depth 10.
        assert_eq!(depth_for_accuracy(1.0, 1000.0), 10);
        // Already fine enough.
        assert_eq!(depth_for_accuracy(2000.0, 1000.0), 0);
        // Never exceeds MAX_DEPTH.
        assert_eq!(depth_for_accuracy(1e-9, 1000.0), MAX_DEPTH);
    }
}