geographdb_core/spatial/

octree.rs

//! Octree spatial indexing for 3D-native graph database.
//!
//! This module provides an octree implementation optimized for 3D spatial
//! queries in graph databases. Unlike traditional R-trees, octrees provide
//! optimal spatial partitioning for uniform 3D data distributions and
//! enable efficient pruning of graph traversal algorithms.

use crate::storage::data_structures::NodePoint;
use glam::Vec3;
use serde::{Deserialize, Serialize};

/// Per-query instrumentation for octree lookup complexity analysis.
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Serialize, Deserialize)]
pub struct OctreeQueryStats {
    /// Number of octree nodes visited during traversal.
    pub nodes_visited: usize,
    /// Number of leaf octants scanned.
    pub leaf_nodes_scanned: usize,
    /// Number of points distance-tested against the query sphere.
    pub points_tested: usize,
    /// Number of points that matched the query predicate.
    pub points_returned: usize,
}

/// An octant in 3D space, represented by its bounding box.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct BoundingBox {
    /// Minimum corner of the bounding box.
    pub min: Vec3,
    /// Maximum corner of the bounding box.
    pub max: Vec3,
}

impl BoundingBox {
    /// Create a new bounding box from minimum and maximum corners.
    pub fn new(min: Vec3, max: Vec3) -> Self {
        Self { min, max }
    }

    /// Create a new bounding box centered at origin with given half-size.
    pub fn centered(center: Vec3, half_size: f32) -> Self {
        Self {
            min: center - Vec3::splat(half_size),
            max: center + Vec3::splat(half_size),
        }
    }

    /// Check if a point is contained within this bounding box.
    pub fn contains(&self, point: Vec3) -> bool {
        point.x >= self.min.x
            && point.x <= self.max.x
            && point.y >= self.min.y
            && point.y <= self.max.y
            && point.z >= self.min.z
            && point.z <= self.max.z
    }

    /// Check if this bounding box intersects with another bounding box.
    pub fn intersects(&self, other: &BoundingBox) -> bool {
        self.min.x <= other.max.x
            && self.max.x >= other.min.x
            && self.min.y <= other.max.y
            && self.max.y >= other.min.y
            && self.min.z <= other.max.z
            && self.max.z >= other.min.z
    }

    /// Calculate the center of this bounding box.
    pub fn center(&self) -> Vec3 {
        (self.min + self.max) * 0.5
    }

    /// Calculate the size (dimensions) of this bounding box.
    pub fn size(&self) -> Vec3 {
        self.max - self.min
    }

    /// Subdivide this bounding box into 8 octants.
    pub fn subdivide(&self) -> [BoundingBox; 8] {
        let center = self.center();
        // Calculate half-size of this bounding box
        let _half_size = (self.max - self.min) * 0.5;

        [
            // Bottom octants (z < center.z)
            BoundingBox::new(self.min, center), // Bottom-front-left
            BoundingBox::new(
                Vec3::new(center.x, self.min.y, self.min.z),
                Vec3::new(self.max.x, center.y, center.z),
            ), // Bottom-front-right
            BoundingBox::new(
                Vec3::new(self.min.x, center.y, self.min.z),
                Vec3::new(center.x, self.max.y, center.z),
            ), // Bottom-back-left
            BoundingBox::new(
                Vec3::new(center.x, center.y, self.min.z),
                Vec3::new(self.max.x, self.max.y, center.z),
            ), // Bottom-back-right
            // Top octants (z >= center.z)
            BoundingBox::new(
                Vec3::new(self.min.x, self.min.y, center.z),
                Vec3::new(center.x, center.y, self.max.z),
            ), // Top-front-left
            BoundingBox::new(
                Vec3::new(center.x, self.min.y, center.z),
                Vec3::new(self.max.x, center.y, self.max.z),
            ), // Top-front-right
            BoundingBox::new(
                Vec3::new(self.min.x, center.y, center.z),
                Vec3::new(center.x, self.max.y, self.max.z),
            ), // Top-back-left
            BoundingBox::new(center, self.max), // Top-back-right
        ]
    }
}

/// A node in the octree structure.
#[derive(Debug, Clone)]
pub struct OctreeNode {
    /// The bounding box that defines this node's spatial extent.
    pub bounds: BoundingBox,

    /// The depth of this node in the octree (0 for root).
    pub depth: u32,

    /// The nodes contained directly in this octant (if this is a leaf).
    pub nodes: Vec<NodePoint>,

    /// Child octants (if this node has been subdivided).
    pub children: Option<[Box<OctreeNode>; 8]>,

    /// Flag indicating if this node has been subdivided.
    pub is_subdivided: bool,
}

impl OctreeNode {
    /// Create a new octree node with the given bounds and depth.
    pub fn new(bounds: BoundingBox, depth: u32) -> Self {
        Self {
            bounds,
            depth,
            nodes: Vec::new(),
            children: None,
            is_subdivided: false,
        }
    }

    /// Check if this node is a leaf (has no children).
    pub fn is_leaf(&self) -> bool {
        !self.is_subdivided
    }

    /// Get the maximum number of nodes this leaf can hold before subdivision.
    pub fn capacity(&self) -> usize {
        // Capacity increases with depth to prevent excessive subdivision
        // at deeper levels where nodes are more sparse
        (8 + self.depth as usize * 2).min(32)
    }

