/*
 * // Copyright (c) 2021 Feng Yang
 * //
 * // I am making my contributions/submissions to this project solely in my
 * // personal capacity and am not conveying any rights to any intellectual
 * // property of any third parties.
 */

use crate::bounding_box2::BoundingBox2D;
use crate::nearest_neighbor_query_engine2::*;
use crate::intersection_query_engine2::*;
use crate::ray2::Ray2D;
use crate::vector2::Vector2D;

enum Info {
    Child(usize, u8),
    Item(usize),
}

struct Node {
    info: Info,
    bound: BoundingBox2D,
}

impl Node {
    fn new() -> Node {
        return Node {
            info: Info::Child(usize::MAX, 0),
            bound: BoundingBox2D::new_default(),
        };
    }

    fn init_leaf(&mut self, it: usize, b: BoundingBox2D) {
        self.info = Info::Item(it);
        self.bound = b;
    }
    fn init_internal(&mut self, axis: u8, c: usize, b: BoundingBox2D) {
        self.info = Info::Child(c, axis);
        self.bound = b;
    }
}

///
/// # Bounding Volume Hierarchy (BVH) in 2D
///
/// This class implements the classic bounding volume hierarchy structure in 2D.
/// It implements IntersectionQueryEngine2 in order to support box/ray
/// intersection tests. Also, NearestNeighborQueryEngine2 is implemented to
/// provide nearest neighbor query.
///
pub struct Bvh2<T: Clone> {
    _bound: BoundingBox2D,
    _items: Vec<T>,
    _item_bounds: Vec<BoundingBox2D>,
    _nodes: Vec<Node>,
}

impl<T: Clone> Bvh2<T> {
    /// Default constructor.
    /// ```
    /// use vox_geometry_rust::bvh2::Bvh2;
    /// use vox_geometry_rust::vector2::Vector2D;
    /// let bvh:Bvh2<Vector2D> = Bvh2::new();
    /// assert_eq!(bvh.number_of_nodes(), 0);
    /// ```
    pub fn new() -> Bvh2<T> {
        return Bvh2 {
            _bound: BoundingBox2D::new_default(),
            _items: vec![],
            _item_bounds: vec![],
            _nodes: vec![],
        };
    }

    /// Builds bounding volume hierarchy.
    /// ```
    /// use vox_geometry_rust::bvh2::Bvh2;
    /// use vox_geometry_rust::vector2::Vector2D;
    /// use vox_geometry_rust::bounding_box2::BoundingBox2D;
    /// let mut bvh:Bvh2<Vector2D> = Bvh2::new();
    ///
    /// let points:Vec<Vector2D> = vec![Vector2D::new_default(), Vector2D::new(1.0, 1.0)];
    /// let mut rootBounds = BoundingBox2D::new_default();
    /// let bounds = (0..points.len()).map(|index|{
    ///     let c = points[index];
    ///     let mut aabb = BoundingBox2D::new(c, c);
    ///     aabb.expand(0.1);
    ///     rootBounds.merge_box(&aabb);
    ///     return aabb.clone();
    /// }).collect();
    ///
    /// bvh.build(&points, &bounds);
    ///
    /// assert_eq!(2, bvh.number_of_items());
    /// assert_eq!(points[0], *bvh.item(0));
    /// assert_eq!(points[1], *bvh.item(1));
    /// assert_eq!(3, bvh.number_of_nodes());
    /// assert_eq!(1, bvh.children(0).0);
    /// assert_eq!(2, bvh.children(0).1);
    /// assert_eq!(bvh.is_leaf(0), false);
    /// assert_eq!(bvh.is_leaf(1), true);
    /// assert_eq!(bvh.is_leaf(2), true);
    /// assert_eq!(rootBounds.lower_corner, bvh.node_bound(0).lower_corner);
    /// assert_eq!(rootBounds.upper_corner, bvh.node_bound(0).upper_corner);
    /// assert_eq!(bounds[0].lower_corner, bvh.node_bound(1).lower_corner);
    /// assert_eq!(bounds[0].upper_corner, bvh.node_bound(1).upper_corner);
    /// assert_eq!(bounds[1].lower_corner, bvh.node_bound(2).lower_corner);
    /// assert_eq!(bounds[1].upper_corner, bvh.node_bound(2).upper_corner);
    /// ```
    pub fn build(&mut self, items: &Vec<T>,
                 item_bounds: &Vec<BoundingBox2D>) {
        self._items = items.clone();
        self._item_bounds = item_bounds.clone();

        if self._items.is_empty() {
            return;
        }

        self._nodes.clear();
        self._bound = BoundingBox2D::new_default();

        for i in 0..self._items.len() {
            self._bound.merge_box(&self._item_bounds[i]);
        }

        let mut item_indices: Vec<usize> = (0..self._items.len()).collect();

        self.build_internal(0, &mut item_indices, 0, self._items.len(), 0);
    }

