aabb 0.7.0

//#![warn(unsafe_code)]

//! Hilbert R-tree implementation for i32 coordinates using unsafe memory layout for performance.
//!
//! All unsafe operations are internal implementation details. The public API is safe.
//! Memory is managed in a single buffer with type-punned box structures and indices.
//! Buffer invariants are maintained throughout the tree's lifetime.
//!
//! This implementation supports i32 integer coordinates, providing 50% better memory efficiency
//! compared to the f64 version while maintaining the same API for supported queries.
//! 
//! **Supported Queries** (pure AABB operations, no distance calculations):
//! - `query_intersecting` - Find boxes intersecting a rectangle
//! - `query_intersecting_k` - Find first K intersecting boxes
//! - `query_point` - Find boxes containing a point
//! - `query_contain` - Find boxes that contain a rectangle
//! - `query_contained_within` - Find boxes contained within a rectangle

use std::mem::size_of;
use std::collections::VecDeque;

/// Box structure: minX, minY, maxX, maxY (16 bytes total for i32)
#[derive(Clone, Copy, Debug)]
pub(crate) struct BoxI32 {
    pub(crate) min_x: i32,
    pub(crate) min_y: i32,
    pub(crate) max_x: i32,
    pub(crate) max_y: i32,
}

impl BoxI32 {
    fn new(min_x: i32, min_y: i32, max_x: i32, max_y: i32) -> Self {
        Self { min_x, min_y, max_x, max_y }
    }
}

/// Hilbert R-tree for i32 spatial queries - following flatbush algorithm
///
/// Memory layout (in single buffer):
/// - Header: 8 bytes (magic, version, `node_size`, `num_items`)
/// - All boxes: `num_total_nodes` * 16 bytes (4 i32 per box) - 50% more efficient than f64!
/// - All indices: `num_total_nodes` * 4 bytes (u32 per node)
///
/// Leaf nodes occupy positions [0, `num_items`), parent nodes appended after.
/// Tree is built bottom-up with Hilbert curve ordering for spatial locality.
#[derive(Clone, Debug)]
pub struct HilbertRTreeI32 {
    /// Single buffer: header + boxes + indices
    data: Vec<u8>,
    /// Level boundaries: end position of each tree level
    pub(crate) level_bounds: Vec<usize>,
    /// Node size for tree construction
    pub(crate) node_size: usize,
    /// Number of leaf items
    pub(crate) num_items: usize,
    /// Current position during building
    pub(crate) position: usize,
    /// Bounding box of all items
    pub(crate) bounds: BoxI32,
    /// Total nodes in tree (cached from level_bounds.last())
    total_nodes: usize,
    /// Pre-allocated capacity in bytes (0 if not pre-allocated)
    allocated_capacity: usize,
}

const MAX_HILBERT: u32 = u16::MAX as u32;
const DEFAULT_NODE_SIZE: usize = 16;
const HEADER_SIZE: usize = 8; // bytes

/// Helper: Estimate total nodes in tree given item count
/// For a tree with node_size, total nodes ≈ N + N/node_size + N/node_size^2 + ...
/// This converges to: N * node_size / (node_size - 1)
#[inline]
fn estimate_total_nodes(num_items: usize, node_size: usize) -> usize {
    if num_items == 0 {
        return 0;
    }
    (num_items * node_size) / (node_size - 1) + 1
}

/// Helper: Calculate EXACT total nodes by simulating tree construction (O(log n) - tree depth)
#[inline]
fn calculate_exact_total_nodes(num_items: usize, node_size: usize) -> usize {
    if num_items == 0 {
        return 0;
    }
    let mut total_nodes = num_items;
    let mut count = num_items;
    loop {
        count = count.div_ceil(node_size);
        total_nodes += count;
        if count <= 1 {
            break;
        }
    }
    total_nodes
}

/// Helper: Calculate required buffer size for estimated nodes
#[inline]
fn estimate_buffer_size(num_items: usize, node_size: usize) -> usize {
    let estimated_nodes = estimate_total_nodes(num_items, node_size);
    HEADER_SIZE + estimated_nodes * (size_of::<BoxI32>() + size_of::<u32>())
}

impl HilbertRTreeI32 {
    /// Creates a new empty Hilbert R-tree for i32 coordinates
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let tree = HilbertRTreeI32::new();
    /// assert_eq!(tree.len(), 0);
    /// ```
    pub fn new() -> Self {
        Self::with_capacity(0)
    }

    /// Creates a new Hilbert R-tree with preallocated capacity
    ///
    /// Preallocating capacity can improve performance by avoiding internal reallocations
    /// during the add phase if you know the approximate number of boxes in advance.
    ///
    /// # Arguments
    /// * `capacity` - Initial capacity for the number of boxes to add
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::with_capacity(100);
    /// assert_eq!(tree.len(), 0);
    /// ```
    pub fn with_capacity(capacity: usize) -> Self {
        let data = if capacity > 0 {
            let needed_size = estimate_buffer_size(capacity, DEFAULT_NODE_SIZE);
            Vec::with_capacity(needed_size)
        } else {
            Vec::new()
        };
        
        let allocated_capacity = data.capacity();
        
        Self {
            data,
            allocated_capacity,
            level_bounds: Vec::new(),
            node_size: DEFAULT_NODE_SIZE,
            num_items: 0,
            position: 0,
            bounds: BoxI32::new(i32::MAX, i32::MAX, i32::MIN, i32::MIN),
            total_nodes: 0,
        }
    }

