gemlab 2.0.0

use super::num_divisions;
use crate::geometry::{point_circle_distance, point_cylinder_distance, point_line_distance, point_point_distance};
use crate::StrError;
use plotpy::{Canvas, Curve, Plot, Text};
use russell_lab::math::{SQRT_2, SQRT_3};
use std::collections::{HashMap, HashSet};
use std::fmt;

/// Returns true to any point coordinate
///
/// This function is useful for the search functions,
/// and corresponds to the following closure:
///
/// ```text
/// |_| true
/// ```
pub fn any_x(_: &[f64]) -> bool {
    true
}

/// Default GridSearch number of divisions for the longest direction
pub const GS_DEFAULT_NDIV: usize = 100;

/// Default GridSearch tolerance for all directions
pub const GS_DEFAULT_TOLERANCE: f64 = 1e-4;

/// Default GridSearch border tolerance to handle imprecision near the borders
pub const GS_DEFAULT_BORDER_TOL: f64 = 1e-2;

/// Specifies the key of containers (or bins in the grid)
type ContainerKey = usize;

/// Specifies the identification number of items
type ItemId = usize;

/// Defines the container type: ID to Coordinates
///
/// Note: we cannot use a HashSet here, because we need to store the coordinates
/// for later use such as when calculating point-to-point distances
type Container = HashMap<ItemId, Vec<f64>>;

/// Defines the containers type: Key to Container
type Containers = HashMap<ContainerKey, Container>;

/// Implements a grid for fast searching entries by coordinates
///
/// # Definitions
///
/// * `xmin` -- (ndim) minimum coordinates to define the boundary
/// * `xmax` -- (ndim) maximum coordinates to define the boundary
/// * `ndiv` -- number of division for the longest direction
/// * `tolerance` -- tolerance used to define the Halo and to compare points; e.g. 1e-4
/// * `border_tol` -- tolerance used to expand the border a little bit and then
///   accommodate eventual imprecision near the borders; e.g. 1e-2
///
/// **Important:** The number of divisions cannot be very large because it could
/// make the container's `side_length` be smaller the the `tolerance`. Conversely,
/// the tolerance cannot be too large making the halo bigger than the container.
/// In fact, the following constraint must be satisfied:
///
/// ```text
/// side_length = (xmax - xmin) / ndiv > 2·tolerance
/// ```
///
/// # Reference
///
/// * Durand, Farias, and Pedroso (2015) Computing intersections between
///   non-compatible curves and finite elements, Computational Mechanics;
///   DOI=10.1007/s00466-015-1181-y
///
/// # Examples
///
/// ```
/// use gemlab::util::GridSearch;
/// use gemlab::StrError;
/// use plotpy::Plot;
/// use russell_lab::math::SQRT_2;
///
/// fn main() -> Result<(), StrError> {
///     let xmin = &[0.0, 0.0];
///     let xmax = &[10.0, 10.0];
///     let mut grid = GridSearch::new(xmin, xmax, Some(2), None, None)?;
///     grid.insert(123, &[5.5, SQRT_2])?;
///     assert_eq!(grid.search(&[5.5, SQRT_2])?, Some(123));
///     assert_eq!(grid.search(&[5.501, SQRT_2])?, None);
///     if false {
///         let mut plot = Plot::new();
///         grid.draw(&mut plot, true)?;
///         plot.set_equal_axes(true).save("/tmp/gemlab/doc_grid_search.svg")?;
///     }
///     Ok(())
/// }
/// ```
///
/// ![Output of the code above](https://github.com/cpmech/gemlab/raw/main/data/figures/doc_grid_search.svg)
///
/// The figure above illustrates the grid generated by the example.
pub struct GridSearch {
    ndim: usize,             // space dimension
    ndiv: Vec<usize>,        // (ndim) number of divisions along each direction
    xmin: Vec<f64>,          // (ndim) min values
    xmax: Vec<f64>,          // (ndim) max values
    coefficient: Vec<usize>, // (3) coefficients [1, ndiv[0], ndiv[0]*ndiv[1]] (Eq. 8)
    side_length: f64,        // side length of a container
    tolerance: f64,          // tolerance to define the halo
    tol_dist: f64,           // tolerance to search points using the distance between points
    radius: f64,             // radius of the circumscribed circle around a container box
    halo: Vec<Vec<f64>>,     // (ncorner) 4 in 2D or 8 in 3D (each contains ndim coords)
    halo_ncorner: usize,     // number of halo corners = 4 in 2D or 8 in 3D
    containers: Containers,  // structure to hold all items
}

impl GridSearch {
    /// Allocates a new instance
    ///
    /// # Input
    ///
    /// * `xmin` -- (ndim) minimum coordinates to define the boundary
    /// * `xmax` -- (ndim) maximum coordinates to define the boundary
    /// * `ndiv` -- number of division (≥ 1)
    ///     - If None, [GS_DEFAULT_NDIV] is used
    ///     - This number is used for the longest direction
    ///     - The other directions will be subdivided with a number such that the
    ///       containers will be square/cubic
    ///     - For the other directions, the minimum number of divisions will be 1,
    ///       in case they are too short compared with the longest direction
    /// * `tolerance` -- tolerance used to define the Halo and to compare points; e.g. 1e-4
    ///     - If None, [GS_DEFAULT_TOLERANCE] is used
    /// * `border_tol` -- tolerance used to expand the border a little bit and then
    ///   accommodate eventual imprecision near the borders; e.g. 1e-2
    ///     - If None, [GS_DEFAULT_BORDER_TOL] is used
    ///
    /// Given the ndiv for the longest direction, the number of divisions for
    /// the other directions are calculated as follows:
    ///
    /// ```text
    /// ndiv_other = truncate((delta_other/delta_long) * ndiv)
    /// ndiv_other = max(1, ndiv_other)
    /// ```
    ///
    /// Thus the resulting grid will be square/cubic
    ///
    /// **Important:** The number of divisions cannot be very large because it could
    /// make the container's `side_length` be smaller the the `tolerance`. Conversely,
    /// the tolerance cannot be too large making the halo bigger than the container.
    /// In fact, the following constraint must be satisfied:
    ///
    /// ```text
    /// side_length = (xmax - xmin) / ndiv > 2·tolerance
    /// ```
    pub fn new(
        xmin: &[f64],
        xmax: &[f64],
        ndiv: Option<usize>,
        tolerance: Option<f64>,
        border_tol: Option<f64>,
    ) -> Result<Self, StrError> {
        // check input
        let ndim = xmin.len();
        if ndim < 2 || ndim > 3 {
            return Err("xmin.len() = ndim must be 2 or 3");
        }
        if xmax.len() != ndim {
            return Err("xmax.len() must equal ndim = xmin.len()");
        }

        // ndiv
        let ndiv_long = match ndiv {
            Some(v) => v,
            None => GS_DEFAULT_NDIV,
        };
        if ndiv_long < 1 {
            return Err("ndiv must be ≥ 1");
        }
        let ndiv = num_divisions(1, ndiv_long, xmin, xmax)?; // checks that xmax > xmin
        let coefficient = vec![1, ndiv[0], ndiv[0] * ndiv[1]];

        // tolerance
        let tolerance = match tolerance {
            Some(v) => v,
            None => GS_DEFAULT_TOLERANCE,
        };
        if tolerance <= 0.0 {
            return Err("tolerance must be > 0.0");
        }

        // border tolerance
        let border_tol = match border_tol {
            Some(v) => v,
            None => GS_DEFAULT_BORDER_TOL,
        };
        if border_tol < 0.0 {
            return Err("border_tol must be ≥ 0.0");
        }

        // expand borders
        let mut xmin = xmin.to_vec();
        let mut xmax = xmax.to_vec();
        if border_tol > 0.0 {
            for i in 0..ndim {
                xmin[i] -= border_tol;
                xmax[i] += border_tol;
            }
        }

        // compute size of square/cubic container
        let mut side_length = (xmax[0] - xmin[0]) / (ndiv[0] as f64);
        for i in 1..ndim {
            let sl = (xmax[i] - xmin[i]) / (ndiv[i] as f64);
            side_length = f64::max(side_length, sl);
        }
        if side_length <= 2.0 * tolerance {
            return Err("(xmax-xmin)/ndiv must be > 2·tolerance; reduce the tolerance (or ndiv)");
        }

        // update xmax after deciding on the side_length
        // (to make sure that all the area/volume is covered by the grid)
        for i in 0..ndim {
            xmax[i] = xmin[i] + side_length * (ndiv[i] as f64);
        }

        // tolerance to search points
        let tol_dist = if ndim == 2 {
            SQRT_2 * tolerance
        } else {
            SQRT_3 * tolerance
        };

        // radius of the circumscribed circle around a container box
        let radius = if ndim == 2 {
            SQRT_2 * side_length / 2.0
        } else {
            SQRT_3 * side_length / 2.0
        };

        // halo
        let halo_ncorner = usize::pow(2, ndim as u32);
        let halo = vec![vec![0.0; ndim]; halo_ncorner];

        // done
        Ok(GridSearch {
            ndim,
            ndiv,
            xmin,
            xmax,
            coefficient,
            side_length,
            tolerance,
            tol_dist,
            radius,
            halo,
            halo_ncorner,
            containers: HashMap::new(),
        })
    }

