plozone 0.1.0

//! 3D voxel A\* pathfinding over the octree (feature `pathfinding`).
//!
//! Free space is defined as voxels that lie **inside a zone** and are **empty**
//! in the octree (occupied = obstacle). Because empty octree regions never
//! subdivide, free voxels are enumerated by walking a regular virtual grid over
//! the zone's AABB — the same reasoning as the hole scanner.

use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap};

use crate::octree::OctreeNode;
use crate::store::ZoneStore;

/// A waypoint on a path: the centre of a free voxel in ENU metres.
#[derive(Clone, Debug, PartialEq)]
pub struct PathNode {
    pub center: [f64; 3],
    pub depth: u8,
}

/// Hashable key for an `[f64; 3]` voxel centre, rounded to 0.1 mm to dodge
/// floating-point equality issues.
#[derive(PartialEq, Eq, Hash, Clone, Copy)]
struct OrdF64x3([i64; 3]);

impl From<[f64; 3]> for OrdF64x3 {
    fn from(p: [f64; 3]) -> Self {
        Self(std::array::from_fn(|i| (p[i] * 10_000.0).round() as i64))
    }
}

#[derive(Clone)]
struct AStarState {
    f: f64,
    center: [f64; 3],
}

impl PartialEq for AStarState {
    fn eq(&self, o: &Self) -> bool {
        self.f == o.f
    }
}
impl Eq for AStarState {}
impl PartialOrd for AStarState {
    fn partial_cmp(&self, o: &Self) -> Option<Ordering> {
        Some(self.cmp(o))
    }
}
impl Ord for AStarState {
    // Min-heap on f: reverse the comparison so BinaryHeap pops the smallest f.
    fn cmp(&self, o: &Self) -> Ordering {
        o.f.partial_cmp(&self.f).unwrap_or(Ordering::Equal)
    }
}