    /// Adds a bounding box to the tree
    ///
    /// Boxes are stored temporarily and reorganized during the `build()` phase.
    /// You must call `build()` before performing any queries.
    ///
    /// # Arguments
    /// * `min_x` - Left edge of the bounding box
    /// * `min_y` - Bottom edge of the bounding box
    /// * `max_x` - Right edge of the bounding box
    /// * `max_y` - Top edge of the bounding box
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::new();
    /// tree.add(0, 0, 10, 10);  // Box 0
    /// tree.add(5, 5, 15, 15);  // Box 1
    /// tree.build();
    /// assert_eq!(tree.len(), 2);
    /// ```
    pub fn add(&mut self, min_x: i32, min_y: i32, max_x: i32, max_y: i32) {
        // Calculate required size for this item
        let required_size = estimate_buffer_size(self.num_items + 1, self.node_size);
        
        // Allocate if needed
        if required_size > self.data.capacity() {
            let new_capacity = (self.data.capacity() * 2).max(required_size);
            self.data.reserve(new_capacity - self.data.capacity());
            self.allocated_capacity = self.data.capacity();
        }
        
        // Ensure len is sufficient for writing at the position we need
        let box_idx = HEADER_SIZE + self.num_items * size_of::<BoxI32>();
        let needed_len = box_idx + size_of::<BoxI32>();
        if needed_len > self.data.len() {
            unsafe {
                self.data.set_len(needed_len);
            }
        }
        
        let box_ptr = &mut self.data[box_idx] as *mut u8 as *mut BoxI32;
        unsafe {
            std::ptr::write_unaligned(box_ptr, BoxI32::new(min_x, min_y, max_x, max_y));
        }

        self.bounds.min_x = self.bounds.min_x.min(min_x);
        self.bounds.min_y = self.bounds.min_y.min(min_y);
        self.bounds.max_x = self.bounds.max_x.max(max_x);
        self.bounds.max_y = self.bounds.max_y.max(max_y);

        self.num_items += 1;
    }