    /// Returns whether a point is outside the grid or not (thus we cannot use insert or search)
    ///
    /// # Panics
    ///
    /// This function requires that `x.len() = ndim`, otherwise a panic occurs.
    pub fn is_outside(&self, x: &[f64]) -> bool {
        assert_eq!(x.len(), self.ndim);
        for i in 0..self.ndim {
            if x[i] < self.xmin[i] || x[i] > self.xmax[i] {
                return true;
            }
        }
        return false;
    }

    /// Inserts a new item to a container in the grid
    ///
    /// # Input
    ///
    /// * `id` -- identification number of the item
    /// * `x` -- (ndim) coordinates of the item
    pub fn insert(&mut self, id: usize, x: &[f64]) -> Result<(), StrError> {
        // check
        if x.len() != self.ndim {
            return Err("x.len() must equal ndim");
        }

        // search the key of the container where the coordinates fall in
        let key = match self.calc_container_key(x) {
            Some(k) => k,
            None => return Err("cannot insert point because its coordinates are outside the grid"),
        };

        // add point to container
        self.update_or_insert(key, id, x);

        // add point to the containers touched by the halo corners
        self.set_halo(x);
        let mut tmp = vec![0.0; self.ndim];
        for c in 0..self.halo_ncorner {
            tmp.copy_from_slice(&self.halo[c][0..self.ndim]);
            if let Some(key_corner) = self.calc_container_key(&tmp) {
                if key_corner != key {
                    self.update_or_insert(key_corner, id, x); // make sure to use original `x`
                }
            }
        }
        Ok(())
    }

    /// Searches an item by coordinates
    ///
    /// # Input
    ///
    /// * `x` -- (ndim) coordinates of the item
    ///
    /// # Output
    ///
    /// * `id` -- if found, returns the ID, otherwise, returns None (not found)
    pub fn search(&self, x: &[f64]) -> Result<Option<usize>, StrError> {
        // check
        if x.len() != self.ndim {
            return Err("x.len() must equal ndim");
        }

        // search the key of the container where the coordinates fall in
        let key = match self.calc_container_key(x) {
            Some(k) => k,
            None => return Err("cannot find point because the coordinates are outside the grid"),
        };

        // get the container that has a point near `x`
        let container = match self.containers.get(&key) {
            Some(c) => c,
            None => return Ok(None), // no container has a point near `x`
        };

        // search the closest point to `x` among all points in the container
        for (id, x_other) in container {
            let distance = point_point_distance(x_other, x)?;
            if distance <= self.tol_dist {
                return Ok(Some(*id));
            }
        }
        Ok(None)
    }

    /// Searches points on a 2D or 3D line
    ///
    /// # Input
    ///
    /// * `a` -- (ndim) first point on the line
    /// * `b` -- (ndim) second point on the line (different than `a`)
    /// * `filter` -- fn(x) -> bool that returns true to **keep** the coordinate just found
    ///   (yields only the elements for which the closure returns true)
    ///
    /// # Output
    ///
    /// Returns the ids of points.
    pub fn search_on_line<F>(&self, a: &[f64], b: &[f64], mut filter: F) -> Result<HashSet<usize>, StrError>
    where
        F: FnMut(&[f64]) -> bool,
    {
        // check
        if a.len() != self.ndim {
            return Err("a.len() must equal ndim");
        }
        if b.len() != self.ndim {
            return Err("b.len() must equal ndim");
        }

        // search containers near the line
        let nearest_containers = self.containers_near_line(a, b)?;

        // search container points near the line
        let mut ids = HashSet::new();
        for index in nearest_containers {
            let container = self.containers.get(&index).unwrap();
            for (id, x_other) in container {
                let distance = point_line_distance(a, b, x_other)?;
                if distance <= self.tol_dist && filter(x_other) {
                    ids.insert(*id);
                }
            }
        }
        Ok(ids)
    }

    /// Searches points on the perimeter of a circle (2D only)
    ///
    /// # Input
    ///
    /// * `center` -- 2D circle center
    /// * `radius` -- circle radius
    /// * `filter` -- fn(x) -> bool that returns true to **keep** the coordinate just found
    ///   (yields only the elements for which the closure returns true)
    ///
    /// # Output
    ///
    /// Returns the ids of points.
    ///
    /// # Note
    ///
    /// This works in 2D only.
    pub fn search_on_circle<F>(&self, center: &[f64], radius: f64, mut filter: F) -> Result<HashSet<usize>, StrError>
    where
        F: FnMut(&[f64]) -> bool,
    {
        // check
        if self.ndim != 2 {
            return Err("search_on_circle works in 2D only");
        }
        if center.len() != self.ndim {
            return Err("center.len() must equal ndim");
        }

        // search containers near the circle
        let nearest_containers = self.containers_near_circle(center, radius)?;

        // search container points near the circle
        let mut ids = HashSet::new();
        for index in nearest_containers {
            let container = self.containers.get(&index).unwrap();
            for (id, x_other) in container {
                let distance = point_circle_distance(center, radius, x_other)?;
                if f64::abs(distance) <= self.tol_dist && filter(x_other) {
                    ids.insert(*id);
                }
            }
        }
        Ok(ids)
    }