    /// Clears all the contents of this instance.
    pub fn clear(&mut self) {
        self._bound = BoundingBox2D::new_default();
        self._items.clear();
        self._item_bounds.clear();
        self._nodes.clear();
    }

    fn build_internal(&mut self, node_index: usize, item_indices: &mut Vec<usize>, item_start: usize, n_items: usize,
                      current_depth: usize) -> usize {
        // add a node
        self._nodes.push(Node::new());

        // initialize leaf node if termination criteria met
        if n_items == 1 {
            self._nodes[node_index].init_leaf(item_indices[item_start], self._item_bounds[item_indices[item_start]].clone());
            return current_depth + 1;
        }

        // find the mid-point of the bounding box to use as a qsplit pivot
        let mut node_bound = BoundingBox2D::new_default();
        for i in 0..n_items {
            node_bound.merge_box(&self._item_bounds[item_indices[item_start + i]]);
        }

        let d = node_bound.upper_corner - node_bound.lower_corner;

        // choose which axis to split along
        let axis: u8;
        if d.x > d.y {
            axis = 0;
        } else {
            axis = 1;
        }

        let pivot = 0.5 * (node_bound.upper_corner[axis.into()] + node_bound.lower_corner[axis.into()]);

        // classify primitives with respect to split
        let mid_point = self.qsplit(item_indices, item_start, n_items, pivot, axis);

        // recursively initialize children _nodes
        let d0 = self.build_internal(node_index + 1, item_indices, item_start, mid_point, current_depth + 1);
        let len = self._nodes.len();
        self._nodes[node_index].init_internal(axis, len, node_bound);
        let d1 = match self._nodes[node_index].info {
            Info::Child(index, _flag) => {
                self.build_internal(index, item_indices, item_start + mid_point,
                                    n_items - mid_point, current_depth + 1)
            }
            Info::Item(_index) => { panic!() }
        };

        return usize::max(d0, d1);
    }

    fn qsplit(&self, item_indices: &mut Vec<usize>, item_start: usize, num_items: usize, pivot: f64,
              axis: u8) -> usize {
        let mut centroid: f64;
        let mut ret = 0;
        for i in 0..num_items {
            let b = self._item_bounds[item_indices[item_start + i]].clone();
            centroid = 0.5 * (b.lower_corner[axis.into()] + b.upper_corner[axis.into()]);
            if centroid < pivot {
                item_indices.swap(item_start + i, item_start + ret);
                ret += 1;
            }
        }
        if ret == 0 || ret == num_items {
            ret = num_items >> 1;
        }
        return ret;
    }

    /// Returns the number of items.
    pub fn number_of_items(&self) -> usize {
        return self._items.len();
    }

    /// Returns the item at \p i.
    pub fn item(&self, i: usize) -> &T {
        return &self._items[i];
    }

    /// Returns the number of nodes.
    pub fn number_of_nodes(&self) -> usize {
        return self._nodes.len();
    }

    /// Returns the children indices of \p i-th node.
    pub fn children(&self, i: usize) -> (usize, usize) {
        return if self.is_leaf(i) {
            (usize::MAX, usize::MAX)
        } else {
            match self._nodes[i].info {
                Info::Child(index, _flag) => {
                    (i + 1, index)
                }
                Info::Item(_) => { panic!() }
            }
        };
    }

    /// Returns true if \p i-th node is a leaf node.
    pub fn is_leaf(&self, i: usize) -> bool {
        return match self._nodes[i].info {
            Info::Child(_, _) => { false }
            Info::Item(_) => { true }
        };
    }

    /// Returns bounding box of \p i-th node.
    pub fn node_bound(&self, i: usize) -> BoundingBox2D {
        return self._nodes[i].bound.clone();
    }

    /// Returns bounding box of every items.
    pub fn bounding_box(&self) -> BoundingBox2D {
        return self._bound.clone();
    }