    /// Builds the Hilbert R-tree index
    ///
    /// This method must be called after adding all boxes and before performing any queries.
    /// It organizes the boxes into a hierarchical structure for efficient spatial queries,
    /// sorting them by their Hilbert curve index for improved cache locality.
    ///
    /// # Performance
    /// Building is O(n log n) due to the sorting phase. After building, queries are O(log n)
    /// on average for well-distributed data.
    ///
    /// # Panics
    /// Does not panic if called on an empty tree or if called multiple times.
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::new();
    /// tree.add(0, 0, 10, 10);
    /// tree.add(5, 5, 15, 15);
    /// tree.build();  // Required before querying
    /// ```
    pub fn build(&mut self) {
        if self.num_items == 0 {
            return;
        }

        let num_items = self.num_items;
        let node_size = self.node_size;

        // Calculate exact total nodes needed (O(log n) - only tree depth iterations)
        let total_nodes = calculate_exact_total_nodes(num_items, node_size);
        let data_size = HEADER_SIZE + total_nodes * (size_of::<BoxI32>() + size_of::<u32>());
        
        // Reserve all needed space at once (avoids reallocation during build)
        if data_size > self.data.capacity() {
            self.data.reserve(data_size - self.data.capacity());
            self.allocated_capacity = self.data.capacity();
        }
        
        // Resize to final size (zero-fill is necessary - build reads uninitialized parent positions)
        if self.data.len() < data_size {
            self.data.resize(data_size, 0);
        }

        // Calculate level bounds
        let mut level_bounds = Vec::with_capacity(16); // Max tree depth ~16 for 1M items
        let mut count = num_items;
        let mut level_total_nodes = num_items;
        level_bounds.push(level_total_nodes);

        // Create parent levels until we have a single root
        loop {
            count = count.div_ceil(node_size);
            level_total_nodes += count;
            level_bounds.push(level_total_nodes);
            if count <= 1 {
                break;
            }
        }

        // Resize data buffer to final size
        let data_size = HEADER_SIZE + total_nodes * (size_of::<BoxI32>() + size_of::<u32>());
        if data_size > self.data.capacity() {
            self.data.reserve(data_size - self.data.capacity());
            self.allocated_capacity = self.data.capacity();
        }
        // Ensure len is sufficient for all writes during build
        if self.data.len() < data_size {
            self.data.resize(data_size, 0);
        }

        // Write header
        self.data[0] = 0xfc; // magic (different from f64 variant 0xfb)
        self.data[1] = 0x01; // version 1 (same version for both variants)
        self.data[2..4].copy_from_slice(&(node_size as u16).to_le_bytes());
        self.data[4..8].copy_from_slice(&(num_items as u32).to_le_bytes());

        self.level_bounds = level_bounds;
        self.position = 0;
        self.total_nodes = total_nodes;

        // If all items fit in one node, create a root level
        if num_items <= node_size {
            // Initialize all leaf indices first
            let indices_start = HEADER_SIZE + total_nodes * size_of::<BoxI32>();
            for i in 0..num_items {
                let idx_ptr = &mut self.data[indices_start + i * size_of::<u32>()] as *mut u8 as *mut u32;
                unsafe {
                    std::ptr::write_unaligned(idx_ptr, i as u32);
                }
            }
            
            // Write the root node box at position num_items
            let root_idx = HEADER_SIZE + num_items * size_of::<BoxI32>();
            let root_ptr = &mut self.data[root_idx] as *mut u8 as *mut BoxI32;
            unsafe {
                std::ptr::write_unaligned(root_ptr, self.bounds);
            }
            
            // Write the root node index (pointer to first child at position 0)
            let root_idx_ptr = &mut self.data[indices_start + num_items * size_of::<u32>()] as *mut u8 as *mut u32;
            unsafe {
                std::ptr::write_unaligned(root_idx_ptr, 0_u32 << 2_u32); // First child at position 0
            }
            return;
        }

        // Compute Hilbert values for leaves
        let hilbert_width = if self.bounds.max_x > self.bounds.min_x {
            MAX_HILBERT as f64 / (self.bounds.max_x - self.bounds.min_x) as f64
        } else {
            0.0
        };
        let hilbert_height = if self.bounds.max_y > self.bounds.min_y {
            MAX_HILBERT as f64 / (self.bounds.max_y - self.bounds.min_y) as f64
        } else {
            0.0
        };

        let mut hilbert_values = Vec::with_capacity(num_items);
        for i in 0..num_items {
            let box_data = self.get_box(i);
            let center_x = ((box_data.min_x as f64 + box_data.max_x as f64) / 2.0 - self.bounds.min_x as f64) * hilbert_width;
            let center_y = ((box_data.min_y as f64 + box_data.max_y as f64) / 2.0 - self.bounds.min_y as f64) * hilbert_height;
            let hx = center_x.max(0.0).min(MAX_HILBERT as f64 - 1.0) as u32;
            let hy = center_y.max(0.0).min(MAX_HILBERT as f64 - 1.0) as u32;
            hilbert_values.push(hilbert_xy_to_index(hx, hy));
        }

        // Initialize leaf indices BEFORE sorting
        let indices_start = HEADER_SIZE + total_nodes * size_of::<BoxI32>();
        for i in 0..num_items {
            let idx_ptr = &mut self.data[indices_start + i * size_of::<u32>()] as *mut u8 as *mut u32;
            unsafe {
                std::ptr::write_unaligned(idx_ptr, i as u32);
            }
        }

        // Create an indirection array to track sorting permutations
        // This allows us to use Rust's optimized sort (introsort) instead of custom quicksort
        let mut sort_indices: Vec<usize> = (0..num_items).collect();
        
        // Sort indices by their corresponding Hilbert values
        sort_indices.sort_unstable_by_key(|&i| hilbert_values[i]);
        
        // Apply the permutation to boxes
        let mut temp_data = Vec::with_capacity(num_items * size_of::<BoxI32>());
        unsafe {
            temp_data.set_len(num_items * size_of::<BoxI32>());
        }
        
        for (new_pos, &old_pos) in sort_indices.iter().enumerate() {
            let old_box_idx = HEADER_SIZE + old_pos * size_of::<BoxI32>();
            let new_box_idx = new_pos * size_of::<BoxI32>();
            temp_data[new_box_idx..new_box_idx + size_of::<BoxI32>()]
                .copy_from_slice(&self.data[old_box_idx..old_box_idx + size_of::<BoxI32>()]);
        }
        
        // Copy sorted boxes back to data
        for i in 0..num_items {
            let src_idx = i * size_of::<BoxI32>();
            let dst_idx = HEADER_SIZE + i * size_of::<BoxI32>();
            self.data[dst_idx..dst_idx + size_of::<BoxI32>()]
                .copy_from_slice(&temp_data[src_idx..src_idx + size_of::<BoxI32>()]);
        }
        
        // Apply the same permutation to hilbert_values array to keep it in sync with boxes
        // let mut temp_hilbert = Vec::with_capacity(num_items);
        // for &old_pos in sort_indices.iter() {
        //     temp_hilbert.push(hilbert_values[old_pos]);
        // }
        // Note: We keep temp_hilbert for consistency but don't need it for later operations
        // since the indices array contains the original box IDs

        let indices_start = HEADER_SIZE + total_nodes * size_of::<BoxI32>();
        for i in 0..num_items {
            let idx_ptr = &mut self.data[indices_start + i * size_of::<u32>()] as *mut u8 as *mut u32;
            unsafe {
                // sort_indices[i] tells us which original box is now at position i
                std::ptr::write_unaligned(idx_ptr, sort_indices[i] as u32);
            }
        }
        let mut pos = 0_usize;
        for level_idx in 0..self.level_bounds.len() - 1 {
            let level_end = self.level_bounds[level_idx];
            let mut parent_pos = level_end;

            while pos < level_end {
                let node_index = (pos as u32) << 2_u32; // for JS compatibility
                let mut node_box = self.get_box(pos);

                // Merge up to node_size children
                for _ in 0..node_size {
                    if pos >= level_end {
                        break;
                    }
                    let child_box = self.get_box(pos);
                    node_box.min_x = node_box.min_x.min(child_box.min_x);
                    node_box.min_y = node_box.min_y.min(child_box.min_y);
                    node_box.max_x = node_box.max_x.max(child_box.max_x);
                    node_box.max_y = node_box.max_y.max(child_box.max_y);
                    pos += 1;
                }

                // Write parent node box
                let box_idx = HEADER_SIZE + parent_pos * size_of::<BoxI32>();
                let box_ptr = &mut self.data[box_idx] as *mut u8 as *mut BoxI32;
                unsafe {
                    std::ptr::write_unaligned(box_ptr, node_box);
                }

                // Write parent node index
                let idx_ptr = &mut self.data[indices_start + parent_pos * size_of::<u32>()] as *mut u8 as *mut u32;
                unsafe {
                    std::ptr::write_unaligned(idx_ptr, node_index);
                }

                parent_pos += 1;
            }
            pos = level_end;
        }
    }