    /// Searches points on the surface of a cylinder (3D only)
    ///
    /// # Input
    ///
    /// * `a` -- 3D point on the cylinder axis
    /// * `b` -- 3D point on the cylinder axis
    /// * `radius` -- cylinder radius
    /// * `filter` -- fn(x) -> bool that returns true to **keep** the coordinate just found
    ///   (yields only the elements for which the closure returns true)
    ///
    /// # Output
    ///
    /// Returns the ids of points.
    ///
    /// # Note
    ///
    /// This works in 3D only.
    pub fn search_on_cylinder<F>(
        &self,
        a: &[f64],
        b: &[f64],
        radius: f64,
        mut filter: F,
    ) -> Result<HashSet<usize>, StrError>
    where
        F: FnMut(&[f64]) -> bool,
    {
        // check
        if self.ndim != 3 {
            return Err("search_on_cylinder works in 3D only");
        }
        if a.len() != self.ndim {
            return Err("a.len() must equal ndim");
        }
        if b.len() != self.ndim {
            return Err("b.len() must equal ndim");
        }

        // search containers near the cylinder
        let nearest_containers = self.containers_near_cylinder(a, b, radius)?;

        // search container points near the cylinder
        let mut ids = HashSet::new();
        for index in nearest_containers {
            let container = self.containers.get(&index).unwrap();
            for (id, x_other) in container {
                let distance = point_cylinder_distance(a, b, radius, x_other)?;
                if f64::abs(distance) <= self.tol_dist && filter(x_other) {
                    ids.insert(*id);
                }
            }
        }
        Ok(ids)
    }

    /// Searches points on the x-y plane (3D only)
    ///
    /// # Input
    ///
    /// * `z` -- the plane passes through `z`
    /// * `filter` -- fn(x) -> bool that returns true to **keep** the coordinate just found
    ///   (yields only the elements for which the closure returns true)
    ///
    /// # Output
    ///
    /// Returns the ids of points.
    ///
    /// # Note
    ///
    /// This works in 3D only.
    pub fn search_on_plane_xy<F>(&self, z: f64, mut filter: F) -> Result<HashSet<usize>, StrError>
    where
        F: FnMut(&[f64]) -> bool,
    {
        // check
        if self.ndim != 3 {
            return Err("search_on_plane_xy works in 3D only");
        }

        // search containers near the plane
        let nearest_containers = self.containers_near_plane(2, z);

        // search container points near the plane
        let mut ids = HashSet::new();
        for index in nearest_containers {
            let container = self.containers.get(&index).unwrap();
            for (id, x_other) in container {
                let distance = f64::abs(x_other[2] - z);
                if f64::abs(distance) <= self.tol_dist && filter(x_other) {
                    ids.insert(*id);
                }
            }
        }
        Ok(ids)
    }

    /// Searches points on the y-z plane (3D only)
    ///
    /// # Input
    ///
    /// * `x` -- the plane passes through `x`
    /// * `filter` -- fn(x) -> bool that returns true to **keep** the coordinate just found
    ///   (yields only the elements for which the closure returns true)
    ///
    /// # Output
    ///
    /// Returns the ids of points.
    ///
    /// # Note
    ///
    /// This works in 3D only.
    pub fn search_on_plane_yz<F>(&self, x: f64, mut filter: F) -> Result<HashSet<usize>, StrError>
    where
        F: FnMut(&[f64]) -> bool,
    {
        // check
        if self.ndim != 3 {
            return Err("search_on_plane_yz works in 3D only");
        }

        // search containers near the plane
        let nearest_containers = self.containers_near_plane(0, x);

        // search container points near the plane
        let mut ids = HashSet::new();
        for index in nearest_containers {
            let container = self.containers.get(&index).unwrap();
            for (id, x_other) in container {
                let distance = f64::abs(x_other[0] - x);
                if f64::abs(distance) <= self.tol_dist && filter(x_other) {
                    ids.insert(*id);
                }
            }
        }
        Ok(ids)
    }

    /// Searches points on the x-z plane (3D only)
    ///
    /// # Input
    ///
    /// * `y` -- the plane passes through `y`
    /// * `filter` -- fn(x) -> bool that returns true to **keep** the coordinate just found
    ///   (yields only the elements for which the closure returns true)
    ///
    /// # Output
    ///
    /// Returns the ids of points.
    ///
    /// # Note
    ///
    /// This works in 3D only.
    pub fn search_on_plane_xz<F>(&self, y: f64, mut filter: F) -> Result<HashSet<usize>, StrError>
    where
        F: FnMut(&[f64]) -> bool,
    {
        // check
        if self.ndim != 3 {
            return Err("search_on_plane_xz works in 3D only");
        }

        // search containers near the plane
        let nearest_containers = self.containers_near_plane(1, y);

        // search container points near the plane
        let mut ids = HashSet::new();
        for index in nearest_containers {
            let container = self.containers.get(&index).unwrap();
            for (id, x_other) in container {
                let distance = f64::abs(x_other[1] - y);
                if f64::abs(distance) <= self.tol_dist && filter(x_other) {
                    ids.insert(*id);
                }
            }
        }
        Ok(ids)
    }

    /// Draws grid and items
    pub fn draw(&self, plot: &mut Plot, with_ids: bool) -> Result<(), StrError> {
        // draw grid
        let mut xmin = vec![0.0; self.ndim];
        let mut xmax = vec![0.0; self.ndim];
        let mut ndiv = vec![0; self.ndim];
        for i in 0..self.ndim {
            xmin[i] = self.xmin[i];
            xmax[i] = self.xmax[i];
            ndiv[i] = self.ndiv[i];
        }
        let mut canvas = Canvas::new();
        canvas
            .set_alt_text_color("#5d5d5d")
            .draw_grid(&xmin, &xmax, &ndiv, false, with_ids)?;
        plot.add(&canvas);

        // draw items
        let mut curve = Curve::new();
        let mut text = Text::new();
        curve
            .set_marker_style("o")
            .set_marker_color("#fab32faa")
            .set_marker_line_color("black")
            .set_marker_line_width(0.5);
        text.set_color("#cd0000");
        for container in self.containers.values() {
            for (id, x) in container {
                let txt = format!("{}", id);
                if self.ndim == 2 {
                    curve.draw(&[x[0]], &[x[1]]);
                    if with_ids {
                        text.draw(x[0], x[1], &txt);
                    }
                } else {
                    curve.draw_3d(&[x[0]], &[x[1]], &[x[2]]);
                    if with_ids {
                        text.draw_3d(x[0], x[1], x[2], &txt);
                    }
                }
            }
        }
        plot.add(&curve).add(&text);
        Ok(())
    }

    /// Calculates the key of the container where the point should fall in
    ///
    /// **Note:** Returns None if the point is out-of-range
    #[inline]
    fn calc_container_key(&self, x: &[f64]) -> Option<usize> {
        let mut ratio = vec![0; self.ndim]; // ratio[i] = trunc(δx[i]/Δx[i]) (Eq. 8)
        let mut key = 0;
        for i in 0..self.ndim {
            if x[i] < self.xmin[i] || x[i] > self.xmax[i] {
                return None;
            }
            ratio[i] = ((x[i] - self.xmin[i]) / self.side_length) as usize;
            if ratio[i] == self.ndiv[i] {
                // the point is exactly on the max edge, thus select inner container
                ratio[i] -= 1; // move to the inside
            }
            key += ratio[i] * self.coefficient[i];
        }
        Some(key)
    }

    /// Computes the i,j,k indices of the lower-left-bottom corner of the container
    #[inline]
    fn container_pivot_indices(&self, key: usize) -> (usize, usize, usize) {
        let i = key % self.ndiv[0];
        let j = (key % self.coefficient[2]) / self.ndiv[0];
        let k = key / self.coefficient[2];
        (i, j, k)
    }

    /// Computes the center coordinates of container
    #[inline]
    fn container_center(&self, cen: &mut [f64], i: usize, j: usize, k: usize) {
        cen[0] = self.xmin[0] + (i as f64) * self.side_length + self.side_length / 2.0;
        cen[1] = self.xmin[1] + (j as f64) * self.side_length + self.side_length / 2.0;
        if self.ndim == 3 {
            cen[2] = self.xmin[2] + (k as f64) * self.side_length + self.side_length / 2.0;
        }
    }