    /// Insert a node into this octree node.
    ///
    /// If this node is a leaf and has capacity, the node is added directly.
    /// If this node is a leaf and is at capacity, it will be subdivided and
    /// the nodes redistributed.
    /// If this node has children, the node is inserted into the appropriate child.
    pub fn insert(&mut self, node: NodePoint) -> bool {
        // Check if the node is within this octant's bounds
        let point = Vec3::new(node.x, node.y, node.z);
        if !self.bounds.contains(point) {
            return false;
        }

        if self.is_leaf() {
            // If we have capacity, add the node directly
            if self.nodes.len() < self.capacity() || self.depth >= 16 {
                self.nodes.push(node);
                return true;
            }

            // Otherwise, subdivide and redistribute existing nodes
            self.subdivide();

            // Redistribute existing nodes to children
            let mut i = 0;
            while i < self.nodes.len() {
                let existing_node = self.nodes[i];
                let redistributed = self.insert_into_children(existing_node);
                if redistributed {
                    self.nodes.swap_remove(i);
                } else {
                    i += 1;
                }
            }
        }

        // Insert the new node into the appropriate child
        self.insert_into_children(node)
    }

    /// Subdivide this node into 8 children.
    fn subdivide(&mut self) {
        let child_bounds = self.bounds.subdivide();
        let child_depth = self.depth + 1;

        let children = [
            Box::new(OctreeNode::new(child_bounds[0], child_depth)),
            Box::new(OctreeNode::new(child_bounds[1], child_depth)),
            Box::new(OctreeNode::new(child_bounds[2], child_depth)),
            Box::new(OctreeNode::new(child_bounds[3], child_depth)),
            Box::new(OctreeNode::new(child_bounds[4], child_depth)),
            Box::new(OctreeNode::new(child_bounds[5], child_depth)),
            Box::new(OctreeNode::new(child_bounds[6], child_depth)),
            Box::new(OctreeNode::new(child_bounds[7], child_depth)),
        ];

        self.children = Some(children);
        self.is_subdivided = true;
    }

    /// Insert a node into the appropriate child octant.
    fn insert_into_children(&mut self, node: NodePoint) -> bool {
        if let Some(ref mut children) = self.children {
            let point = Vec3::new(node.x, node.y, node.z);
            for child in children.iter_mut() {
                if child.bounds.contains(point) {
                    return child.insert(node);
                }
            }
        }
        false
    }

    /// Find all nodes within a spherical query region.
    ///
    /// This efficiently prunes octants that don't intersect with the query sphere.
    pub fn query_sphere(&self, center: Vec3, radius: f32, results: &mut Vec<NodePoint>) {
        let mut stats = OctreeQueryStats::default();
        self.query_sphere_with_stats(center, radius, results, &mut stats);
    }

    /// Find all nodes within a spherical query region while collecting traversal stats.
    pub fn query_sphere_with_stats(
        &self,
        center: Vec3,
        radius: f32,
        results: &mut Vec<NodePoint>,
        stats: &mut OctreeQueryStats,
    ) {
        stats.nodes_visited += 1;

        // Quick rejection: if this octant doesn't intersect the query sphere, skip it
        if !self.intersects_sphere(center, radius) {
            return;
        }

        // If this is a leaf, check all nodes directly
        if self.is_leaf() {
            stats.leaf_nodes_scanned += 1;
            for node in &self.nodes {
                stats.points_tested += 1;
                let node_pos = Vec3::new(node.x, node.y, node.z);
                let distance_sq = (node_pos - center).length_squared();
                if distance_sq <= radius * radius {
                    results.push(*node);
                    stats.points_returned += 1;
                }
            }
        } else {
            // Recursively check children
            if let Some(ref children) = self.children {
                for child in children {
                    child.query_sphere_with_stats(center, radius, results, stats);
                }
            }
        }
    }

    /// Check if this octant intersects with a spherical query region.
    fn intersects_sphere(&self, center: Vec3, radius: f32) -> bool {
        // Find the closest point in the bounding box to the sphere center
        let closest_x = center.x.clamp(self.bounds.min.x, self.bounds.max.x);
        let closest_y = center.y.clamp(self.bounds.min.y, self.bounds.max.y);
        let closest_z = center.z.clamp(self.bounds.min.z, self.bounds.max.z);
        let closest_point = Vec3::new(closest_x, closest_y, closest_z);

        // Check if the closest point is within the sphere
        let distance_sq = (closest_point - center).length_squared();
        distance_sq <= radius * radius
    }

    /// Get the total number of nodes in this subtree.
    pub fn node_count(&self) -> usize {
        let mut count = self.nodes.len();
        if let Some(ref children) = self.children {
            for child in children {
                count += child.node_count();
            }
        }
        count
    }

    /// Get the total number of leaf nodes in this subtree.
    pub fn leaf_count(&self) -> usize {
        if self.is_leaf() {
            1
        } else {
            let mut count = 0;
            if let Some(ref children) = self.children {
                for child in children {
                    count += child.leaf_count();
                }
            }
            count
        }
    }

    /// Calculate the maximum depth of this subtree.
    pub fn max_depth(&self) -> u32 {
        if self.is_leaf() {
            self.depth
        } else {
            let mut max_child_depth = self.depth;
            if let Some(ref children) = self.children {
                for child in children {
                    max_child_depth = max_child_depth.max(child.max_depth());
                }
            }
            max_child_depth
        }
    }
}