    /// Returns the number of items in the tree
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::new();
    /// assert_eq!(tree.len(), 0);
    /// tree.add(0, 0, 10, 10);
    /// assert_eq!(tree.len(), 1);
    /// ```
    pub fn len(&self) -> usize {
        self.num_items
    }

    /// Returns whether the tree is empty
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::new();
    /// assert!(tree.is_empty());
    /// tree.add(0, 0, 10, 10);
    /// assert!(!tree.is_empty());
    /// ```
    pub fn is_empty(&self) -> bool {
        self.num_items == 0
    }

    /// Finds all boxes that intersect with a given rectangular region.
    ///
    /// This query returns all boxes whose bounding boxes overlap with the query rectangle,
    /// including boxes that merely touch at edges or corners. This is useful for broad-phase
    /// collision detection, finding objects in a viewport, or spatial filtering.
    ///
    /// # Arguments
    /// * `min_x` - Left edge of query rectangle
    /// * `min_y` - Bottom edge of query rectangle
    /// * `max_x` - Right edge of query rectangle
    /// * `max_y` - Top edge of query rectangle
    /// * `results` - Output vector; will be cleared and populated with matching box indices
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::with_capacity(3);
    /// tree.add(0, 0, 2, 2);  // Box 0
    /// tree.add(1, 1, 3, 3);  // Box 1
    /// tree.add(4, 4, 5, 5);  // Box 2
    /// tree.build();
    ///
    /// let mut results = Vec::new();
    /// tree.query_intersecting(0, 0, 2, 2, &mut results);
    /// // Results include box 0 and 1 (both intersect the query rectangle)
    /// ```
    pub fn query_intersecting(
        &self,
        min_x: i32,
        min_y: i32,
        max_x: i32,
        max_y: i32,
        results: &mut Vec<usize>,
    ) {
        results.clear();
        if self.num_items == 0 || self.level_bounds.is_empty() {
            return;
        }
        
        // Query area heuristic for early termination decision
        let query_area = (max_x as i64 - min_x as i64) * (max_y as i64 - min_y as i64);
        let bounds_area = (self.bounds.max_x as i64 - self.bounds.min_x as i64)
            * (self.bounds.max_y as i64 - self.bounds.min_y as i64);

        // If query covers >50% of space, full scan is faster than hierarchical traversal
        if query_area > bounds_area / 2 {
            // Fast path: scan all leaf nodes directly
            for pos in 0..self.num_items {
                let node_box = self.get_box(pos);

                if max_x >= node_box.min_x && max_y >= node_box.min_y
                    && min_x <= node_box.max_x && min_y <= node_box.max_y
                {
                    let index = self.get_index(pos);
                    results.push(index as usize);
                }
            }
            return;
        }

        // Slow path: hierarchical traversal with pruning
        let mut queue = VecDeque::new();
        let mut node_index = self.total_nodes - 1;
        
        loop {
            let node_end = self.upper_bound(node_index);
            let end_pos = (node_index + self.node_size).min(node_end);
            
            // Process groups of 2 nodes at a time
            let mut pos = node_index;
            while pos + 2 <= end_pos {
                let boxes = self.get_boxes_batch(pos);
                
                for i in 0..2 {
                    let node_box = boxes[i];
                    if !(max_x < node_box.min_x || max_y < node_box.min_y ||
                       min_x > node_box.max_x || min_y > node_box.max_y)
                    {
                        let current_pos = pos + i;
                        let index = self.get_index(current_pos);
                        if current_pos >= self.num_items {
                            queue.push_back((index >> 2) as usize);
                        } else {
                            results.push(index as usize);
                        }
                    }
                }
                
                pos += 2;
            }
            
            // Process remaining nodes (1) individually
            while pos < end_pos {
                let node_box = self.get_box(pos);
                
                if !(max_x < node_box.min_x || max_y < node_box.min_y ||
                   min_x > node_box.max_x || min_y > node_box.max_y)
                {
                    let index = self.get_index(pos);
                    if pos >= self.num_items {
                        queue.push_back((index >> 2) as usize);
                    } else {
                        results.push(index as usize);
                    }
                }
                pos += 1;
            }
            
            if queue.is_empty() {
                break;
            }
            
            node_index = queue.pop_front().unwrap();
        }
    }

    
    /// Finds all boxes that intersect with a box already in the index.
    ///
    /// This query is useful when you have an item already added to the tree and want to find
    /// all other items that intersect with it, excluding the query item itself. It's more
    /// convenient than calling `query_intersecting()` with manually extracted bounds, and
    /// avoids redundant lookups and self-intersection checks.
    ///
    /// # Arguments
    /// * `item_id` - The index of an item already in the tree (0 to `num_items - 1`)
    /// * `results` - Output vector; will be cleared and populated with matching box indices
    ///              (excluding the query item itself)
    ///
    /// # Errors
    /// Returns an error if `item_id >= num_items` (the item doesn't exist in the tree).
    ///
    /// # Example
    /// ```
    /// use aabb::prelude::*;
    /// let mut tree = AABBI32::with_capacity(3);
    /// tree.add(0, 0, 2, 2);  // Item 0
    /// tree.add(1, 1, 3, 3);  // Item 1
    /// tree.add(4, 4, 5, 5);  // Item 2
    /// tree.build();
    ///
    /// let mut results = Vec::new();
    /// tree.query_intersecting_id(0, &mut results).unwrap();
    /// // Results include item 1 (intersects with item 0), but not item 0 itself
    /// ```
    pub fn query_intersecting_id(
        &self,
        item_id: usize,
        results: &mut Vec<usize>,
    ) -> Result<(), String> {
        if item_id >= self.num_items {
            return Err(format!("item_id {} is out of bounds (tree has {} items)", item_id, self.num_items));
        }
        
        // Get the bounding box of the query item
        let query_box = self.get_box(item_id);
        
        // Use the existing query_intersecting method with the box bounds
        self.query_intersecting(query_box.min_x, query_box.min_y, query_box.max_x, query_box.max_y, results);
        
        // Always exclude the query item itself (no self-intersections)
        results.retain(|&idx| idx != item_id);
        
        Ok(())
    }