    /// Returns item of \p i-th node.
    pub fn item_of_node(&self, i: usize) -> usize {
        match self._nodes[i].info {
            Info::Child(_, _) => {
                return self._nodes.len();
            }
            Info::Item(index) => {
                return index;
            }
        }
    }
}

impl<T: Clone> IntersectionQueryEngine2<T> for Bvh2<T> {
    /// ```
    /// use vox_geometry_rust::bvh2::Bvh2;
    /// use vox_geometry_rust::vector2::Vector2D;
    /// use vox_geometry_rust::bounding_box2::BoundingBox2D;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine2::*;
    /// use vox_geometry_rust::intersection_query_engine2::IntersectionQueryEngine2;
    /// let mut bvh:Bvh2<Vector2D> = Bvh2::new();
    ///
    /// let mut overlaps_func = |pt:&Vector2D, bbox:&BoundingBox2D| {
    ///     let mut aabb = BoundingBox2D::new(*pt, *pt);
    ///     aabb.expand(0.1);
    ///     return bbox.overlaps(&aabb);
    /// };
    ///
    /// let num_samples = get_number_of_sample_points2();
    /// let points = (0..num_samples).map(|index| Vector2D::new_lst(get_sample_points2()[index])).collect();
    ///
    /// let bounds = (0..num_samples).map(|index| {
    ///     let c = Vector2D::new_lst(get_sample_points2()[index]);
    ///     let mut aabb = BoundingBox2D::new(c, c);
    ///     aabb.expand(0.1);
    ///     aabb
    /// }).collect();
    ///
    /// bvh.build(&points, &bounds);
    ///
    /// let test_box = BoundingBox2D::new(Vector2D::new(0.25, 0.15), Vector2D::new(0.5, 0.6));
    /// let mut has_overlaps = false;
    /// for i in 0..num_samples {
    ///     has_overlaps |= overlaps_func(&Vector2D::new_lst(get_sample_points2()[i]), &test_box);
    /// }
    ///
    /// assert_eq!(has_overlaps, bvh.intersects_aabb(&test_box, &mut overlaps_func));
    ///
    /// let test_box2 = BoundingBox2D::new(Vector2D::new(0.2, 0.2), Vector2D::new(0.6, 0.5));
    /// has_overlaps = false;
    /// for i in 0..num_samples {
    ///     has_overlaps |= overlaps_func(&Vector2D::new_lst(get_sample_points2()[i]), &test_box2);
    /// }
    ///
    /// assert_eq!(has_overlaps, bvh.intersects_aabb(&test_box2, &mut overlaps_func));
    /// ```
    fn intersects_aabb<Callback>(&self, aabb: &BoundingBox2D, test_func: &mut Callback) -> bool
        where Callback: BoxIntersectionTestFunc2<T> {
        if !self._bound.overlaps(aabb) {
            return false;
        }

        // prepare to traverse BVH for box
        const K_MAX_TREE_DEPTH: usize = 8 * 32;
        let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
        let mut todo_pos = 0;

        // traverse BVH nodes for box
        let mut node: usize = 0;
        while node < self._nodes.len() {
            match self._nodes[node].info {
                Info::Child(index, _flag) => {
                    // get node children pointers for box
                    let first_child = node + 1;
                    let second_child = index;

                    // advance to next child node, possibly enqueue other child
                    if !self._nodes[first_child].bound.overlaps(aabb) {
                        node = second_child;
                    } else if !self._nodes[second_child].bound.overlaps(aabb) {
                        node = first_child;
                    } else {
                        // enqueue second_child in todo stack
                        todo[todo_pos] = Some(second_child);
                        todo_pos += 1;
                        node = first_child;
                    }
                }
                Info::Item(index) => {
                    if test_func(&self._items[index], aabb) {
                        return true;
                    }

                    // grab next node to process from todo stack
                    if todo_pos > 0 {
                        // Dequeue
                        todo_pos -= 1;
                        node = todo[todo_pos].unwrap();
                    } else {
                        break;
                    }
                }
            }
        }

        return false;
    }