    /// Returns the keys of (non-empty) containers near a line
    #[inline]
    fn containers_near_line(&self, a: &[f64], b: &[f64]) -> Result<Vec<usize>, StrError> {
        let mut nearest_containers = Vec::new();
        let mut cen = vec![0.0; self.ndim];
        for key in self.containers.keys() {
            // compute container center
            let (i, j, k) = self.container_pivot_indices(*key);
            self.container_center(&mut cen, i, j, k);
            // check if the center of container is near the segment
            let distance = point_line_distance(a, b, &cen)?;
            if distance <= self.radius + self.tol_dist {
                nearest_containers.push(*key);
            }
        }
        Ok(nearest_containers)
    }

    /// Returns the keys of (non-empty) containers near a circle
    #[inline]
    fn containers_near_circle(&self, circle_center: &[f64], radius: f64) -> Result<Vec<usize>, StrError> {
        let mut nearest_containers = Vec::new();
        let mut cen = vec![0.0; self.ndim];
        for key in self.containers.keys() {
            // compute container center
            let (i, j, k) = self.container_pivot_indices(*key);
            self.container_center(&mut cen, i, j, k);
            // check if the center of container is near the circle
            let distance = point_circle_distance(circle_center, radius, &cen)?;
            if distance <= self.radius + self.tol_dist {
                nearest_containers.push(*key);
            }
        }
        Ok(nearest_containers)
    }

    /// Returns the keys of (non-empty) containers near a circle
    #[inline]
    fn containers_near_cylinder(&self, a: &[f64], b: &[f64], radius: f64) -> Result<Vec<usize>, StrError> {
        let mut nearest_containers = Vec::new();
        let mut cen = vec![0.0; self.ndim];
        for key in self.containers.keys() {
            // compute container center
            let (i, j, k) = self.container_pivot_indices(*key);
            self.container_center(&mut cen, i, j, k);
            // check if the center of container is near the cylinder
            let distance = point_cylinder_distance(a, b, radius, &cen)?;
            if distance <= self.radius + self.tol_dist {
                nearest_containers.push(*key);
            }
        }
        Ok(nearest_containers)
    }

    /// Returns the keys of (non-empty) containers near plane xy
    #[inline]
    fn containers_near_plane(&self, fixed_dim: usize, fixed_coord: f64) -> Vec<usize> {
        let mut nearest_containers = Vec::new();
        let mut cen = vec![0.0; self.ndim];
        for key in self.containers.keys() {
            // compute container center
            let (i, j, k) = self.container_pivot_indices(*key);
            self.container_center(&mut cen, i, j, k);
            // check if the center of container is near the plane
            let distance = f64::abs(cen[fixed_dim] - fixed_coord);
            if distance <= self.radius + self.tol_dist {
                nearest_containers.push(*key);
            }
        }
        nearest_containers
    }

    /// Updates a container or inserts a point into an existing container
    ///
    /// Note: we need to store the coordinates for later use
    /// such as when calculating point-to-point distances
    #[inline]
    fn update_or_insert(&mut self, key: ContainerKey, id: ItemId, x: &[f64]) {
        let container = self.containers.entry(key).or_insert(HashMap::new());
        container.insert(id, x.to_vec());
    }

    /// Sets square/cubic halo around point
    #[inline]
    fn set_halo(&mut self, x: &[f64]) {
        if self.ndim == 2 {
            self.halo[0][0] = x[0] - self.tolerance;
            self.halo[0][1] = x[1] - self.tolerance;

            self.halo[1][0] = x[0] + self.tolerance;
            self.halo[1][1] = x[1] - self.tolerance;

            self.halo[2][0] = x[0] + self.tolerance;
            self.halo[2][1] = x[1] + self.tolerance;

            self.halo[3][0] = x[0] - self.tolerance;
            self.halo[3][1] = x[1] + self.tolerance;
        } else {
            self.halo[0][0] = x[0] - self.tolerance;
            self.halo[0][1] = x[1] - self.tolerance;
            self.halo[0][2] = x[2] - self.tolerance;

            self.halo[1][0] = x[0] + self.tolerance;
            self.halo[1][1] = x[1] - self.tolerance;
            self.halo[1][2] = x[2] - self.tolerance;

            self.halo[2][0] = x[0] + self.tolerance;
            self.halo[2][1] = x[1] + self.tolerance;
            self.halo[2][2] = x[2] - self.tolerance;

            self.halo[3][0] = x[0] - self.tolerance;
            self.halo[3][1] = x[1] + self.tolerance;
            self.halo[3][2] = x[2] - self.tolerance;

            self.halo[4][0] = x[0] - self.tolerance;
            self.halo[4][1] = x[1] - self.tolerance;
            self.halo[4][2] = x[2] + self.tolerance;

            self.halo[5][0] = x[0] + self.tolerance;
            self.halo[5][1] = x[1] - self.tolerance;
            self.halo[5][2] = x[2] + self.tolerance;

            self.halo[6][0] = x[0] + self.tolerance;
            self.halo[6][1] = x[1] + self.tolerance;
            self.halo[6][2] = x[2] + self.tolerance;

            self.halo[7][0] = x[0] - self.tolerance;
            self.halo[7][1] = x[1] + self.tolerance;
            self.halo[7][2] = x[2] + self.tolerance;
        }
    }
}

impl fmt::Display for GridSearch {
    /// Shows info about the items in the grid containers
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        // items
        let mut unique_items = HashSet::new();
        let mut indices: Vec<_> = self.containers.keys().collect();
        indices.sort();
        for index in indices {
            let container = self.containers.get(index).unwrap();
            let mut ids: Vec<_> = container.keys().map(|id| *id).collect();
            ids.sort();
            write!(f, "{}: {:?}\n", index, ids).unwrap();
            for id in ids {
                unique_items.insert(id);
            }
        }
        // summary
        let mut ids: Vec<_> = unique_items.iter().collect();
        ids.sort();
        write!(f, "ids = {:?}\n", ids).unwrap();
        write!(f, "nitem = {}\n", unique_items.len()).unwrap();
        write!(f, "ncontainer = {}\n", self.containers.len()).unwrap();
        write!(f, "ndiv = {:?}\n", self.ndiv).unwrap();
        Ok(())
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#[cfg(test)]
mod tests {
    use super::{any_x, GridSearch, GS_DEFAULT_TOLERANCE};
    use plotpy::Plot;
    use russell_lab::math::{SQRT_2, SQRT_3};
    use russell_lab::{approx_eq, array_approx_eq};

    #[allow(unused_imports)]
    use plotpy::{Canvas, Curve, RayEndpoint, Surface};