    /// Finds the first K intersecting boxes within a rectangular region.
    ///
    /// This query finds boxes intersecting a query rectangle and stops after collecting K results.
    /// Unlike `query_intersecting` which finds all matches, this variant returns early when K
    /// results are found, making it more efficient when only a limited number of results are needed.
    ///
    /// # Arguments
    /// * `min_x` - Left edge of query rectangle
    /// * `min_y` - Bottom edge of query rectangle
    /// * `max_x` - Right edge of query rectangle
    /// * `max_y` - Top edge of query rectangle
    /// * `k` - Maximum number of results to return
    /// * `results` - Output vector; will be cleared and populated with up to K matching box indices
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::with_capacity(5);
    /// tree.add(0, 0, 1, 1);  // Box 0
    /// tree.add(0, 0, 2, 2);  // Box 1
    /// tree.add(1, 1, 2, 2);  // Box 2
    /// tree.add(1, 1, 3, 3);  // Box 3
    /// tree.add(4, 4, 5, 5);  // Box 4
    /// tree.build();
    ///
    /// let mut results = Vec::new();
    /// tree.query_intersecting_k(0, 0, 2, 2, 2, &mut results);
    /// // Results contain at most 2 of the intersecting boxes
    /// ```
    pub fn query_intersecting_k(
        &self,
        min_x: i32,
        min_y: i32,
        max_x: i32,
        max_y: i32,
        k: usize,
        results: &mut Vec<usize>,
    ) {
        if self.num_items == 0 || self.level_bounds.is_empty() || k == 0 {
            results.clear();
            return;
        }

        results.clear();
        
        let mut queue = VecDeque::with_capacity(self.level_bounds.len() * 2);
        let mut node_index = self.total_nodes - 1;
        
        loop {
            if results.len() >= k {
                break;
            }

            let node_end = self.upper_bound(node_index);
            let end_pos = (node_index + self.node_size).min(node_end);
            
            for pos in node_index..end_pos {
                if results.len() >= k {
                    break;
                }

                let node_box = self.get_box(pos);
                
                if max_x < node_box.min_x || max_y < node_box.min_y ||
                   min_x > node_box.max_x || min_y > node_box.max_y {
                    continue;
                }
                
                let index = self.get_index(pos);
                if pos < self.num_items {
                    results.push(index as usize);
                } else {
                    queue.push_back((index >> 2) as usize);
                }
            }
            
            if queue.is_empty() {
                break;
            }
            
            node_index = queue.pop_front().unwrap();
        }
    }