/// Find a path from `start` to `goal` (ENU metres) through free voxels inside
/// `zone_id` at the given `depth`. Returns `None` if the zone is unknown, has
/// no free space, or start and goal are disconnected.
///
/// Uses 26-connectivity (face + edge + corner neighbours) with an admissible
/// Euclidean heuristic.
pub fn find_path(
    octree: &OctreeNode,
    store: &ZoneStore,
    zone_id: u32,
    start: [f64; 3],
    goal: [f64; 3],
    depth: u8,
) -> Option<Vec<PathNode>> {
    let voxel = octree.half_len * 2.0 / (1u64 << depth) as f64;
    if voxel <= 0.0 {
        return None;
    }
    let aabb = store.zone_aabb(zone_id)?;

    // Enumerate free voxels (inside the zone and empty in the octree). The grid
    // is anchored at aabb_min + voxel/2 so every centre is reproducible.
    let mut free: HashMap<OrdF64x3, [f64; 3]> = HashMap::new();
    let mut x = aabb[0];
    while x < aabb[3] {
        let mut y = aabb[1];
        while y < aabb[4] {
            let mut z = aabb[2];
            while z < aabb[5] {
                let c = [x + voxel / 2.0, y + voxel / 2.0, z + voxel / 2.0];
                let empty = octree
                    .range_query([x, y, z], [x + voxel, y + voxel, z + voxel])
                    .is_empty();
                if empty && store.zone_contains(zone_id, c) {
                    free.insert(OrdF64x3::from(c), c);
                }
                z += voxel;
            }
            y += voxel;
        }
        x += voxel;
    }
    if free.is_empty() {
        return None;
    }

    // Snap to the same grid the free map uses: centre = aabb_min + voxel/2 + k*voxel.
    let snap = |p: [f64; 3]| -> [f64; 3] {
        std::array::from_fn(|i| {
            let k = ((p[i] - aabb[i] - voxel / 2.0) / voxel).round();
            aabb[i] + voxel / 2.0 + k * voxel
        })
    };
    // Snap, then fall back to the nearest free voxel if the snapped cell is occupied.
    let nearest_free = |p: [f64; 3]| -> Option<[f64; 3]> {
        let s = snap(p);
        if free.contains_key(&OrdF64x3::from(s)) {
            return Some(s);
        }
        free.values()
            .min_by(|a, b| {
                let da = (0..3).map(|i| (a[i] - p[i]).powi(2)).sum::<f64>();
                let db = (0..3).map(|i| (b[i] - p[i]).powi(2)).sum::<f64>();
                da.partial_cmp(&db).unwrap_or(Ordering::Equal)
            })
            .copied()
    };

    let start_v = nearest_free(start)?;
    let goal_v = nearest_free(goal)?;

    let h = |p: [f64; 3]| -> f64 { (0..3).map(|i| (p[i] - goal_v[i]).powi(2)).sum::<f64>().sqrt() };

    let mut open = BinaryHeap::new();
    let mut came: HashMap<OrdF64x3, [f64; 3]> = HashMap::new();
    let mut g_cost: HashMap<OrdF64x3, f64> = HashMap::new();

    open.push(AStarState { f: h(start_v), center: start_v });
    g_cost.insert(OrdF64x3::from(start_v), 0.0);

    // 26-connectivity neighbour offsets.
    let mut offsets = Vec::with_capacity(26);
    for dz in -1i32..=1 {
        for dy in -1i32..=1 {
            for dx in -1i32..=1 {
                if dx != 0 || dy != 0 || dz != 0 {
                    offsets.push([dx, dy, dz]);
                }
            }
        }
    }

    while let Some(AStarState { center, .. }) = open.pop() {
        if OrdF64x3::from(center) == OrdF64x3::from(goal_v) {
            // Reconstruct the path back to start.
            let mut path = vec![PathNode { center: goal_v, depth }];
            let mut cur = goal_v;
            while let Some(&prev) = came.get(&OrdF64x3::from(cur)) {
                path.push(PathNode { center: prev, depth });
                cur = prev;
                if OrdF64x3::from(cur) == OrdF64x3::from(start_v) {
                    break;
                }
            }
            path.reverse();
            return Some(path);
        }

        let g = *g_cost.get(&OrdF64x3::from(center)).unwrap_or(&f64::MAX);
        for off in &offsets {
            let nc = std::array::from_fn(|i| center[i] + off[i] as f64 * voxel);
            if !free.contains_key(&OrdF64x3::from(nc)) {
                continue;
            }
            let step = (0..3).map(|i| (off[i] as f64 * voxel).powi(2)).sum::<f64>().sqrt();
            let ng = g + step;
            if ng < *g_cost.get(&OrdF64x3::from(nc)).unwrap_or(&f64::MAX) {
                g_cost.insert(OrdF64x3::from(nc), ng);
                came.insert(OrdF64x3::from(nc), center);
                open.push(AStarState { f: ng + h(nc), center: nc });
            }
        }
    }

    None
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::coord::EnuConverter;
    use crate::zone::{Zone, ZoneEntry};

    fn box_zone() -> (OctreeNode, ZoneStore) {
        let conv = EnuConverter::new(0.0, 0.0, 0.0);
        // A flat ENU box zone via geodetic AABB roughly centred at origin.
        let store = ZoneStore::from_entries(
        &[ZoneEntry::new(
            1,
            Zone::Aabb { min: [-0.0002, -0.0002, 0.0], max: [0.0002, 0.0002, 4.0] },
        )],
            &conv,
        );
        let octree = OctreeNode::new([0.0; 3], 64.0);
        (octree, store)
    }

    #[test]
    fn empty_zone_path_exists_and_is_connected() {
        let (octree, store) = box_zone();
        let aabb = store.zone_aabb(1).unwrap();
        let start = [aabb[0] + 1.0, aabb[1] + 1.0, aabb[2] + 1.0];
        let goal = [aabb[3] - 1.0, aabb[4] - 1.0, aabb[2] + 1.0];
        let depth = 5;
        let path = find_path(&octree, &store, 1, start, goal, depth).expect("path exists");
        assert!(path.len() >= 2);

        let voxel = octree.half_len * 2.0 / (1u64 << depth) as f64;
        // Consecutive waypoints are 26-connected (each axis step ≤ ~1 voxel).
        for w in path.windows(2) {
            for i in 0..3 {
                let d = (w[0].center[i] - w[1].center[i]).abs();
                assert!(d <= voxel * 1.001, "step too large on axis {i}: {d} > {voxel}");
            }
        }
    }

    #[test]
    fn unknown_zone_has_no_path() {
        let (octree, store) = box_zone();
        assert!(find_path(&octree, &store, 99, [0.0; 3], [1.0; 3], 5).is_none());
    }

    #[test]
    fn fully_occupied_zone_has_no_free_space() {
        let (mut octree, store) = box_zone();
        let aabb = store.zone_aabb(1).unwrap();
        // Flood the zone footprint densely so every voxel at depth 4 is occupied.
        let voxel = octree.half_len * 2.0 / (1u64 << 4) as f64;
        let mut x = aabb[0];
        while x <= aabb[3] {
            let mut y = aabb[1];
            while y <= aabb[4] {
                let mut z = aabb[2];
                while z <= aabb[5] {
                    octree.insert([x, y, z], 256);
                    z += voxel / 3.0;
                }
                y += voxel / 3.0;
            }
            x += voxel / 3.0;
        }
        assert!(find_path(&octree, &store, 1, [aabb[0], aabb[1], aabb[2]], [aabb[3], aabb[4], aabb[5]], 4).is_none());
    }
}