    /// ```
    /// use vox_geometry_rust::bvh2::Bvh2;
    /// use vox_geometry_rust::vector2::Vector2D;
    /// use vox_geometry_rust::bounding_box2::BoundingBox2D;
    /// use vox_geometry_rust::ray2::Ray2D;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine2::*;
    /// use vox_geometry_rust::intersection_query_engine2::IntersectionQueryEngine2;
    /// let mut bvh:Bvh2<BoundingBox2D> = Bvh2::new();
    ///
    /// let mut intersects_func = |a:&BoundingBox2D, ray:&Ray2D| {
    ///         return a.intersects(ray);
    /// };
    ///
    /// let num_samples = get_number_of_sample_points2();
    /// let items = (0..num_samples/2).map(|index| {
    ///     let c = Vector2D::new_lst(get_sample_points2()[index]);
    ///     let mut aabb = BoundingBox2D::new(c, c);
    ///     aabb.expand(0.1);
    ///     aabb
    /// }).collect();
    ///
    /// bvh.build(&items, &items);
    ///
    /// for i in 0..num_samples/2 {
    ///     let ray = Ray2D::new(Vector2D::new_lst(get_sample_dirs2()[i + num_samples / 2]),
    ///                          Vector2D::new_lst(get_sample_dirs2()[i + num_samples / 2]));
    ///     // ad-hoc search
    ///     let mut ans_ints = false;
    ///     for j in 0..num_samples/2 {
    ///         if intersects_func(&items[j], &ray) {
    ///             ans_ints = true;
    ///             break;
    ///         }
    ///     }
    ///
    ///     // bvh search
    ///     let oct_ints = bvh.intersects_ray(&ray, &mut intersects_func);
    ///
    ///     assert_eq!(ans_ints, oct_ints);
    /// }
    /// ```
    fn intersects_ray<Callback>(&self, ray: &Ray2D, test_func: &mut Callback) -> bool
        where Callback: RayIntersectionTestFunc2<T> {
        if !self._bound.intersects(ray) {
            return false;
        }

        // prepare to traverse BVH for ray
        const K_MAX_TREE_DEPTH: usize = 8 * 32;
        let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
        let mut todo_pos = 0;

        // traverse BVH nodes for box
        let mut node: usize = 0;
        while node < self._nodes.len() {
            match self._nodes[node].info {
                Info::Child(index, flag) => {
                    // get node children pointers for ray
                    let first_child: usize;
                    let second_child: usize;
                    if ray.direction[flag.into()] > 0.0 {
                        first_child = node + 1;
                        second_child = index;
                    } else {
                        first_child = index;
                        second_child = node + 1;
                    }

                    // advance to next child node, possibly enqueue other child
                    if !self._nodes[first_child].bound.intersects(ray) {
                        node = second_child;
                    } else if !self._nodes[second_child].bound.intersects(ray) {
                        node = first_child;
                    } else {
                        // enqueue second_child in todo stack
                        todo[todo_pos] = Some(second_child);
                        todo_pos += 1;
                        node = first_child;
                    }
                }
                Info::Item(index) => {
                    if test_func(&self._items[index], ray) {
                        return true;
                    }

                    // grab next node to process from todo stack
                    if todo_pos > 0 {
                        // Dequeue
                        todo_pos -= 1;
                        node = todo[todo_pos].unwrap();
                    } else {
                        break;
                    }
                }
            }
        }

        return false;
    }