    /// Finds all boxes that contain a specific point.
    ///
    /// This query returns all boxes whose boundaries include the given point.
    /// A point on the edge or corner of a box is considered contained (inclusive test).
    /// This is useful for hit testing, picking objects at a screen location, or
    /// determining which regions contain a specific coordinate.
    ///
    /// # Arguments
    /// * `x` - X coordinate of the query point
    /// * `y` - Y coordinate of the query point
    /// * `results` - Output vector; will be cleared and populated with indices of all
    ///              boxes that contain the point
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::with_capacity(2);
    /// tree.add(0, 0, 2, 2);  // Box 0
    /// tree.add(1, 1, 3, 3);  // Box 1
    /// tree.build();
    ///
    /// let mut results = Vec::new();
    /// tree.query_point(1, 1, &mut results);
    /// // Results contain both box 0 and box 1 (point is inside both)
    /// ```
    pub fn query_point(&self, x: i32, y: i32, results: &mut Vec<usize>) {
        results.clear();
        if self.num_items == 0 || self.level_bounds.is_empty() {
            return;
        }
        
        let mut queue = VecDeque::new();
        let mut node_index = self.total_nodes - 1;
        
        loop {
            let node_end = self.upper_bound(node_index);
            let end_pos = (node_index + self.node_size).min(node_end);
            
            for pos in node_index..end_pos {
                let node_box = self.get_box(pos);
                
                // Check if point is inside box
                if x < node_box.min_x || x > node_box.max_x ||
                   y < node_box.min_y || y > node_box.max_y {
                    continue;
                }
                
                let index = self.get_index(pos);
                if pos >= self.num_items {
                    queue.push_back((index >> 2) as usize);
                } else {
                    results.push(index as usize);
                }
            }
            
            if queue.is_empty() {
                break;
            }
            
            node_index = queue.pop_front().unwrap();
        }
    }

    /// Finds all boxes that completely contain a given rectangular region.
    ///
    /// This query returns all boxes whose boundaries fully enclose the query rectangle.
    /// The query rectangle must be fully contained within each result box for inclusion.
    /// This is useful for finding container regions, parent regions, or areas that
    /// fully cover a given space.
    ///
    /// # Arguments
    /// * `min_x` - Left edge of query rectangle
    /// * `min_y` - Bottom edge of query rectangle
    /// * `max_x` - Right edge of query rectangle
    /// * `max_y` - Top edge of query rectangle
    /// * `results` - Output vector; will be cleared and populated with indices of boxes
    ///              that completely contain the query rectangle
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::with_capacity(3);
    /// tree.add(0, 0, 5, 5);  // Box 0 (large)
    /// tree.add(1, 1, 4, 4);  // Box 1 (medium)
    /// tree.add(6, 6, 8, 8);  // Box 2 (separate)
    /// tree.build();
    ///
    /// let mut results = Vec::new();
    /// tree.query_contain(1, 1, 3, 3, &mut results);
    /// // Results contain box 0 and box 1 (both contain the query rectangle)
    /// ```
    pub fn query_contain(&self, min_x: i32, min_y: i32, max_x: i32, max_y: i32, results: &mut Vec<usize>) {
        results.clear();
        if self.num_items == 0 || self.level_bounds.is_empty() {
            return;
        }
        
        let mut queue = VecDeque::new();
        let mut node_index = self.total_nodes - 1;
        
        loop {
            let node_end = self.upper_bound(node_index);
            let end_pos = (node_index + self.node_size).min(node_end);
            
            for pos in node_index..end_pos {
                let node_box = self.get_box(pos);
                
                // Check if node contains the query rectangle
                if node_box.min_x <= min_x && node_box.max_x >= max_x &&
                   node_box.min_y <= min_y && node_box.max_y >= max_y {
                    
                    let index = self.get_index(pos);
                    if pos >= self.num_items {
                        queue.push_back((index >> 2) as usize);
                    } else {
                        results.push(index as usize);
                    }
                }
            }
            
            if queue.is_empty() {
                break;
            }
            
            node_index = queue.pop_front().unwrap();
        }
    }