/// An octree spatial index for 3D point data.
///
/// Octrees provide efficient spatial partitioning by recursively subdividing
/// 3D space into 8 octants. This makes them particularly well-suited for
/// uniform 3D data distributions and enables massive pruning during spatial queries.
#[derive(Debug, Clone)]
pub struct Octree {
    /// The root node of the octree.
    root: OctreeNode,

    /// The total number of nodes in the octree.
    node_count: usize,
}

impl Octree {
    /// Create a new octree with the given bounds.
    pub fn new(bounds: BoundingBox) -> Self {
        Self {
            root: OctreeNode::new(bounds, 0),
            node_count: 0,
        }
    }

    /// Serialize the octree to bytes.
    ///
    /// This enables persistence by converting the octree structure to a byte array
    /// that can be saved to disk and later restored.
    pub fn to_bytes(&self) -> Result<Vec<u8>, Box<dyn std::error::Error>> {
        // Collect all nodes from the octree
        let mut nodes = Vec::with_capacity(self.node_count);
        self.collect_nodes(&self.root, &mut nodes);

        // Simple binary format: [bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z, node_count, nodes...]
        let mut bytes = Vec::new();

        // Serialize bounds
        bytes.extend_from_slice(&self.root.bounds.min.x.to_le_bytes());
        bytes.extend_from_slice(&self.root.bounds.min.y.to_le_bytes());
        bytes.extend_from_slice(&self.root.bounds.min.z.to_le_bytes());
        bytes.extend_from_slice(&self.root.bounds.max.x.to_le_bytes());
        bytes.extend_from_slice(&self.root.bounds.max.y.to_le_bytes());
        bytes.extend_from_slice(&self.root.bounds.max.z.to_le_bytes());

        // Serialize node count
        bytes.extend_from_slice(&nodes.len().to_le_bytes());

        // Serialize each node
        for node in nodes {
            bytes.extend_from_slice(&node.id.to_le_bytes());
            bytes.extend_from_slice(&node.x.to_le_bytes());
            bytes.extend_from_slice(&node.y.to_le_bytes());
            bytes.extend_from_slice(&node.z.to_le_bytes());
        }

        Ok(bytes)
    }

    /// Deserialize an octree from bytes.
    ///
    /// Restores an octree that was previously serialized with `to_bytes`.
    pub fn from_bytes(bytes: &[u8]) -> Result<Self, Box<dyn std::error::Error>> {
        use std::convert::TryInto;

        let mut offset = 0;

        // Deserialize bounds
        let min_x = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
        offset += 4;
        let min_y = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
        offset += 4;
        let min_z = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
        offset += 4;
        let max_x = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
        offset += 4;
        let max_y = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
        offset += 4;
        let max_z = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
        offset += 4;

        let bounds = BoundingBox::new(
            Vec3::new(min_x, min_y, min_z),
            Vec3::new(max_x, max_y, max_z),
        );

        // Deserialize node count
        let node_count = usize::from_le_bytes(bytes[offset..offset + 8].try_into()?);
        offset += 8;

        // Deserialize nodes
        let mut nodes = Vec::with_capacity(node_count);
        for _ in 0..node_count {
            let id = u64::from_le_bytes(bytes[offset..offset + 8].try_into()?);
            offset += 8;
            let x = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
            offset += 4;
            let y = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
            offset += 4;
            let z = f32::from_le_bytes(bytes[offset..offset + 4].try_into()?);
            offset += 4;
            nodes.push(NodePoint { id, x, y, z });
        }

        // Rebuild the octree
        let mut octree = Self::new(bounds);
        for node in nodes {
            octree.insert(node);
        }

        Ok(octree)
    }

    /// Recursively collect all nodes from the octree.
    fn collect_nodes(&self, node: &OctreeNode, output: &mut Vec<NodePoint>) {
        // Add nodes from this octant
        output.extend(&node.nodes);

        // Recursively collect from children
        if let Some(ref children) = node.children {
            for child in children.iter() {
                self.collect_nodes(child, output);
            }
        }
    }

    /// Create a new octree that encompasses all the given nodes.
    ///
    /// This automatically calculates appropriate bounds to contain all nodes
    /// with some padding.
    pub fn from_nodes(nodes: &[NodePoint]) -> Self {
        if nodes.is_empty() {
            return Self::new(BoundingBox::new(Vec3::ZERO, Vec3::ONE));
        }

        // Calculate bounds that encompass all nodes with padding
        let mut min = Vec3::new(nodes[0].x, nodes[0].y, nodes[0].z);
        let mut max = min;

        for node in nodes {
            let pos = Vec3::new(node.x, node.y, node.z);
            min = min.min(pos);
            max = max.max(pos);
        }

        // Add 10% padding
        let size = max - min;
        let padding = size * 0.1;
        let bounds = BoundingBox::new(min - padding, max + padding);

        let mut octree = Self::new(bounds);
        for &node in nodes {
            octree.insert(node);
        }
        octree
    }

    /// Insert a node into the octree.
    pub fn insert(&mut self, node: NodePoint) -> bool {
        if self.root.insert(node) {
            self.node_count += 1;
            true
        } else {
            false
        }
    }

    /// Find all nodes within a spherical query region.
    ///
    /// This efficiently prunes octants that don't intersect with the query sphere,
    /// providing massive performance improvements over brute-force scanning.
    pub fn query_sphere(&self, center: Vec3, radius: f32) -> Vec<NodePoint> {
        let (results, _stats) = self.query_sphere_with_stats(center, radius);
        results
    }