    const NOISE: f64 = 1.23456e-5;
    const CIRCLE: ([f64; 2], f64) = ([-0.2, 1.8], 0.45); // [xc,yc],radius
    const POINTS_2D: [[f64; 2]; 12] = [
        [-0.1, -0.1 + NOISE],
        [0.0, 0.0],
        [0.185, -0.185],
        [0.6 + NOISE, 0.0],
        [0.0, 0.3],
        [0.2, 0.5 + NOISE],
        [0.31, 0.42],
        [0.8, 1.8],
        [0.6 + NOISE, 1.5],
        [CIRCLE.0[0] + NOISE, CIRCLE.0[1] - CIRCLE.1 + NOISE],
        [CIRCLE.0[0] + CIRCLE.1 + NOISE, CIRCLE.0[1] + NOISE],
        [
            CIRCLE.0[0] + CIRCLE.1 * SQRT_2 / 2.0,
            CIRCLE.0[1] - CIRCLE.1 * SQRT_2 / 2.0,
        ],
    ];
    const LINES_2D: [[[f64; 2]; 2]; 3] = [
        [[0.6, -0.2], [0.6, 1.8]], // vertical line
        [[-0.2, 1.8], [0.8, 1.8]], // horizontal line
        [[0.4, -0.1], [0.8, 0.1]], // semi-diagonal line
    ];
    const CYLINDER: ([f64; 3], [f64; 3], f64) = ([1.0, -1.0, -1.0], [1.0, 1.0, -1.0], 0.4); // a,b,radius
    const POINTS_3D: [[f64; 3]; 8] = [
        [-1.0 + NOISE, -1.0 + NOISE, -1.0 + NOISE],
        [0.0, 0.0, 0.0],
        [1.0, 1.0 + NOISE, 1.0],
        [0.0, -0.5, -1.0 + NOISE],
        [CYLINDER.0[0] - CYLINDER.2, CYLINDER.0[1], CYLINDER.0[2] + NOISE],
        [CYLINDER.0[0], CYLINDER.0[1], CYLINDER.0[2] + CYLINDER.2 + NOISE],
        [CYLINDER.1[0] - CYLINDER.2, CYLINDER.1[1], CYLINDER.1[2] + NOISE],
        [CYLINDER.1[0], CYLINDER.1[1], CYLINDER.1[2] + CYLINDER.2 + NOISE],
    ];
    const LINES_3D: [[[f64; 3]; 2]; 2] = [
        [[-1.0, -1.0, -1.0], [1.0, -1.0, -1.0]], // line along x
        [[-1.0, -1.0, -1.0], [1.0, 1.0, 1.0]],   // diagonal
    ];

    fn add_sample_points_to_grid_2d(grid: &mut GridSearch) {
        let mut id = 100;
        for x in &POINTS_2D {
            grid.insert(id, x).unwrap();
            id += 1;
        }
    }

    fn add_sample_points_to_grid_3d(grid: &mut GridSearch) {
        let mut id = 100;
        for x in &POINTS_3D {
            grid.insert(id, x).unwrap();
            id += 1;
        }
    }

    fn sample_grid_2d() -> GridSearch {
        GridSearch::new(&[-0.2, -0.2], &[0.8, 1.8], Some(8), None, Some(0.1)).unwrap()
    }

    fn sample_grid_3d() -> GridSearch {
        GridSearch::new(&[-1.0, -1.0, -1.0], &[1.0, 1.0, 1.0], Some(2), None, Some(0.1)).unwrap()
    }

    #[test]
    fn new_handles_wrong_input() {
        assert_eq!(
            GridSearch::new(&[0.0], &[1.0, 1.0], None, None, None).err(),
            Some("xmin.len() = ndim must be 2 or 3")
        );

        assert_eq!(
            GridSearch::new(&[0.0, 0.0], &[1.0], None, None, None).err(),
            Some("xmax.len() must equal ndim = xmin.len()")
        );

        assert_eq!(
            GridSearch::new(&[0.0, 0.0], &[1.0, 1.0], None, Some(-0.1), None).err(),
            Some("tolerance must be > 0.0")
        );

        assert_eq!(
            GridSearch::new(&[0.0, 0.0], &[1.0, 1.0], None, None, Some(-0.1)).err(),
            Some("border_tol must be ≥ 0.0")
        );

        assert_eq!(
            GridSearch::new(&[0.0, 0.0], &[1.0, 1.0], Some(0), None, None).err(),
            Some("ndiv must be ≥ 1")
        );

        assert_eq!(
            GridSearch::new(&[0.0, 0.0], &[0.0, 1.0], None, None, None).err(),
            Some("xmax must be greater than xmin")
        );

        assert_eq!(
            GridSearch::new(&[0.0, 0.0, 0.0], &[1.0, 0.0, 1.0], None, None, None,).err(),
            Some("xmax must be greater than xmin")
        );

        assert_eq!(
            GridSearch::new(&[0.0, 0.0, 0.0], &[1.0, 1.0, 0.0], None, None, None,).err(),
            Some("xmax must be greater than xmin")
        );

        assert_eq!(
            GridSearch::new(
                &[0.0, 0.0],
                &[1.0, 1.0],
                Some(100),
                Some(0.5 * (1.0 / 100.0)),
                Some(0.0),
            )
            .err(),
            Some("(xmax-xmin)/ndiv must be > 2·tolerance; reduce the tolerance (or ndiv)")
        );
    }

    #[test]
    fn new_works() {
        let grid = GridSearch::new(&[-0.2, -0.2], &[0.8, 1.8], Some(20), None, None).unwrap();
        assert_eq!(grid.ndim, 2);
        assert_eq!(grid.ndiv, [10, 20]);
        approx_eq(grid.side_length, 0.102, 1e-15);
        array_approx_eq(&grid.xmin, &[-0.21, -0.21], 1e-15);
        array_approx_eq(&grid.xmax, &[0.81, -0.21 + 20.0 * 0.102], 1e-15);
        assert_eq!(grid.coefficient, &[1, 10, 10 * 20]);
        assert_eq!(grid.tolerance, GS_DEFAULT_TOLERANCE);
        approx_eq(grid.tol_dist, SQRT_2 * GS_DEFAULT_TOLERANCE, 1e-15);
        approx_eq(grid.radius, SQRT_2 * 0.102 / 2.0, 1e-15);
        assert_eq!(grid.halo.len(), 4);
        assert_eq!(grid.halo_ncorner, 4);
        assert_eq!(grid.containers.len(), 0);

        let grid = GridSearch::new(&[-0.2, -0.2], &[0.8, 1.8], Some(8), None, Some(0.1)).unwrap();
        assert_eq!(grid.ndim, 2);
        assert_eq!(grid.ndiv, [4, 8]);
        approx_eq(grid.side_length, 0.3, 1e-15);
        array_approx_eq(&grid.xmin, &[-0.3, -0.3], 1e-15);
        array_approx_eq(&grid.xmax, &[0.9, 2.1], 1e-15);
        assert_eq!(grid.coefficient, &[1, 4, 4 * 8]);
        assert_eq!(grid.tolerance, GS_DEFAULT_TOLERANCE);
        approx_eq(grid.tol_dist, SQRT_2 * GS_DEFAULT_TOLERANCE, 1e-15);
        approx_eq(grid.radius, SQRT_2 * 0.3 / 2.0, 1e-15);
        assert_eq!(grid.halo.len(), 4);
        assert_eq!(grid.halo_ncorner, 4);
        assert_eq!(grid.containers.len(), 0);

        let grid = GridSearch::new(&[-1.0, -1.0, -1.0], &[1.0, 1.0, 1.0], Some(2), None, Some(0.1)).unwrap();
        assert_eq!(grid.ndim, 3);
        assert_eq!(grid.ndiv, [2, 2, 2]);
        approx_eq(grid.side_length, 1.1, 1e-15);
        array_approx_eq(&grid.xmin, &[-1.1, -1.1, -1.1], 1e-15);
        array_approx_eq(&grid.xmax, &[1.1, 1.1, 1.1], 1e-15);
        assert_eq!(grid.coefficient, &[1, 2, 2 * 2]);
        assert_eq!(grid.tolerance, GS_DEFAULT_TOLERANCE);
        approx_eq(grid.tol_dist, SQRT_3 * GS_DEFAULT_TOLERANCE, 1e-15);
        approx_eq(grid.radius, SQRT_3 * 1.1 / 2.0, 1e-15);
        assert_eq!(grid.halo.len(), 8);
        assert_eq!(grid.halo_ncorner, 8);
        assert_eq!(grid.containers.len(), 0);
    }