    /// ```
    /// use vox_geometry_rust::bvh2::Bvh2;
    /// use vox_geometry_rust::vector2::Vector2D;
    /// use vox_geometry_rust::bounding_box2::BoundingBox2D;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine2::*;
    /// use vox_geometry_rust::intersection_query_engine2::IntersectionQueryEngine2;
    /// let mut bvh:Bvh2<Vector2D> = Bvh2::new();
    ///
    /// let mut overlaps_func = |pt:&Vector2D, bbox:&BoundingBox2D| {
    ///         return bbox.contains(pt);
    /// };
    ///
    /// let overlaps_func2 = |pt:&Vector2D, bbox:&BoundingBox2D| {
    ///         return bbox.contains(pt);
    /// };
    ///
    /// let num_samples = get_number_of_sample_points2();
    /// let points = (0..num_samples).map(|index| Vector2D::new_lst(get_sample_points2()[index])).collect();
    ///
    /// let bounds = (0..num_samples).map(|index| {
    ///     let c = Vector2D::new_lst(get_sample_points2()[index]);
    ///     let mut aabb = BoundingBox2D::new(c, c);
    ///     aabb.expand(0.1);
    ///     aabb
    /// }).collect();
    ///
    /// bvh.build(&points, &bounds);
    ///
    /// let test_box = BoundingBox2D::new(Vector2D::new(0.2, 0.2), Vector2D::new(0.6, 0.5));
    /// let mut num_overlaps = 0;
    /// for i in 0..num_samples {
    ///     num_overlaps += overlaps_func(&Vector2D::new_lst(get_sample_points2()[i]), &test_box) as usize;
    /// }
    ///
    /// let mut measured = 0;
    /// bvh.for_each_intersecting_item_aabb(&test_box, &mut overlaps_func, &mut |pt:&Vector2D| {
    ///     assert_eq!(overlaps_func2(pt, &test_box), true);
    ///     measured += 1;
    /// });
    ///
    /// assert_eq!(num_overlaps, measured);
    /// ```
    fn for_each_intersecting_item_aabb<Callback, Visitor>(&self, aabb: &BoundingBox2D,
                                                          test_func: &mut Callback, visitor_func: &mut Visitor)
        where Callback: BoxIntersectionTestFunc2<T>,
              Visitor: IntersectionVisitorFunc2<T> {
        if !self._bound.overlaps(aabb) {
            return;
        }

        // prepare to traverse BVH for box
        const K_MAX_TREE_DEPTH: usize = 8 * 32;
        let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
        let mut todo_pos = 0;

        // traverse BVH nodes for box
        let mut node: usize = 0;
        while node < self._nodes.len() {
            match self._nodes[node].info {
                Info::Child(index, _flag) => {
                    // get node children pointers for box
                    let first_child = node + 1;
                    let second_child = index;

                    // advance to next child node, possibly enqueue other child
                    if !self._nodes[first_child].bound.overlaps(aabb) {
                        node = second_child;
                    } else if !self._nodes[second_child].bound.overlaps(aabb) {
                        node = first_child;
                    } else {
                        // enqueue second_child in todo stack
                        todo[todo_pos] = Some(second_child);
                        todo_pos += 1;
                        node = first_child;
                    }
                }
                Info::Item(index) => {
                    if test_func(&self._items[index], aabb) {
                        visitor_func(&self._items[index]);
                    }

                    // grab next node to process from todo stack
                    if todo_pos > 0 {
                        // Dequeue
                        todo_pos -= 1;
                        node = todo[todo_pos].unwrap();
                    } else {
                        break;
                    }
                }
            }
        }
    }

    fn for_each_intersecting_item_ray<Callback, Visitor>(&self, ray: &Ray2D,
                                                         test_func: &mut Callback, visitor_func: &mut Visitor)
        where Callback: RayIntersectionTestFunc2<T>,
              Visitor: IntersectionVisitorFunc2<T> {
        if !self._bound.intersects(ray) {
            return;
        }

        // prepare to traverse BVH for box
        const K_MAX_TREE_DEPTH: usize = 8 * 32;
        let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
        let mut todo_pos = 0;

        // traverse BVH nodes for box
        let mut node: usize = 0;
        while node < self._nodes.len() {
            match self._nodes[node].info {
                Info::Child(index, flag) => {
                    // get node children pointers for ray
                    let first_child: usize;
                    let second_child: usize;
                    if ray.direction[flag.into()] > 0.0 {
                        first_child = node + 1;
                        second_child = index;
                    } else {
                        first_child = index;
                        second_child = node + 1;
                    }

                    // advance to next child node, possibly enqueue other child
                    if !self._nodes[first_child].bound.intersects(ray) {
                        node = second_child;
                    } else if !self._nodes[second_child].bound.intersects(ray) {
                        node = first_child;
                    } else {
                        // enqueue second_child in todo stack
                        todo[todo_pos] = Some(second_child);
                        todo_pos += 1;
                        node = first_child;
                    }
                }
                Info::Item(index) => {
                    if test_func(&self._items[index], ray) {
                        visitor_func(&self._items[index]);
                    }

                    // grab next node to process from todo stack
                    if todo_pos > 0 {
                        // Dequeue
                        todo_pos -= 1;
                        node = todo[todo_pos].unwrap();
                    } else {
                        break;
                    }
                }
            }
        }
    }