    /// Query nodes and return both results and traversal metrics.
    pub fn query_sphere_with_stats(
        &self,
        center: Vec3,
        radius: f32,
    ) -> (Vec<NodePoint>, OctreeQueryStats) {
        let mut results = Vec::new();
        let mut stats = OctreeQueryStats::default();
        self.root
            .query_sphere_with_stats(center, radius, &mut results, &mut stats);
        (results, stats)
    }

    /// Find all nodes within a distance from a given point.
    pub fn locate_within_distance(
        &self,
        center: NodePoint,
        distance_squared: f32,
    ) -> impl Iterator<Item = NodePoint> {
        let center_vec = Vec3::new(center.x, center.y, center.z);
        let radius = distance_squared.sqrt();
        self.query_sphere(center_vec, radius).into_iter()
    }

    /// Find all nodes within an axis-aligned bounding box.
    ///
    /// This is useful for rectangular region queries.
    pub fn query_aabb(&self, bounds: &BoundingBox) -> Vec<NodePoint> {
        // For simplicity, we'll use the sphere query implementation and then filter
        // In a production implementation, we'd have a dedicated AABB query method
        let center = bounds.center();
        let size = bounds.size();
        let radius = size.length() * 0.5; // Conservative estimate

        let candidates = self.query_sphere(center, radius);
        candidates
            .into_iter()
            .filter(|node| {
                let pos = Vec3::new(node.x, node.y, node.z);
                bounds.contains(pos)
            })
            .collect()
    }

    /// Find the k nearest neighbors to a query point.
    ///
    /// Returns up to k nearest nodes sorted by ascending distance.
    /// If fewer than k nodes exist in the octree, returns all nodes.
    ///
    /// # Arguments
    /// * `center` - The query center point
    /// * `k` - Maximum number of neighbors to return
    ///
    /// # Returns
    /// Vec of (NodePoint, distance_squared) sorted by distance
    pub fn query_knn(&self, center: Vec3, k: usize) -> Vec<(NodePoint, f32)> {
        if k == 0 || self.is_empty() {
            return Vec::new();
        }

        // Start with a conservative radius estimate based on octree bounds
        let bounds_size = self.root.bounds.size();
        let max_radius = bounds_size.length();

        // Initial query with large radius to get candidates
        let candidates = self.query_sphere(center, max_radius);

        if candidates.is_empty() {
            return Vec::new();
        }

        // Compute distances and sort
        let mut with_distances: Vec<(NodePoint, f32)> = candidates
            .into_iter()
            .map(|node| {
                let node_pos = Vec3::new(node.x, node.y, node.z);
                let dist_sq = (node_pos - center).length_squared();
                (node, dist_sq)
            })
            .collect();

        // Sort by distance
        with_distances.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal));

        // Truncate to k results
        with_distances.truncate(k);
        with_distances
    }

    /// Find the k nearest neighbors with stats.
    ///
    /// Returns the k nearest nodes along with query statistics.
    pub fn query_knn_with_stats(
        &self,
        center: Vec3,
        k: usize,
    ) -> (Vec<(NodePoint, f32)>, OctreeQueryStats) {
        let mut stats = OctreeQueryStats::default();

        if k == 0 || self.is_empty() {
            return (Vec::new(), stats);
        }

        // Use expanding search: start with small radius, grow until we have k results
        let mut radius = 1.0f32;
        let max_radius = self.root.bounds.size().length();
        let mut results = Vec::new();

        while radius <= max_radius && results.len() < k {
            let mut batch = Vec::new();
            self.root
                .query_sphere_with_stats(center, radius, &mut batch, &mut stats);

            // Compute distances for new candidates
            for node in batch {
                let node_pos = Vec3::new(node.x, node.y, node.z);
                let dist_sq = (node_pos - center).length_squared();
                results.push((node, dist_sq));
            }

            // Remove duplicates (a node might be found in multiple radius expansions)
            results.sort_by_key(|result| result.0.id);
            results.dedup_by(|a, b| a.0.id == b.0.id);

            radius *= 2.0;
        }

        // Final sort by distance and truncate
        results.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal));
        results.truncate(k);

        (results, stats)
    }

    /// Get the bounding box of the entire octree.
    pub fn bounds(&self) -> &BoundingBox {
        &self.root.bounds
    }

    /// Get the total number of nodes in the octree.
    pub fn len(&self) -> usize {
        self.node_count
    }

    /// Check if the octree is empty.
    pub fn is_empty(&self) -> bool {
        self.node_count == 0
    }

    /// Get statistics about the octree structure.
    pub fn statistics(&self) -> OctreeStats {
        OctreeStats {
            node_count: self.node_count,
            leaf_count: self.root.leaf_count(),
            max_depth: self.root.max_depth(),
        }
    }
}

/// Statistics about an octree's structure.
#[derive(Debug, Clone, Copy)]
pub struct OctreeStats {
    /// Total number of nodes in the octree.
    pub node_count: usize,

    /// Total number of leaf nodes in the octree.
    pub leaf_count: usize,