    /// Finds all boxes that are completely contained within a given rectangle.
    ///
    /// This query returns all boxes that fit entirely within the query rectangle's boundaries.
    /// Each result box must be fully contained for inclusion. This is the opposite of
    /// `query_contain` and is useful for finding items within a region, filtering
    /// objects by area, or identifying sub-regions.
    ///
    /// # Arguments
    /// * `min_x` - Left edge of query rectangle
    /// * `min_y` - Bottom edge of query rectangle
    /// * `max_x` - Right edge of query rectangle
    /// * `max_y` - Top edge of query rectangle
    /// * `results` - Output vector; will be cleared and populated with indices of boxes
    ///              that are completely contained within the query rectangle
    ///
    /// # Example
    /// ```
    /// use aabb::HilbertRTreeI32;
    /// let mut tree = HilbertRTreeI32::with_capacity(3);
    /// tree.add(0, 0, 5, 5);  // Box 0 (large)
    /// tree.add(1, 1, 2, 2);  // Box 1 (small, inside)
    /// tree.add(6, 6, 8, 8);  // Box 2 (outside)
    /// tree.build();
    ///
    /// let mut results = Vec::new();
    /// tree.query_contained_within(0, 0, 4, 4, &mut results);
    /// // Results contain only box 1 (box 0 is too large, box 2 is outside)
    /// ```
    pub fn query_contained_within(&self, min_x: i32, min_y: i32, max_x: i32, max_y: i32, results: &mut Vec<usize>) {
        results.clear();
        if self.num_items == 0 || self.level_bounds.is_empty() {
            return;
        }
        
        let mut queue = VecDeque::new();
        let mut node_index = self.total_nodes - 1;
        
        loop {
            let node_end = self.upper_bound(node_index);
            let end_pos = (node_index + self.node_size).min(node_end);
            
            for pos in node_index..end_pos {
                let node_box = self.get_box(pos);
                
                if pos >= self.num_items {
                    // This is a parent node - check if it could have matching children
                    // (any overlap with query region)
                    if node_box.max_x >= min_x && node_box.max_y >= min_y &&
                       node_box.min_x <= max_x && node_box.min_y <= max_y {
                        let index = self.get_index(pos);
                        queue.push_back((index >> 2) as usize);
                    }
                } else {
                    // This is a leaf - check if fully contained
                    if node_box.min_x >= min_x && node_box.max_x <= max_x &&
                       node_box.min_y >= min_y && node_box.max_y <= max_y {
                        let index = self.get_index(pos);
                        results.push(index as usize);
                    }
                }
            }
            
            if queue.is_empty() {
                break;
            }
            
            node_index = queue.pop_front().unwrap();
        }
    }

    // --- Private helpers ---

    /// Get box at position using read_unaligned
    #[inline]
    pub(crate) fn get_box(&self, pos: usize) -> BoxI32 {
        let idx = HEADER_SIZE + pos * size_of::<BoxI32>();
        unsafe {
            std::ptr::read_unaligned(&self.data[idx] as *const u8 as *const BoxI32)
        }
    }

    /// Get 2 boxes at once for batch processing - single read_unaligned call
    #[allow(dead_code)]
    #[inline]
    pub(crate) fn get_boxes_batch(&self, start_pos: usize) -> [BoxI32; 2] {
        let base_idx = HEADER_SIZE + start_pos * size_of::<BoxI32>();
        unsafe {
            std::ptr::read_unaligned(self.data.as_ptr().add(base_idx) as *const [BoxI32; 2])
        }
    }

    /// Get index at position using read_unaligned
    #[inline(always)]
    pub(crate) fn get_index(&self, pos: usize) -> u32 {
        let indices_start = HEADER_SIZE + self.total_nodes * size_of::<BoxI32>();
        unsafe {
            std::ptr::read_unaligned(&self.data[indices_start + pos * size_of::<u32>()] as *const u8 as *const u32)
        }
    }

    /// Find upper bound of a node in `level_bounds`
    #[inline(always)]
    fn upper_bound(&self, node_index: usize) -> usize {
        for &bound in &self.level_bounds {
            if bound > node_index {
                return bound;
            }
        }
        self.total_nodes
    }

    /// to enable fast loading without rebuilding. The file format includes a magic number and version
    /// Saves the built Hilbert R-tree to a file.
    ///
    /// Serializes the complete tree structure including the header, buffer, metadata, and level bounds
    /// to enable fast loading without rebuilding. The file format includes a magic number and version
    /// for integrity checking during load.
    ///
    /// # Arguments
    /// * `path` - File path where the tree will be saved
    ///
    /// # Errors
    /// Returns an error if the file cannot be created or written to.
    pub fn save<P: AsRef<std::path::Path>>(&self, path: P) -> std::io::Result<()> {
        use std::io::Write;
        let mut file = std::fs::File::create(path)?;
        
        // Write magic number and version (file header for validation)
        file.write_all(&[0xfc])?;  // magic (different from f64 variant 0xfb)
        file.write_all(&[0x01])?;  // version 1 (same version for both variants)
        
        // Write node_size
        file.write_all(&(self.node_size as u32).to_le_bytes())?;
        
        // Write num_items
        file.write_all(&(self.num_items as u32).to_le_bytes())?;
        
        // Write total_nodes
        file.write_all(&(self.total_nodes as u32).to_le_bytes())?;
        
        // Write level_bounds length
        file.write_all(&(self.level_bounds.len() as u32).to_le_bytes())?;
        
        // Write level_bounds
        for &bound in &self.level_bounds {
            file.write_all(&(bound as u32).to_le_bytes())?;
        }
        
        // Write bounds
        file.write_all(&self.bounds.min_x.to_le_bytes())?;
        file.write_all(&self.bounds.min_y.to_le_bytes())?;
        file.write_all(&self.bounds.max_x.to_le_bytes())?;
        file.write_all(&self.bounds.max_y.to_le_bytes())?;
        
        // Write data buffer
        file.write_all(&(self.data.len() as u32).to_le_bytes())?;
        file.write_all(&self.data)?;
        
        Ok(())
    }