    /// ```
    /// use vox_geometry_rust::bvh2::Bvh2;
    /// use vox_geometry_rust::vector2::Vector2D;
    /// use vox_geometry_rust::bounding_box2::BoundingBox2D;
    /// use vox_geometry_rust::ray2::Ray2D;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine2::*;
    /// use vox_geometry_rust::intersection_query_engine2::*;
    /// let mut bvh:Bvh2<BoundingBox2D> = Bvh2::new();
    ///
    /// let mut intersects_func = |a:&BoundingBox2D, ray:&Ray2D| {
    ///     let bbox_result = a.closest_intersection(ray);
    ///     return if bbox_result.is_intersecting {
    ///          bbox_result.t_near
    ///     } else {
    ///          f64::MAX
    ///     };
    /// };
    ///
    /// let num_samples = get_number_of_sample_points2();
    /// let items = (0..num_samples/2).map(|index| {
    ///     let c = Vector2D::new_lst(get_sample_points2()[index]);
    ///     let mut aabb = BoundingBox2D::new(c, c);
    ///     aabb.expand(0.1);
    ///     aabb
    /// }).collect();
    ///
    /// bvh.build(&items, &items);
    ///
    /// for i in 0..num_samples/2 {
    ///     let ray = Ray2D::new(Vector2D::new_lst(get_sample_points2()[i + num_samples / 2]),
    ///                          Vector2D::new_lst(get_sample_dirs2()[i + num_samples / 2]));
    ///         // ad-hoc search
    ///     let mut ans_ints:ClosestIntersectionQueryResult2<BoundingBox2D> = ClosestIntersectionQueryResult2::new();
    ///     for j in 0..num_samples/2 {
    ///         let dist = intersects_func(&items[j], &ray);
    ///         if dist < ans_ints.distance {
    ///             ans_ints.distance = dist;
    ///             ans_ints.item = Some(bvh.item(j).clone());
    ///         }
    ///     }
    ///
    ///     // bvh search
    ///     let bvh_ints = bvh.closest_intersection(&ray, &mut intersects_func);
    ///
    ///     assert_eq!(ans_ints.distance, bvh_ints.distance);
    ///     let ans_aabb = ans_ints.item.unwrap_or(BoundingBox2D::new_default());
    ///     let oct_aabb = bvh_ints.item.unwrap_or(BoundingBox2D::new_default());
    ///     assert_eq!(ans_aabb.upper_corner, oct_aabb.upper_corner);
    ///     assert_eq!(ans_aabb.lower_corner, oct_aabb.lower_corner);
    /// }
    /// ```
    fn closest_intersection<Callback>(&self, ray: &Ray2D, test_func: &mut Callback) -> ClosestIntersectionQueryResult2<T>
        where Callback: GetRayIntersectionFunc2<T> {
        let mut best: ClosestIntersectionQueryResult2<T> = ClosestIntersectionQueryResult2::new();
        best.distance = f64::MAX;
        best.item = None;

        if !self._bound.intersects(ray) {
            return best;
        }

        // prepare to traverse BVH for ray
        const K_MAX_TREE_DEPTH: usize = 8 * 32;
        let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
        let mut todo_pos = 0;

        // traverse BVH nodes for ray
        let mut node: usize = 0;
        while node < self._nodes.len() {
            match self._nodes[node].info {
                Info::Child(index, flag) => {
                    // get node children pointers for ray
                    let first_child: usize;
                    let second_child: usize;
                    if ray.direction[flag.into()] > 0.0 {
                        first_child = node + 1;
                        second_child = index;
                    } else {
                        first_child = index;
                        second_child = node + 1;
                    }

                    // advance to next child node, possibly enqueue other child
                    if !self._nodes[first_child].bound.intersects(ray) {
                        node = second_child;
                    } else if !self._nodes[second_child].bound.intersects(ray) {
                        node = first_child;
                    } else {
                        // enqueue second_child in todo stack
                        todo[todo_pos] = Some(second_child);
                        todo_pos += 1;
                        node = first_child;
                    }
                }
                Info::Item(index) => {
                    let dist = test_func(&self._items[index], ray);
                    if dist < best.distance {
                        best.distance = dist;
                        best.item = Some(self._items[index].clone());
                    }

                    // grab next node to process from todo stack
                    if todo_pos > 0 {
                        // Dequeue
                        todo_pos -= 1;
                        node = todo[todo_pos].unwrap();
                    } else {
                        break;
                    }
                }
            }
        }

        return best;
    }
}