    /// Maximum depth of any node in the octree.
    pub max_depth: u32,
}

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

    #[test]
    fn test_bounding_box_creation() {
        let min = Vec3::new(-1.0, -2.0, -3.0);
        let max = Vec3::new(1.0, 2.0, 3.0);
        let bbox = BoundingBox::new(min, max);

        assert_eq!(bbox.min, min);
        assert_eq!(bbox.max, max);
    }

    #[test]
    fn test_bounding_box_contains() {
        let bbox = BoundingBox::new(Vec3::new(-1.0, -1.0, -1.0), Vec3::new(1.0, 1.0, 1.0));

        // Points inside
        assert!(bbox.contains(Vec3::ZERO));
        assert!(bbox.contains(Vec3::new(0.5, 0.5, 0.5)));
        assert!(bbox.contains(Vec3::new(-1.0, -1.0, -1.0))); // On boundary
        assert!(bbox.contains(Vec3::new(1.0, 1.0, 1.0))); // On boundary

        // Points outside
        assert!(!bbox.contains(Vec3::new(2.0, 0.0, 0.0)));
        assert!(!bbox.contains(Vec3::new(0.0, 2.0, 0.0)));
        assert!(!bbox.contains(Vec3::new(0.0, 0.0, 2.0)));
    }

    #[test]
    fn test_bounding_box_intersects() {
        let bbox1 = BoundingBox::new(Vec3::new(-1.0, -1.0, -1.0), Vec3::new(1.0, 1.0, 1.0));

        let bbox2 = BoundingBox::new(Vec3::new(0.5, 0.5, 0.5), Vec3::new(2.0, 2.0, 2.0));

        let bbox3 = BoundingBox::new(Vec3::new(2.0, 2.0, 2.0), Vec3::new(3.0, 3.0, 3.0));

        // Intersecting boxes
        assert!(bbox1.intersects(&bbox2));

        // Non-intersecting boxes
        assert!(!bbox1.intersects(&bbox3));
    }

    #[test]
    fn test_bounding_box_center_and_size() {
        let bbox = BoundingBox::new(Vec3::new(-2.0, -1.0, 0.0), Vec3::new(2.0, 3.0, 4.0));

        assert_eq!(bbox.center(), Vec3::new(0.0, 1.0, 2.0));
        assert_eq!(bbox.size(), Vec3::new(4.0, 4.0, 4.0));
    }

    #[test]
    fn test_bounding_box_subdivide() {
        let bbox = BoundingBox::new(Vec3::new(-2.0, -2.0, -2.0), Vec3::new(2.0, 2.0, 2.0));

        let octants = bbox.subdivide();
        assert_eq!(octants.len(), 8);

        // Check that all octants are within the original bounds
        for octant in &octants {
            assert!(octant.min.x >= bbox.min.x);
            assert!(octant.min.y >= bbox.min.y);
            assert!(octant.min.z >= bbox.min.z);
            assert!(octant.max.x <= bbox.max.x);
            assert!(octant.max.y <= bbox.max.y);
            assert!(octant.max.z <= bbox.max.z);
        }

        // Check that octants don't overlap (except at boundaries)
        for i in 0..8 {
            for j in (i + 1)..8 {
                // They should only intersect at boundaries
                if octants[i].intersects(&octants[j]) {
                    let intersection = BoundingBox::new(
                        octants[i].min.max(octants[j].min),
                        octants[i].max.min(octants[j].max),
                    );
                    // Intersection should have zero volume (be a point, line, or plane)
                    let size = intersection.size();
                    assert!(size.x == 0.0 || size.y == 0.0 || size.z == 0.0);
                }
            }
        }
    }

    #[test]
    fn test_octree_node_creation() {
        let bounds = BoundingBox::new(Vec3::ZERO, Vec3::ONE);
        let node = OctreeNode::new(bounds, 0);

        assert_eq!(node.bounds, bounds);
        assert_eq!(node.depth, 0);
        assert!(node.is_leaf());
        assert!(node.nodes.is_empty());
        assert!(node.children.is_none());
    }

    #[test]
    fn test_octree_node_capacity() {
        let bounds = BoundingBox::new(Vec3::ZERO, Vec3::ONE);
        let node = OctreeNode::new(bounds, 0);

        // Root node should have capacity of 8
        assert_eq!(node.capacity(), 8);

        // Deeper nodes should have higher capacity
        let deep_node = OctreeNode::new(bounds, 5);
        assert_eq!(deep_node.capacity(), 18); // 8 + 5*2 = 18
    }

    #[test]
    fn test_octree_creation() {
        let bounds = BoundingBox::new(Vec3::ZERO, Vec3::ONE);
        let octree = Octree::new(bounds);

        assert_eq!(octree.len(), 0);
        assert!(octree.is_empty());
    }

    #[test]
    fn test_octree_from_nodes() {
        let nodes = vec![
            NodePoint {
                id: 1,
                x: 0.0,
                y: 0.0,
                z: 0.0,
            },
            NodePoint {
                id: 2,
                x: 1.0,
                y: 1.0,
                z: 1.0,
            },
            NodePoint {
                id: 3,
                x: -1.0,
                y: -1.0,
                z: -1.0,
            },
        ];

        let octree = Octree::from_nodes(&nodes);

        assert_eq!(octree.len(), 3);
        assert!(!octree.is_empty());
    }

    #[test]
    fn test_octree_insert_and_query() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-10.0, -10.0, -10.0),
            Vec3::new(10.0, 10.0, 10.0),
        ));

        // Insert some nodes
        let node1 = NodePoint {
            id: 1,
            x: 1.0,
            y: 1.0,
            z: 1.0,
        };
        let node2 = NodePoint {
            id: 2,
            x: -2.0,
            y: -2.0,
            z: -2.0,
        };
        let node3 = NodePoint {
            id: 3,
            x: 5.0,
            y: 5.0,
            z: 5.0,
        };

        assert!(octree.insert(node1));
        assert!(octree.insert(node2));
        assert!(octree.insert(node3));

        assert_eq!(octree.len(), 3);

        // Query nodes in a sphere around the origin
        let results = octree.query_sphere(Vec3::ZERO, 3.0);
        println!("Results in radius 3.0: {:?}", results);
        // node1 (1,1,1) distance = sqrt(3) ≈ 1.73 < 3.0 ✓
        // node2 (-2,-2,-2) distance = sqrt(12) ≈ 3.46 > 3.0 ✗
        // For now, let's just check that we get the right count
        assert_eq!(results.len(), 1); // Only node1 should be within radius 3.0

        // Query nodes in a larger sphere
        let results = octree.query_sphere(Vec3::ZERO, 10.0);
        println!("Results in radius 10.0: {:?}", results);
        assert_eq!(results.len(), 3); // all nodes should be within radius 10
    }

    #[test]
    fn test_octree_out_of_bounds() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-10.0, -10.0, -10.0),
            Vec3::new(10.0, 10.0, 10.0),
        ));

        // Try to insert a node outside bounds
        let out_of_bounds_node = NodePoint {
            id: 1,
            x: 15.0,
            y: 15.0,
            z: 15.0,
        };
        assert!(!octree.insert(out_of_bounds_node));

        // Verify octree is still empty
        assert_eq!(octree.len(), 0);

        // Insert a node within bounds
        let in_bounds_node = NodePoint {
            id: 2,
            x: 5.0,
            y: 5.0,
            z: 5.0,
        };
        assert!(octree.insert(in_bounds_node));

        // Verify octree now has one node
        assert_eq!(octree.len(), 1);
    }

    #[test]
    fn test_octree_query_stats_collects_lookup_counts() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-100.0, -100.0, -100.0),
            Vec3::new(100.0, 100.0, 100.0),
        ));

        for i in 0..32 {
            let x = (i as f32 - 16.0) * 3.0;
            assert!(octree.insert(NodePoint {
                id: i as u64,
                x,
                y: 0.0,
                z: 0.0,
            }));
        }

        let (results, stats) = octree.query_sphere_with_stats(Vec3::ZERO, 12.0);

        assert!(!results.is_empty());
        assert!(stats.nodes_visited > 0);
        assert!(stats.leaf_nodes_scanned > 0);
        assert!(stats.points_tested >= results.len());
        assert_eq!(stats.points_returned, results.len());
    }

    #[test]
    fn test_octree_lookup_growth_is_sublinear_for_8x_more_points() {
        fn populate_axis_grid(octree: &mut Octree, axis: usize) {
            let mut id = 0u64;
            let span = 400.0f32;
            let step = span / axis as f32;
            let offset = -span * 0.5;
            for xi in 0..axis {
                for yi in 0..axis {
                    for zi in 0..axis {
                        let x = offset + xi as f32 * step;
                        let y = offset + yi as f32 * step;
                        let z = offset + zi as f32 * step;
                        let inserted = octree.insert(NodePoint { id, x, y, z });
                        assert!(inserted);
                        id += 1;
                    }
                }
            }
        }

        let bounds = BoundingBox::new(
            Vec3::new(-256.0, -256.0, -256.0),
            Vec3::new(256.0, 256.0, 256.0),
        );

        let mut small = Octree::new(bounds);
        populate_axis_grid(&mut small, 8); // 512 points
        let (_, small_stats) = small.query_sphere_with_stats(Vec3::new(0.0, 0.0, 0.0), 20.0);

        let mut large = Octree::new(bounds);
        populate_axis_grid(&mut large, 16); // 4096 points (8x)
        let (_, large_stats) = large.query_sphere_with_stats(Vec3::new(0.0, 0.0, 0.0), 20.0);

        assert!(small_stats.nodes_visited > 0);
        assert!(large_stats.nodes_visited > 0);
        assert!(
            large_stats.nodes_visited <= small_stats.nodes_visited * 5,
            "lookup growth too steep: small={} large={}",
            small_stats.nodes_visited,
            large_stats.nodes_visited
        );
    }

    /// Phase D Layer 1 Test: Octree serialization/deserialization
    ///
    /// Tests that an octree can be saved to bytes and restored.
    /// This is the foundation for persistent spatial indexing.
    #[test]
    fn test_octree_serialization_basic() {
        // Create an octree with some nodes
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-100.0, -100.0, -100.0),
            Vec3::new(100.0, 100.0, 100.0),
        ));

        let nodes = vec![
            NodePoint {
                id: 1,
                x: 10.0,
                y: 20.0,
                z: 30.0,
            },
            NodePoint {
                id: 2,
                x: -15.0,
                y: 25.0,
                z: -35.0,
            },
            NodePoint {
                id: 3,
                x: 50.0,
                y: 50.0,
                z: 50.0,
            },
        ];

        for node in &nodes {
            assert!(octree.insert(*node), "Should insert node {}", node.id);
        }

        assert_eq!(octree.len(), 3, "Should have 3 nodes");

        // Serialize the octree
        let serialized = octree.to_bytes().expect("Should serialize octree");
        assert!(
            !serialized.is_empty(),
            "Serialized data should not be empty"
        );

        // Deserialize the octree
        let restored = Octree::from_bytes(&serialized).expect("Should deserialize octree");

        // Verify restored octree has same nodes
        assert_eq!(restored.len(), 3, "Restored octree should have 3 nodes");

        // Verify queries work on restored octree
        let query_results = restored.query_sphere(Vec3::new(10.0, 20.0, 30.0), 5.0);
        assert!(
            query_results.iter().any(|n| n.id == 1),
            "Restored octree should find node 1"
        );
    }

    /// Phase D Layer 2 Test: Octree persistence roundtrip
    ///
    /// Tests that an octree survives a save/load cycle with complex structure.
    #[test]
    fn test_octree_persistence_roundtrip() {
        // Create octree with enough nodes to trigger subdivision
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-500.0, -500.0, -500.0),
            Vec3::new(500.0, 500.0, 500.0),
        ));

        // Insert nodes across different octants to create tree structure
        for i in 0..100 {
            let x = ((i % 10) as f32 - 5.0) * 50.0;
            let y = ((i / 10) as f32 - 5.0) * 50.0;
            let z = 0.0;
            octree.insert(NodePoint {
                id: i as u64,
                x,
                y,
                z,
            });
        }

        assert_eq!(octree.len(), 100, "Should have 100 nodes");

        // Query before serialization
        let original_results = octree.query_sphere(Vec3::ZERO, 100.0);
        let original_count = original_results.len();

        // Serialize and deserialize
        let bytes = octree.to_bytes().expect("Should serialize");
        let restored = Octree::from_bytes(&bytes).expect("Should deserialize");

        assert_eq!(restored.len(), 100, "Restored octree should have 100 nodes");

        // Query after serialization - should get same results
        let restored_results = restored.query_sphere(Vec3::ZERO, 100.0);
        assert_eq!(
            restored_results.len(),
            original_count,
            "Queries should return same count after roundtrip"
        );
    }

    /// Phase D Layer 3 Test: Octree persistence with CfgStore integration
    ///
    /// Tests that octree can be saved and loaded within a CfgStore context.
    #[test]
    fn test_octree_cfg_store_integration() {
        use crate::cfg_store::{CfgBlock, CfgStore};
        use tempfile::tempdir;

        let temp_dir = tempdir().unwrap();
        let db_path = temp_dir.path().join("test_octree_integration.db");

        // Create CfgStore and insert blocks
        let mut store = CfgStore::create(&db_path).unwrap();

        let blocks = vec![
            CfgBlock {
                id: 1,
                function_id: 42,
                block_kind: "entry".to_string(),
                terminator: "fallthrough".to_string(),
                byte_start: 0,
                byte_end: 50,
                start_line: 1,
                start_col: 0,
                end_line: 5,
                end_col: 10,
                dominator_depth: 0,
                loop_nesting: 0,
                branch_count: 0,
            },
            CfgBlock {
                id: 2,
                function_id: 42,
                block_kind: "if".to_string(),
                terminator: "conditional".to_string(),
                byte_start: 50,
                byte_end: 100,
                start_line: 6,
                start_col: 4,
                end_line: 10,
                end_col: 15,
                dominator_depth: 1,
                loop_nesting: 0,
                branch_count: 1,
            },
        ];

        for block in &blocks {
            store.insert_block(block.clone()).unwrap();
        }

        // Query before persistence
        let nearby_before = store.query_nearby(Vec3::new(0.0, 0.0, 42.0), 5.0);
        assert!(
            !nearby_before.is_empty(),
            "Should find blocks before persistence"
        );

        // Serialize octree
        let octree_bytes = store.octree.to_bytes().expect("Should serialize octree");

        // Restore octree
        let restored_octree = Octree::from_bytes(&octree_bytes).expect("Should deserialize octree");

        // Verify restored octree works
        let nearby_after = restored_octree.query_sphere(Vec3::new(0.0, 0.0, 42.0), 5.0);
        assert!(!nearby_after.is_empty(), "Should find blocks after restore");
    }

    /// Phase D Layer 4 Test: Telemetry for octree operations
    ///
    /// Verifies loop guards prevent excessive recursion in octree operations.
    #[test]
    #[cfg(feature = "telemetry")]
    fn test_octree_telemetry_loop_guard() {
        use crate::telemetry::LoopGuard;

        // Test that loop guard catches excessive iterations
        let guard = LoopGuard::new("octree_traversal", 100);

        // Simulate octree traversal
        for i in 0..150 {
            if i < 100 {
                assert!(guard.check().is_ok(), "Should allow iteration {}", i);
            } else {
                assert!(guard.check().is_err(), "Should fail at iteration {}", i);
                break;
            }
        }
    }

    // KNN Query Tests - Ported from geographdb_prototype
    #[test]
    fn test_knn_basic() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-100.0, -100.0, -100.0),
            Vec3::new(100.0, 100.0, 100.0),
        ));

        // Insert nodes at known positions
        let nodes = vec![
            NodePoint {
                id: 1,
                x: 0.0,
                y: 0.0,
                z: 0.0,
            }, // Distance 0
            NodePoint {
                id: 2,
                x: 1.0,
                y: 0.0,
                z: 0.0,
            }, // Distance 1
            NodePoint {
                id: 3,
                x: 3.0,
                y: 0.0,
                z: 0.0,
            }, // Distance 9
            NodePoint {
                id: 4,
                x: 10.0,
                y: 0.0,
                z: 0.0,
            }, // Distance 100
        ];

        for node in &nodes {
            octree.insert(*node);
        }

        // Query for 2 nearest neighbors
        let results = octree.query_knn(Vec3::new(0.0, 0.0, 0.0), 2);

        assert_eq!(results.len(), 2);
        assert_eq!(results[0].0.id, 1); // Nearest (distance 0)
        assert_eq!(results[1].0.id, 2); // Second nearest (distance 1)
        assert!((results[0].1 - 0.0).abs() < 0.001);
        assert!((results[1].1 - 1.0).abs() < 0.001);
    }

    #[test]
    fn test_knn_empty_octree() {
        let octree = Octree::new(BoundingBox::new(
            Vec3::new(-10.0, -10.0, -10.0),
            Vec3::new(10.0, 10.0, 10.0),
        ));

        let results = octree.query_knn(Vec3::ZERO, 5);
        assert!(results.is_empty());
    }

    #[test]
    fn test_knn_k_larger_than_count() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-100.0, -100.0, -100.0),
            Vec3::new(100.0, 100.0, 100.0),
        ));

        // Insert only 3 nodes
        for i in 0..3 {
            octree.insert(NodePoint {
                id: i as u64,
                x: i as f32 * 10.0,
                y: 0.0,
                z: 0.0,
            });
        }

        // Ask for 10 neighbors
        let results = octree.query_knn(Vec3::ZERO, 10);
        assert_eq!(results.len(), 3); // Returns all available
    }

    #[test]
    fn test_knn_k_zero() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-10.0, -10.0, -10.0),
            Vec3::new(10.0, 10.0, 10.0),
        ));
        octree.insert(NodePoint {
            id: 1,
            x: 0.0,
            y: 0.0,
            z: 0.0,
        });

        let results = octree.query_knn(Vec3::ZERO, 0);
        assert!(results.is_empty());
    }

    #[test]
    fn test_knn_sorted_by_distance() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-100.0, -100.0, -100.0),
            Vec3::new(100.0, 100.0, 100.0),
        ));

        // Insert nodes at random distances
        let positions = [
            (5.0, 0.0, 0.0),  // dist 25
            (1.0, 0.0, 0.0),  // dist 1
            (10.0, 0.0, 0.0), // dist 100
            (2.0, 0.0, 0.0),  // dist 4
        ];

        for (i, (x, y, z)) in positions.iter().enumerate() {
            octree.insert(NodePoint {
                id: i as u64,
                x: *x,
                y: *y,
                z: *z,
            });
        }

        let results = octree.query_knn(Vec3::ZERO, 4);

        // Verify sorted by ascending distance
        for i in 1..results.len() {
            assert!(
                results[i].1 >= results[i - 1].1,
                "Results should be sorted by distance"
            );
        }

        // Verify correct order
        assert_eq!(results[0].0.id, 1); // dist 1
        assert_eq!(results[1].0.id, 3); // dist 4
        assert_eq!(results[2].0.id, 0); // dist 25
        assert_eq!(results[3].0.id, 2); // dist 100
    }

    #[test]
    fn test_knn_3d_positions() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-100.0, -100.0, -100.0),
            Vec3::new(100.0, 100.0, 100.0),
        ));

        // Nodes in 3D space
        let nodes = vec![
            NodePoint {
                id: 1,
                x: 1.0,
                y: 1.0,
                z: 1.0,
            }, // dist 3
            NodePoint {
                id: 2,
                x: 2.0,
                y: 2.0,
                z: 2.0,
            }, // dist 12
            NodePoint {
                id: 3,
                x: 0.0,
                y: 0.0,
                z: 0.0,
            }, // dist 0
        ];

        for node in &nodes {
            octree.insert(*node);
        }

        let results = octree.query_knn(Vec3::new(0.0, 0.0, 0.0), 3);

        assert_eq!(results.len(), 3);
        assert_eq!(results[0].0.id, 3); // Origin
        assert_eq!(results[1].0.id, 1); // (1,1,1)
        assert_eq!(results[2].0.id, 2); // (2,2,2)

        // Verify distances
        assert!((results[0].1 - 0.0).abs() < 0.001);
        assert!((results[1].1 - 3.0).abs() < 0.001); // 1^2 + 1^2 + 1^2 = 3
        assert!((results[2].1 - 12.0).abs() < 0.001); // 2^2 + 2^2 + 2^2 = 12
    }

    #[test]
    fn test_knn_with_stats() {
        let mut octree = Octree::new(BoundingBox::new(
            Vec3::new(-100.0, -100.0, -100.0),
            Vec3::new(100.0, 100.0, 100.0),
        ));

        for i in 0..100 {
            octree.insert(NodePoint {
                id: i as u64,
                x: (i as f32 - 50.0) * 2.0,
                y: 0.0,
                z: 0.0,
            });
        }

        let (results, stats) = octree.query_knn_with_stats(Vec3::ZERO, 10);

        assert_eq!(results.len(), 10);
        assert!(stats.nodes_visited > 0, "Should track nodes visited");
        assert!(stats.points_tested > 0, "Should track points tested");
    }
}