    #[test]
    fn display_trait_works() {
        let grid = GridSearch::new(&[-0.2, -0.2], &[0.8, 1.8], Some(6), None, None).unwrap();
        assert_eq!(
            format!("{}", grid),
            "ids = []\n\
             nitem = 0\n\
             ncontainer = 0\n\
             ndiv = [3, 6]\n"
        );

        let grid = GridSearch::new(&[-1.0, -1.0, -1.0], &[1.0, 1.0, 1.0], Some(3), None, None).unwrap();
        assert_eq!(
            format!("{}", grid),
            "ids = []\n\
             nitem = 0\n\
             ncontainer = 0\n\
             ndiv = [3, 3, 3]\n"
        );
    }

    #[test]
    fn draw_works_2d() {
        let mut grid = sample_grid_2d();
        add_sample_points_to_grid_2d(&mut grid);
        let mut plot = Plot::new();
        grid.draw(&mut plot, true).unwrap();
        grid.draw(&mut plot, false).unwrap();

        // DO NOT DELETE THE CODE BELOW (to generate figure)
        /*
        if true {
            let h = grid.side_length / 2.0;
            let r = grid.radius;
            let mut lines = Curve::new();
            let mut canvas = Canvas::new();
            // draw rectangle representing the original limits
            canvas
                .set_face_color("#00000015")
                .set_edge_color("None")
                .draw_polyline(&[[-0.2, -0.2], [0.8, -0.2], [0.8, 1.8], [-0.2, 1.8]], true);
            // draw circle circumscribing the lower left container
            canvas
                .set_face_color("None")
                .set_edge_color("magenta")
                .draw_circle(grid.xmin[0] + h, grid.xmin[1] + h, r);
            // draw lines
            lines.set_line_color("#fab32f").set_line_width(1.5);
            for l in &LINES_2D {
                lines.draw_ray(l[0][0], l[0][1], RayEndpoint::Coords(l[1][0], l[1][1]));
            }
            // draw circle used in search
            canvas
                .set_edge_color("#05480480")
                .set_line_width(2.0)
                .draw_circle(CIRCLE.0[0], CIRCLE.0[1], CIRCLE.1);
            // setup and save figure
            plot.add(&lines).add(&canvas);
            plot.set_equal_axes(true)
                .set_ticks_x(0.1, 0.0, "")
                .set_ticks_y(0.1, 0.0, "")
                .grid_and_labels("x", "y")
                .set_figure_size_points(500.0, 1000.0);
            plot.save("/tmp/gemlab/test_plot_grid_search_2d.svg").unwrap();
        }
        */
    }

    #[test]
    fn draw_works_3d() {
        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);
        let mut plot = Plot::new();
        grid.draw(&mut plot, true).unwrap();
        grid.draw(&mut plot, false).unwrap();

        // DO NOT DELETE THE CODE BELOW (to generate figure)
        /*
        if true {
            // draw lines
            let mut canvas = Canvas::new();
            canvas.set_edge_color("#fab32f").set_line_width(1.5);
            for l in &LINES_3D {
                canvas.draw_polyline(l, false);
            }
            // draw cylinder used in search
            let (a, b, r) = CYLINDER;
            let mut surface = Surface::new();
            surface
                .set_with_surface(false)
                .set_with_wireframe(true)
                .set_line_color("#3da83b")
                .draw_cylinder(&a, &b, r, 12, 30)
                .unwrap();
            // setup and save figure
            plot.add(&canvas).add(&surface);
            plot.set_equal_axes(true)
                .set_figure_size_points(800.0, 800.0)
                .save("/tmp/gemlab/test_plot_grid_search_3d.svg")
                .unwrap();
        }
        */
    }

    #[test]
    fn set_halo_works() {
        let mut grid = sample_grid_2d();
        grid.set_halo(&[0.5, 0.5]);
        assert_eq!(grid.halo[0], [0.4999, 0.4999]);
        assert_eq!(grid.halo[1], [0.5001, 0.4999]);
        assert_eq!(grid.halo[2], [0.5001, 0.5001]);
        assert_eq!(grid.halo[3], [0.4999, 0.5001]);

        let mut grid = sample_grid_3d();
        grid.set_halo(&[0.5, 0.5, 0.5]);
        assert_eq!(grid.halo[0], [0.4999, 0.4999, 0.4999]);
        assert_eq!(grid.halo[1], [0.5001, 0.4999, 0.4999]);
        assert_eq!(grid.halo[2], [0.5001, 0.5001, 0.4999]);
        assert_eq!(grid.halo[3], [0.4999, 0.5001, 0.4999]);
        assert_eq!(grid.halo[4], [0.4999, 0.4999, 0.5001]);
        assert_eq!(grid.halo[5], [0.5001, 0.4999, 0.5001]);
        assert_eq!(grid.halo[6], [0.5001, 0.5001, 0.5001]);
        assert_eq!(grid.halo[7], [0.4999, 0.5001, 0.5001]);
    }

    #[test]
    fn calc_container_key_works() {
        let grid = sample_grid_2d();
        // outside
        assert_eq!(grid.calc_container_key(&[-10.0, 0.0]), None);
        assert_eq!(grid.calc_container_key(&[10.0, 0.0]), None);
        assert_eq!(grid.calc_container_key(&[0.0, -10.0]), None);
        assert_eq!(grid.calc_container_key(&[0.0, 10.0]), None);
        assert_eq!(grid.calc_container_key(&[-100.0, -100.0]), None);
        assert_eq!(grid.calc_container_key(&[100.0, 100.0]), None);
        // inside
        assert_eq!(grid.calc_container_key(&[0.0 - 1e-4, 0.0 - 1e-4]), Some(0));
        assert_eq!(grid.calc_container_key(&[0.1, 0.5]), Some(9));
        assert_eq!(grid.calc_container_key(&[0.7, 0.8]), Some(15));
        assert_eq!(grid.calc_container_key(&[-0.2, 1.8]), Some(24));
        assert_eq!(grid.calc_container_key(&[0.8, 1.8]), Some(27));

        let grid = sample_grid_3d();
        // outside
        assert_eq!(grid.calc_container_key(&[-10.0, -10.0, -10.0]), None);
        assert_eq!(grid.calc_container_key(&[10.0, -10.0, -10.0]), None);
        assert_eq!(grid.calc_container_key(&[-10.0, 10.0, -10.0]), None);
        assert_eq!(grid.calc_container_key(&[10.0, 10.0, -10.0]), None);
        assert_eq!(grid.calc_container_key(&[-10.0, -10.0, 10.0]), None);
        assert_eq!(grid.calc_container_key(&[10.0, -10.0, 10.0]), None);
        assert_eq!(grid.calc_container_key(&[-10.0, 10.0, 10.0]), None);
        assert_eq!(grid.calc_container_key(&[10.0, 10.0, 10.0]), None);
        // inside
        assert_eq!(grid.calc_container_key(&[-1.0, -1.0, -1.0]), Some(0));
        assert_eq!(grid.calc_container_key(&[1.0, 1.0, 1.0]), Some(7));
    }

    #[test]
    fn container_pivot_indices_works() {
        let grid = sample_grid_2d();
        assert_eq!(grid.container_pivot_indices(0), (0, 0, 0));
        assert_eq!(grid.container_pivot_indices(3), (3, 0, 0));
        assert_eq!(grid.container_pivot_indices(4), (0, 1, 0));
        assert_eq!(grid.container_pivot_indices(20), (0, 5, 0));
        assert_eq!(grid.container_pivot_indices(27), (3, 6, 0));

        let grid = sample_grid_3d();
        assert_eq!(grid.container_pivot_indices(0), (0, 0, 0));
        assert_eq!(grid.container_pivot_indices(2), (0, 1, 0));
        assert_eq!(grid.container_pivot_indices(7), (1, 1, 1));
    }