impl<T: Clone> NearestNeighborQueryEngine2<T> for Bvh2<T> {
    /// ```
    /// use vox_geometry_rust::bvh2::Bvh2;
    /// use vox_geometry_rust::vector2::Vector2D;
    /// use vox_geometry_rust::bounding_box2::BoundingBox2D;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine2::*;
    /// let mut bvh:Bvh2<Vector2D> = Bvh2::new();
    ///
    /// let mut distance_func = |a:&Vector2D, b:&Vector2D| {
    ///         return a.distance_to(*b);
    ///     };
    ///
    /// let num_samples = get_number_of_sample_points2();
    /// let points = (0..num_samples).map(|index| Vector2D::new_lst(get_sample_points2()[index])).collect();
    ///
    /// let bounds = (0..num_samples).map(|index| {
    ///     let c = Vector2D::new_lst(get_sample_points2()[index]);
    ///     let mut aabb = BoundingBox2D::new(c, c);
    ///     aabb.expand(0.1);
    ///     aabb
    /// }).collect();
    ///
    /// bvh.build(&points, &bounds);
    ///
    /// let test_pt = Vector2D::new(0.5, 0.5);
    /// let nearest = bvh.nearest(&test_pt, &mut distance_func);
    /// let mut answer_idx = 0;
    /// let mut best_dist = test_pt.distance_to(points[answer_idx]);
    /// for i in 1..num_samples {
    /// let dist = test_pt.distance_to(Vector2D::new_lst(get_sample_points2()[i]));
    ///     if dist < best_dist {
    ///         best_dist = dist;
    ///         answer_idx = i;
    ///     }
    /// }
    ///
    /// assert_eq!(nearest.item.unwrap(), *bvh.item(answer_idx));
    /// ```
    fn nearest<Callback>(&self, pt: &Vector2D, distance_func: &mut Callback) -> NearestNeighborQueryResult2<T>
        where Callback: NearestNeighborDistanceFunc2<T> {
        let mut best: NearestNeighborQueryResult2<T> = NearestNeighborQueryResult2::new();
        best.distance = f64::MAX;
        best.item = None;

        // Prepare to traverse BVH
        const K_MAX_TREE_DEPTH: usize = 8 * 32;
        let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
        let mut todo_pos = 0;

        // Traverse BVH nodes
        let mut node: usize = 0;
        while node < self._nodes.len() {
            match self._nodes[node].info {
                Info::Child(index, _flag) => {
                    let best_dist_sqr = best.distance * best.distance;

                    let left = node + 1;
                    let right = index;

                    // If pt is inside the box, then the closest_left and Right will be
                    // identical to pt. This will make dist_min_left_sqr and
                    // dist_min_right_sqr zero, meaning that such a box will have higher
                    // priority.
                    let closest_left = self._nodes[left].bound.clamp(pt);
                    let closest_right = self._nodes[right].bound.clamp(pt);

                    let dist_min_left_sqr = closest_left.distance_squared_to(*pt);
                    let dist_min_right_sqr = closest_right.distance_squared_to(*pt);

                    let should_visit_left = dist_min_left_sqr < best_dist_sqr;
                    let should_visit_right = dist_min_right_sqr < best_dist_sqr;

                    let first_child: usize;
                    let second_child: usize;
                    if should_visit_left && should_visit_right {
                        if dist_min_left_sqr < dist_min_right_sqr {
                            first_child = left;
                            second_child = right;
                        } else {
                            first_child = right;
                            second_child = left;
                        }

                        // Enqueue second_child in todo stack
                        todo[todo_pos] = Some(second_child);
                        todo_pos += 1;
                        node = first_child;
                    } else if should_visit_left {
                        node = left;
                    } else if should_visit_right {
                        node = right;
                    } else {
                        if todo_pos > 0 {
                            // Dequeue
                            todo_pos -= 1;
                            node = todo[todo_pos].unwrap();
                        } else {
                            break;
                        }
                    }
                }
                Info::Item(index) => {
                    let dist = distance_func(&self._items[index], pt);
                    if dist < best.distance {
                        best.distance = dist;
                        best.item = Some(self._items[index].clone());
                    }

                    // Grab next node to process from todo stack
                    if todo_pos > 0 {
                        // Dequeue
                        todo_pos -= 1;
                        node = todo[todo_pos].unwrap();
                    } else {
                        break;
                    }
                }
            }
        }

        return best;
    }
}