    /// Loads a Hilbert R-tree from a file.
    ///
    /// Deserializes a tree that was previously saved with `save()`.
    /// Validates the file format by checking the magic number and version.
    /// The loaded tree is immediately ready for querying without rebuilding.
    ///
    /// # Arguments
    /// * `path` - File path of the saved tree
    ///
    /// # Errors
    /// Returns an error if the file cannot be read, the format is invalid,
    /// or the magic number/version check fails.
    pub fn load<P: AsRef<std::path::Path>>(path: P) -> std::io::Result<Self> {
        use std::io::Read;
        let mut file = std::fs::File::open(path)?;
        
        // Read and validate magic number
        let mut magic_buf = [0u8; 1];
        file.read_exact(&mut magic_buf)?;
        if magic_buf[0] != 0xfc {
            return Err(std::io::Error::new(
                std::io::ErrorKind::InvalidData,
                "Invalid file format: magic number mismatch (expected i32 variant 0xfc)",
            ));
        }
        
        // Read and validate version
        let mut version_buf = [0u8; 1];
        file.read_exact(&mut version_buf)?;
        if version_buf[0] != 0x01 {
            return Err(std::io::Error::new(
                std::io::ErrorKind::InvalidData,
                "Unsupported file version (expected v1, got different version)",
            ));
        }
        
        // Read node_size
        let mut buf = [0u8; 4];
        file.read_exact(&mut buf)?;
        let node_size = u32::from_le_bytes(buf) as usize;
        
        // Read num_items
        file.read_exact(&mut buf)?;
        let num_items = u32::from_le_bytes(buf) as usize;
        
        // Read total_nodes
        file.read_exact(&mut buf)?;
        let total_nodes = u32::from_le_bytes(buf) as usize;
        
        // Read level_bounds length
        file.read_exact(&mut buf)?;
        let level_bounds_len = u32::from_le_bytes(buf) as usize;
        
        // Read level_bounds
        let mut level_bounds = Vec::with_capacity(level_bounds_len);
        for _ in 0..level_bounds_len {
            file.read_exact(&mut buf)?;
            level_bounds.push(u32::from_le_bytes(buf) as usize);
        }
        
        // Read bounds
        file.read_exact(&mut buf)?;
        let min_x = i32::from_le_bytes(buf);
        file.read_exact(&mut buf)?;
        let min_y = i32::from_le_bytes(buf);
        file.read_exact(&mut buf)?;
        let max_x = i32::from_le_bytes(buf);
        file.read_exact(&mut buf)?;
        let max_y = i32::from_le_bytes(buf);
        
        let bounds = BoxI32::new(min_x, min_y, max_x, max_y);
        
        // Read data buffer
        file.read_exact(&mut buf)?;
        let data_len = u32::from_le_bytes(buf) as usize;
        let mut data = vec![0u8; data_len];
        file.read_exact(&mut data)?;
        
        Ok(Self {
            data,
            level_bounds,
            node_size,
            num_items,
            position: 0,
            bounds,
            total_nodes,
            allocated_capacity: data_len,
        })
    }
}

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

/// Hilbert curve index computation
/// From <https://github.com/rawrunprotected/hilbert_curves> (public domain)
fn interleave(mut x: u32) -> u32 {
    x = (x | (x << 8)) & 0x00FF00FF;
    x = (x | (x << 4)) & 0x0F0F0F0F;
    x = (x | (x << 2)) & 0x33333333;
    x = (x | (x << 1)) & 0x55555555;
    x
}

#[expect(non_snake_case)]
fn hilbert_xy_to_index(x: u32, y: u32) -> u32 {
    // Initial prefix scan round, prime with x and y
    let mut a = x ^ y;
    let mut b = 0xFFFF ^ a;
    let mut c = 0xFFFF ^ (x | y);
    let mut d = x & (y ^ 0xFFFF);
    let mut A = a | (b >> 1);
    let mut B = (a >> 1) ^ a;
    let mut C = ((c >> 1) ^ (b & (d >> 1))) ^ c;
    let mut D = ((a & (c >> 1)) ^ (d >> 1)) ^ d;

    a = A;
    b = B;
    c = C;
    d = D;
    A = (a & (a >> 2)) ^ (b & (b >> 2));
    B = (a & (b >> 2)) ^ (b & ((a ^ b) >> 2));
    C ^= (a & (c >> 2)) ^ (b & (d >> 2));
    D ^= (b & (c >> 2)) ^ ((a ^ b) & (d >> 2));

    a = A;
    b = B;
    c = C;
    d = D;
    A = (a & (a >> 4)) ^ (b & (b >> 4));
    B = (a & (b >> 4)) ^ (b & ((a ^ b) >> 4));
    C ^= (a & (c >> 4)) ^ (b & (d >> 4));
    D ^= (b & (c >> 4)) ^ ((a ^ b) & (d >> 4));

    // Final round and projection
    a = A;
    b = B;
    c = C;
    d = D;
    C ^= (a & (c >> 8)) ^ (b & (d >> 8));
    D ^= (b & (c >> 8)) ^ ((a ^ b) & (d >> 8));

    // Undo transformation prefix scan
    a = C ^ (C >> 1);
    b = D ^ (D >> 1);

    // Recover index bits
    let i0 = x ^ y;
    let i1 = b | (0xFFFF ^ (i0 | a));

    (interleave(i1) << 1) | interleave(i0)
}