    #[test]
    fn container_center_works() {
        let grid = sample_grid_2d();
        let mut x = vec![0.0; 2];
        let (xa, ya) = (grid.xmin[0], grid.xmin[1]);
        let (xb, yb) = (grid.xmax[0], grid.xmax[1]);
        let h = grid.side_length / 2.0;
        grid.container_center(&mut x, 0, 0, 0);
        array_approx_eq(&x, &[xa + h, ya + h], 1e-15);
        grid.container_center(&mut x, grid.ndiv[0] - 1, grid.ndiv[1] - 1, 0);
        array_approx_eq(&x, &[xb - h, yb - h], 1e-15);

        let grid = sample_grid_3d();
        let mut x = vec![0.0; 3];
        let (xa, ya, za) = (grid.xmin[0], grid.xmin[1], grid.xmin[2]);
        let (xb, yb, zb) = (grid.xmax[0], grid.xmax[1], grid.xmax[2]);
        let h = grid.side_length / 2.0;
        grid.container_center(&mut x, 0, 0, 0);
        array_approx_eq(&x, &[xa + h, ya + h, za + h], 1e-15);
        grid.container_center(&mut x, grid.ndiv[0] - 1, grid.ndiv[1] - 1, grid.ndiv[2] - 1);
        array_approx_eq(&x, &[xb - h, yb - h, zb - h], 1e-15);
    }

    #[test]
    fn is_outside_works() {
        let grid = sample_grid_2d();
        assert_eq!(grid.is_outside(&[-10.0, 0.0]), true);
        assert_eq!(grid.is_outside(&[10.0, 0.0]), true);
        assert_eq!(grid.is_outside(&[0.0, 10.0]), true);
        assert_eq!(grid.is_outside(&[0.0, -10.0]), true);
        assert_eq!(grid.is_outside(&[0.0, 0.0]), false);

        let grid = sample_grid_3d();
        assert_eq!(grid.is_outside(&[-10.0, 0.0, 0.0]), true);
        assert_eq!(grid.is_outside(&[0.0, -10.0, 0.0]), true);
        assert_eq!(grid.is_outside(&[0.0, 0.0, 0.0]), false);
    }

    #[test]
    fn insert_handles_wrong_input() {
        let mut grid = sample_grid_2d();
        assert_eq!(grid.insert(0, &[0.0, 0.0, 0.0]), Err("x.len() must equal ndim"));
        assert_eq!(
            grid.insert(1000, &[10.0, 0.0]),
            Err("cannot insert point because its coordinates are outside the grid")
        );

        let mut grid = sample_grid_3d();
        assert_eq!(grid.insert(0, &[0.0, 0.0]), Err("x.len() must equal ndim"));
        assert_eq!(
            grid.insert(1000, &[10.0, 0.0, 0.0]),
            Err("cannot insert point because its coordinates are outside the grid")
        );
    }

    #[test]
    fn insert_works_2d() {
        let mut grid = sample_grid_2d();
        add_sample_points_to_grid_2d(&mut grid);
        assert_eq!(
            format!("{}", grid),
            "0: [100, 101]\n\
             1: [101, 102]\n\
             2: [103]\n\
             3: [103]\n\
             4: [101, 104]\n\
             5: [101, 104]\n\
             6: [103]\n\
             7: [103]\n\
             8: [104]\n\
             9: [104, 105]\n\
             10: [106]\n\
             20: [109]\n\
             21: [111]\n\
             22: [108]\n\
             23: [108]\n\
             25: [110]\n\
             26: [108]\n\
             27: [107, 108]\n\
             29: [110]\n\
             31: [107]\n\
             ids = [100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111]\n\
             nitem = 12\n\
             ncontainer = 20\n\
             ndiv = [4, 8]\n"
        );
    }

    #[test]
    fn insert_works_3d() {
        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);
        assert_eq!(
            format!("{}", grid),
            "0: [100, 101, 103]\n\
             1: [101, 103, 104, 105]\n\
             2: [101]\n\
             3: [101, 106, 107]\n\
             4: [101]\n\
             5: [101]\n\
             6: [101]\n\
             7: [101, 102]\n\
             ids = [100, 101, 102, 103, 104, 105, 106, 107]\n\
             nitem = 8\n\
             ncontainer = 8\n\
             ndiv = [2, 2, 2]\n"
        );
    }

    #[test]
    fn search_handles_wrong_input() {
        let grid = sample_grid_2d();
        assert_eq!(grid.search(&[0.0, 0.0, 0.0]), Err("x.len() must equal ndim"));
        assert_eq!(
            grid.search(&[10.0, 0.0]),
            Err("cannot find point because the coordinates are outside the grid")
        );

        let grid = sample_grid_3d();
        assert_eq!(grid.search(&[0.0, 0.0]), Err("x.len() must equal ndim"));
        assert_eq!(
            grid.search(&[10.0, 0.0, 0.0]),
            Err("cannot find point because the coordinates are outside the grid")
        );
    }

    #[test]
    fn search_works() {
        const NOISE: f64 = 1e-4 / 2.0;

        let mut grid = sample_grid_2d();
        add_sample_points_to_grid_2d(&mut grid);
        let mut id = 100;
        for x in &POINTS_2D {
            let mut y = x.clone();
            y[0] += NOISE;
            y[1] -= NOISE;
            assert_eq!(grid.search(&y).unwrap(), Some(id));
            id += 1;
        }
        assert_eq!(grid.search(&[-0.2, 0.7]).unwrap(), None);

        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);
        let mut id = 100;
        for x in &POINTS_3D {
            let mut y = x.clone();
            y[0] += NOISE;
            y[1] -= NOISE;
            y[2] += NOISE;
            assert_eq!(grid.search(&y).unwrap(), Some(id));
            id += 1;
        }
        assert_eq!(grid.search(&[-0.9, 0.9, 0.9]).unwrap(), None);
    }

    #[test]
    fn containers_near_line_works_2d() {
        let mut grid = sample_grid_2d();
        add_sample_points_to_grid_2d(&mut grid);

        // vertical line
        let mut indices = grid.containers_near_line(&LINES_2D[0][0], &LINES_2D[0][1]).unwrap();
        indices.sort();
        assert_eq!(indices, &[2, 3, 6, 7, 10, 22, 23, 26, 27, 31]);

        // horizontal line
        let mut indices = grid.containers_near_line(&LINES_2D[1][0], &LINES_2D[1][1]).unwrap();
        indices.sort();
        assert_eq!(indices, &[25, 26, 27, 29, 31]);

        // semi-diagonal line
        let mut indices = grid.containers_near_line(&LINES_2D[2][0], &LINES_2D[2][1]).unwrap();
        indices.sort();
        assert_eq!(indices, &[0, 1, 2, 3, 6, 7]);
    }

    #[test]
    fn containers_near_line_works_3d() {
        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);

        // line parallel to x
        let mut indices = grid.containers_near_line(&LINES_3D[0][0], &LINES_3D[0][1]).unwrap();
        indices.sort();
        assert_eq!(indices, &[0, 1]);

        // diagonal line
        let mut indices = grid.containers_near_line(&LINES_3D[1][0], &LINES_3D[1][1]).unwrap();
        indices.sort();
        assert_eq!(indices, &[0, 1, 2, 3, 4, 5, 6, 7]);
    }

    #[test]
    fn search_on_line_handles_wrong_input() {
        assert_eq!(any_x(&vec![]), true);
        let grid = sample_grid_2d();
        assert_eq!(
            grid.search_on_line(&[0.0], &[1.0, 1.0], any_x),
            Err("a.len() must equal ndim")
        );
        assert_eq!(
            grid.search_on_line(&[0.0, 0.0], &[1.0], any_x),
            Err("b.len() must equal ndim")
        );
    }

    #[test]
    fn search_on_line_works_2d() {
        let mut grid = sample_grid_2d();
        add_sample_points_to_grid_2d(&mut grid);

        // vertical line (without filter)
        let res = grid.search_on_line(&LINES_2D[0][0], &LINES_2D[0][1], any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [103, 108]);

        // vertical line (with filter)
        let res = grid
            .search_on_line(&LINES_2D[0][0], &LINES_2D[0][1], |x| x[1] > 0.0)
            .unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [108]);

        // horizontal line (without filter)
        let res = grid.search_on_line(&LINES_2D[1][0], &LINES_2D[1][1], any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [107, 110]);

        // horizontal line (with filter)
        let res = grid
            .search_on_line(&LINES_2D[1][0], &LINES_2D[1][1], |x| x[0] < 0.8)
            .unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [110]);

        // semi-diagonal line
        let res = grid.search_on_line(&LINES_2D[2][0], &LINES_2D[2][1], any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [103]);
    }

    #[test]
    fn search_on_line_works_3d() {
        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);

        // line parallel to x (without filter)
        let res = grid.search_on_line(&LINES_3D[0][0], &LINES_3D[0][1], any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [100, 104]);

        // line parallel to x (with filter)
        let res = grid
            .search_on_line(&LINES_3D[0][0], &LINES_3D[0][1], |x| x[0] < 0.0)
            .unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [100]);

        // diagonal (without filter)
        let res = grid.search_on_line(&LINES_3D[1][0], &LINES_3D[1][1], any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [100, 101, 102]);

        // diagonal (with filter)
        let res = grid
            .search_on_line(&LINES_3D[1][0], &LINES_3D[1][1], |x| x[2] <= 0.0)
            .unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [100, 101]);
    }

    #[test]
    fn containers_near_circle_works() {
        let mut grid = sample_grid_2d();
        add_sample_points_to_grid_2d(&mut grid);

        let mut indices = grid.containers_near_circle(&CIRCLE.0, CIRCLE.1).unwrap();
        indices.sort();
        assert_eq!(indices, &[20, 21, 25, 29]);
    }

    #[test]
    fn search_on_circle_fails_on_wrong_input() {
        let grid = sample_grid_2d();
        assert_eq!(
            grid.search_on_circle(&[-0.2], 0.3, any_x),
            Err("center.len() must equal ndim")
        );

        let grid = sample_grid_3d();
        assert_eq!(
            grid.search_on_circle(&[0.0, 0.0, 0.0], 1.0, any_x),
            Err("search_on_circle works in 2D only")
        );
    }

    #[test]
    fn search_on_circle_works() {
        let mut grid = sample_grid_2d();
        add_sample_points_to_grid_2d(&mut grid);

        let res = grid.search_on_circle(&CIRCLE.0, CIRCLE.1, any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [109, 110, 111]);

        let res = grid
            .search_on_circle(&CIRCLE.0, CIRCLE.1, |x| x[0] < 0.0 || x[0] > 0.2)
            .unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [109, 110]);
    }

    #[test]
    fn containers_near_cylinder_works() {
        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);

        let mut indices = grid
            .containers_near_cylinder(&CYLINDER.0, &CYLINDER.1, CYLINDER.2)
            .unwrap();
        indices.sort();
        assert_eq!(indices, &[1, 3]);
    }

    #[test]
    fn search_on_cylinder_fails_on_wrong_input() {
        let grid = sample_grid_3d();
        assert_eq!(
            grid.search_on_cylinder(&[0.0, 0.0], &[1.0, 0.0, 0.0], 1.0, any_x),
            Err("a.len() must equal ndim")
        );
        assert_eq!(
            grid.search_on_cylinder(&[0.0, 0.0, 0.0], &[1.0, 0.0], 1.0, any_x),
            Err("b.len() must equal ndim")
        );

        let grid = sample_grid_2d();
        assert_eq!(
            grid.search_on_cylinder(&[0.0, 0.0, 0.0], &[1.0, 0.0, 0.0], 1.0, any_x),
            Err("search_on_cylinder works in 3D only")
        );
    }

    #[test]
    fn search_on_cylinder_works() {
        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);

        let res = grid
            .search_on_cylinder(&CYLINDER.0, &CYLINDER.1, CYLINDER.2, any_x)
            .unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [104, 105, 106, 107]);

        let res = grid
            .search_on_cylinder(&CYLINDER.0, &CYLINDER.1, CYLINDER.2, |x| x[1] > 0.0)
            .unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [106, 107]);
    }

    #[test]
    fn containers_near_plane_works() {
        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);

        let mut indices = grid.containers_near_plane(0, -1.0);
        indices.sort();
        assert_eq!(indices, &[0, 2, 4, 6]);

        let mut indices = grid.containers_near_plane(0, 1.0);
        indices.sort();
        assert_eq!(indices, &[1, 3, 5, 7]);

        let mut indices = grid.containers_near_plane(1, -1.0);
        indices.sort();
        assert_eq!(indices, &[0, 1, 4, 5]);

        let mut indices = grid.containers_near_plane(1, 1.0);
        indices.sort();
        assert_eq!(indices, &[2, 3, 6, 7]);

        let mut indices = grid.containers_near_plane(2, -1.0);
        indices.sort();
        assert_eq!(indices, &[0, 1, 2, 3]);

        let mut indices = grid.containers_near_plane(2, 1.0);
        indices.sort();
        assert_eq!(indices, &[4, 5, 6, 7]);

        let mut indices = grid.containers_near_plane(2, 0.0);
        indices.sort();
        assert_eq!(indices, &[0, 1, 2, 3, 4, 5, 6, 7]);
    }

    #[test]
    fn search_on_plane_fails_on_wrong_input() {
        let grid = sample_grid_2d();
        assert_eq!(
            grid.search_on_plane_xy(-1.0, any_x),
            Err("search_on_plane_xy works in 3D only")
        );
        assert_eq!(
            grid.search_on_plane_yz(-1.0, any_x),
            Err("search_on_plane_yz works in 3D only")
        );
        assert_eq!(
            grid.search_on_plane_xz(-1.0, any_x),
            Err("search_on_plane_xz works in 3D only")
        );
    }

    #[test]
    fn search_on_plane_works() {
        let mut grid = sample_grid_3d();
        add_sample_points_to_grid_3d(&mut grid);

        // xy ------------

        let res = grid.search_on_plane_xy(-1.0, any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [100, 103, 104, 106]);

        let res = grid.search_on_plane_xy(-1.0, |x| x[1] < 0.0).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [100, 103, 104]);

        // yz ------------

        let res = grid.search_on_plane_yz(-1.0, any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [100]);

        let res = grid.search_on_plane_yz(-1.0, |x| x[2] > 1.0).unwrap();
        assert_eq!(res.len(), 0);

        // xz ------------

        let res = grid.search_on_plane_xz(-1.0, any_x).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [100, 104, 105]);

        let res = grid.search_on_plane_xz(-1.0, |x| x[0] > 0.0).unwrap();
        let mut ids: Vec<_> = res.iter().copied().collect();
        ids.sort();
        assert_eq!(ids, [104, 105]);
    }
}