spade 2.15.0

use core::cell::RefCell;

use crate::{
    delaunay_core::math,
    handles::{FixedDirectedEdgeHandle, FixedVertexHandle},
    DelaunayTriangulation, HasPosition, HintGenerator, Point2, PositionInTriangulation,
    Triangulation,
};
use num_traits::{one, zero, Float};

use alloc::vec::Vec;

use super::VertexHandle;

/// Implements methods for natural neighbor interpolation.
///
/// Natural neighbor interpolation is a spatial interpolation method. For a given set of 2D input points with an
/// associated value (e.g. a height field of some terrain or temperature measurements at different locations in
/// a country), natural neighbor interpolation allows to smoothly interpolate the associated value for every
/// location within the convex hull of the input points.
///
/// Spade currently assists with 4 interpolation strategies:
///  - **Nearest neighbor interpolation:** Fastest. Exhibits too poor quality for many tasks. Not continuous
///    along the edges of the voronoi diagram. Use [DelaunayTriangulation::nearest_neighbor].
///  - **Barycentric interpolation:** Fast. Not smooth on the edges of the Delaunay triangulation.
///    See [Barycentric].
///  - **Natural neighbor interpolation:** Slower. Smooth everywhere except the input points. The input points
///    have no derivative. See [NaturalNeighbor::interpolate]
///  - **Natural neighbor interpolation with gradients:** Slowest. Smooth everywhere, even at the input points.
///    See [NaturalNeighbor::interpolate_gradient].
///
/// # Performance comparison
///
/// In general, the speed of interpolating random points in a triangulation will be determined by the speed of the
/// lookup operation after a certain triangulation size is reached. Thus, if random sampling is required, using a
/// [crate::HierarchyHintGenerator] may be beneficial.
///
/// If subsequent queries are close to each other, [crate::LastUsedVertexHintGenerator] should also work fine.
/// In this case, interpolating a single value should result in the following (relative) run times:
///
/// | nearest neighbor | barycentric | natural neighbor (no gradients) | natural neighbor (gradients) |
/// | ---------------- | ----------- | ------------------------------- | ---------------------------- |
/// | 23ns             | 103ns       | 307ns                           | 422ns                        |
///
/// # Usage
///
/// This type is created by calling [DelaunayTriangulation::natural_neighbor]. It contains a few internal buffers
/// that are used to prevent recurring allocations. For best performance it should be created only once per thread
/// and then used in all interpolation activities (see example).
///
/// # Example
/// ```
/// use spade::{Point2, HasPosition, DelaunayTriangulation, InsertionError, Triangulation as _};
///
/// struct PointWithHeight {
///     position: Point2<f64>,
///     height: f64,
/// }
///
/// impl HasPosition for PointWithHeight {
///     type Scalar = f64;
///
///     fn position(&self) -> Point2<f64> {
///         self.position
///     }
/// }
///
/// fn main() -> Result<(), InsertionError> {
///     let mut t = DelaunayTriangulation::<PointWithHeight>::new();
///     t.insert(PointWithHeight { position: Point2::new(-1.0, -1.0), height: 42.0})?;
///     t.insert(PointWithHeight { position: Point2::new(-1.0, 1.0), height: 13.37})?;
///     t.insert(PointWithHeight { position: Point2::new(1.0, -1.0), height: 27.18})?;
///     t.insert(PointWithHeight { position: Point2::new(1.0, 1.0), height: 31.41})?;
///
///     // Set of query points (would be many more realistically):
///     let query_points = [Point2::new(0.0, 0.1), Point2::new(0.5, 0.1)];
///
///     // Good: Re-use interpolation object!
///     let nn = t.natural_neighbor();
///     for p in &query_points {
///         println!("Value at {:?}: {:?}", p, nn.interpolate(|v| v.data().height, *p));
///     }
///
///     // Bad (slower): Don't re-use internal buffers
///     for p in &query_points {
///         println!("Value at {:?}: {:?}", p, t.natural_neighbor().interpolate(|v| v.data().height, *p));
///     }
///     Ok(())
/// }
/// ```
///
/// # Visual comparison of interpolation algorithms
///
/// *Note: All of these images are generated by the "interpolation" example*
///
/// Nearest neighbor interpolation exhibits discontinuities along the voronoi edges:
///
#[doc = include_str!("../../images/interpolation_nearest_neighbor.img")]
///
/// Barycentric interpolation, by contrast, is continuous everywhere but has no derivative on the edges of the
/// Delaunay triangulation. These show up as sharp corners in the color gradients on the drawn edges:
///
#[doc = include_str!("../../images/interpolation_barycentric.img")]
///
/// By contrast, natural neighbor interpolation is smooth on the edges - the previously sharp angles are now rounded
/// off. However, the vertices themselves are still not continuous and will form sharp "peaks" in the resulting
/// surface.
///
#[doc = include_str!("../../images/interpolation_nn_c0.img")]
///
/// With a gradient, the sharp peaks are gone - the surface will smoothly approximate a linear function as defined
/// by the gradient in the vicinity of each vertex. The gradients are estimated with the estimate_gradients function.
/// With flatness = 0.5
#[doc = include_str!("../../images/interpolation_nn_c1_flatness_05.img")]
/// With flatness = 1.0
#[doc = include_str!("../../images/interpolation_nn_c1_flatness_1.img")]
///
#[doc(alias = "Interpolation")]
pub struct NaturalNeighbor<'a, T>
where
    T: Triangulation,
    T::Vertex: HasPosition,
{
    triangulation: &'a T,
    // Various buffers that are used for identifying natural neighbors and their weights. It's significantly faster
    // to clear and re-use these buffers instead of allocating them anew.
    // We also don't run the risk of mutably borrowing them twice (causing a RefCell panic) as the RefCells never leak
    // any of the interpolation methods.
    // Not implementing `Sync` is also fine as the workaround - creating a new `NaturalNeighbor` instance per thread -
    // is very simple.
    inspect_edges_buffer: RefCell<Vec<FixedDirectedEdgeHandle>>,
    natural_neighbor_buffer: RefCell<Vec<FixedDirectedEdgeHandle>>,
    insert_cell_buffer: RefCell<Vec<Point2<<T::Vertex as HasPosition>::Scalar>>>,
    weight_buffer: RefCell<Vec<(FixedVertexHandle, <T::Vertex as HasPosition>::Scalar)>>,
}

/// Implements methods related to barycentric interpolation.
///
/// Created by calling [crate::FloatTriangulation::barycentric].
///
/// Refer to the documentation of [NaturalNeighbor] for an overview of different interpolation methods.
#[doc(alias = "Interpolation")]
pub struct Barycentric<'a, T>
where
    T: Triangulation,
{
    triangulation: &'a T,
    weight_buffer: RefCell<Vec<(FixedVertexHandle, <T::Vertex as HasPosition>::Scalar)>>,
}

impl<'a, T> Barycentric<'a, T>
where
    T: Triangulation,
    <T::Vertex as HasPosition>::Scalar: Float,
{
    pub(crate) fn new(triangulation: &'a T) -> Self {
        Self {
            triangulation,
            weight_buffer: Default::default(),
        }
    }

    /// Returns the barycentric coordinates and the respective vertices for a given query position.
    ///
    /// The resulting coordinates and vertices are stored within the given `result` `vec`` to prevent
    /// unneeded allocations. `result` will be cleared initially.
    ///
    /// The number of returned elements depends on the query positions location:
    ///  - `result` will be **empty** if the query position lies outside the triangulation's convex hull
    ///  - `result` will contain **a single element** (with weight 1.0) if the query position lies exactly on a vertex
    ///  - `result` will contain **two vertices** if the query point lies exactly on any edge of the triangulation.
    ///  - `result` will contain **exactly three** elements if the query point lies on an inner face of the
    ///    triangulation.
    pub fn get_weights(
        &self,
        position: Point2<<T::Vertex as HasPosition>::Scalar>,
        result: &mut Vec<(FixedVertexHandle, <T::Vertex as HasPosition>::Scalar)>,
    ) {
        result.clear();
        match self.triangulation.locate(position) {
            PositionInTriangulation::OnVertex(vertex) => {
                result.push((vertex, <T::Vertex as HasPosition>::Scalar::from(1.0)))
            }
            PositionInTriangulation::OnEdge(edge) => {
                let [v0, v1] = self.triangulation.directed_edge(edge).vertices();
                let [w0, w1] = two_point_interpolation::<T>(v0, v1, position);
                result.push((v0.fix(), w0));
                result.push((v1.fix(), w1));
            }
            PositionInTriangulation::OnFace(face) => {
                let face = self.triangulation.face(face);
                let [v0, v1, v2] = face.vertices();
                let [c0, c1, c2] = face.barycentric_interpolation(position);
                result.extend([(v0.fix(), c0), (v1.fix(), c1), (v2.fix(), c2)]);
            }
            _ => {}
        }
    }

    /// Performs barycentric interpolation on this triangulation at a given position.
    ///
    /// Returns `None` for any value outside the triangulation's convex hull.
    /// The value to interpolate is given by the `i` parameter.
    ///
    /// Refer to [NaturalNeighbor] for a comparison with other interpolation methods.
    pub fn interpolate<I>(
        &self,
        i: I,
        position: Point2<<T::Vertex as HasPosition>::Scalar>,
    ) -> Option<<T::Vertex as HasPosition>::Scalar>
    where
        I: Fn(
            VertexHandle<T::Vertex, T::DirectedEdge, T::UndirectedEdge, T::Face>,
        ) -> <T::Vertex as HasPosition>::Scalar,
    {
        let nns = &mut *self.weight_buffer.borrow_mut();
        self.get_weights(position, nns);
        if nns.is_empty() {
            return None;
        }

        let mut total_sum = zero();
        for (vertex, weight) in nns {
            total_sum = total_sum + i(self.triangulation.vertex(*vertex)) * *weight;
        }
        Some(total_sum)
    }
}

impl<'a, V, DE, UE, F, L> NaturalNeighbor<'a, DelaunayTriangulation<V, DE, UE, F, L>>
where
    V: HasPosition,
    DE: Default,
    UE: Default,
    F: Default,
    L: HintGenerator<<V as HasPosition>::Scalar>,
    <V as HasPosition>::Scalar: Float,
{
    pub(crate) fn new(triangulation: &'a DelaunayTriangulation<V, DE, UE, F, L>) -> Self {
        Self {
            triangulation,
            inspect_edges_buffer: Default::default(),
            insert_cell_buffer: Default::default(),
            natural_neighbor_buffer: Default::default(),
            weight_buffer: Default::default(),
        }
    }

    /// Calculates the natural neighbors and their weights (sibson coordinates) of a given query position.
    ///
    /// The neighbors are returned in clockwise order. The weights will add up to 1.0.
    /// The neighbors are stored in the `result` parameter to prevent unnecessary allocations.
    /// `result` will be cleared initially.
    ///
    /// The number of returned natural neighbors depends on the given query position:
    /// - `result` will be **empty** if the query position lies outside the triangulation's convex hull
    /// - `result` will contain **exactly one** vertex if the query position is equal to that vertex position.
    /// - `result` will contain **exactly two** entries if the query position lies exactly *on* an edge of the
    ///    convex hull.
    /// - `result` will contain **at least three** `(vertex, weight)` tuples if the query point lies on an inner
    ///    face or an inner edge.
    ///
    /// *Example: The natural neighbors (red vertices) of the query point (blue dot) with their weights.
    /// The elements will be returned in clockwise order as indicated by the indices drawn within the red circles.*
    ///
    #[doc = include_str!("../../images/natural_neighbor_scenario.svg")]
    pub fn get_weights(
        &self,
        position: Point2<<V as HasPosition>::Scalar>,
        result: &mut Vec<(FixedVertexHandle, <V as HasPosition>::Scalar)>,
    ) {
        let nns = &mut *self.natural_neighbor_buffer.borrow_mut();
        get_natural_neighbor_edges(
            self.triangulation,
            &mut self.inspect_edges_buffer.borrow_mut(),
            position,
            nns,
        );
        self.get_natural_neighbor_weights(position, nns, result);
    }

    /// Interpolates a value at a given position.
    ///
    /// Returns `None` for any point outside the triangulations convex hull.
    /// The value to interpolate is given by the `i` parameter. The resulting interpolation will be smooth
    /// everywhere except at the input vertices.
    ///
    /// Refer to [NaturalNeighbor] for an example on how to use this function.
    pub fn interpolate<I>(
        &self,
        i: I,
        position: Point2<<V as HasPosition>::Scalar>,
    ) -> Option<<V as HasPosition>::Scalar>
    where
        I: Fn(VertexHandle<V, DE, UE, F>) -> <V as HasPosition>::Scalar,
    {
        let nns = &mut *self.weight_buffer.borrow_mut();
        self.get_weights(position, nns);
        if nns.is_empty() {
            return None;
        }

        let mut total_sum = zero();
        for (vertex, weight) in nns {
            total_sum = total_sum + i(self.triangulation.vertex(*vertex)) * *weight;
        }
        Some(total_sum)
    }

    /// Interpolates a value at a given position.
    ///
    /// In contrast to [Self::interpolate], this method has a well-defined derivative at each vertex and will
    /// approximate a linear function in the proximity of any vertex.
    ///
    /// The value to interpolate is given by the `i` parameter. The gradient that defines the derivative at
    /// each input vertex is given by the `g` parameter.
    ///
    /// The `flatness` parameter (should be >= 0.0) blends between an interpolation that adheres less to the gradient (small values)
    /// or adheres to it strongly (values larger than ~2.0) in the vicinity of any vertex. A value of 0.5 reproduces
    /// the original Sibson C1 interpolant. A value of 1.0 is the fastest and works well in many situations.
    /// A value of 0.0 reproduces the I1 interpolant introduced in Flötotto's thesis chapter 4.4.
    /// If both the flatness and the gradients are 0.0 Sibsons C0 interpolant is recreated and [Self::interpolate] should be used.
    ///
    /// Returns `None` for any point outside the triangulation's convex hull.
    ///
    /// Refer to [NaturalNeighbor] for more information and a visual example.
    ///
    /// # Example 1
    ///
    /// ```
    /// use spade::{DelaunayTriangulation, HasPosition, Point2};
    /// # use spade::Triangulation;
    ///
    /// struct PointWithData {
    ///     position: Point2<f64>,
    ///     height: f64,
    ///     grad: [f64; 2],
    /// }
    ///
    /// impl HasPosition for PointWithData {
    ///     type Scalar = f64;
    ///     fn position(&self) -> Point2<f64> { self.position }
    /// }
    ///
    /// let mut triangulation: DelaunayTriangulation<PointWithData> = Default::default();
    /// // Insert some points into the triangulation
    /// triangulation.insert(PointWithData { position: Point2::new(10.0, 10.0), height: 0.0, grad: [-1., -1.] });
    /// triangulation.insert(PointWithData { position: Point2::new(10.0, -10.0), height: 0.0, grad: [-1., 1.] });
    /// triangulation.insert(PointWithData { position: Point2::new(-10.0, 10.0), height: 0.0, grad: [1., -1.] });
    /// triangulation.insert(PointWithData { position: Point2::new(-10.0, -10.0), height: 0.0, grad: [1., 1.] });
    ///
    /// let nn = triangulation.natural_neighbor();
    ///
    /// // Interpolate point at coordinates (1.0, 2.0). This example uses a known gradients at each vertex.
    /// // The gradient is the direction of steepest ascent for the function at that point
    /// let query_point = Point2::new(1.0, 2.0);
    /// let value: f64 = nn.interpolate_gradient(|v| v.data().height, |v| v.data().grad, 1.0, query_point).unwrap();
    /// ```
    ///
    /// /// # Example 2
    ///
    /// ```
    /// use spade::{DelaunayTriangulation, HasPosition, Point2};
    /// # use spade::Triangulation;
    ///
    /// struct PointWithHeight {
    ///     position: Point2<f64>,
    ///     height: f64,
    /// }
    ///
    /// impl HasPosition for PointWithHeight {
    ///     type Scalar = f64;
    ///     fn position(&self) -> Point2<f64> { self.position }
    /// }
    ///
    /// let mut triangulation: DelaunayTriangulation<PointWithHeight> = Default::default();
    /// // Insert some points into the triangulation
    /// triangulation.insert(PointWithHeight { position: Point2::new(10.0, 10.0), height: 0.0 });
    /// triangulation.insert(PointWithHeight { position: Point2::new(10.0, -10.0), height: 0.0 });
    /// triangulation.insert(PointWithHeight { position: Point2::new(-10.0, 10.0), height: 0.0 });
    /// triangulation.insert(PointWithHeight { position: Point2::new(-10.0, -10.0), height: 0.0 });
    ///
    /// let nn = triangulation.natural_neighbor();
    ///
    /// // Interpolate point at coordinates (1.0, 2.0).
    /// let query_point = Point2::new(1.0, 2.0);
    /// // Let's interpolate the value with estimated gradients. The gradient is the direction of steepest ascent for the function at that point
    /// let value: f64 = nn.interpolate_gradient(|v| v.data().height, |v| nn.estimate_gradient(v, |h| h.data().height), 1.0, query_point).unwrap();
    ///
    /// // could also have gone and calculated all grads upfront like this, notice the s in gradients
    /// let grads = nn.estimate_gradients(|v| v.data().height);
    /// let value: f64 = nn.interpolate_gradient(|v| v.data().height, &grads, 1.0, query_point).unwrap();
    /// ```
    ///
    /// # References
    ///
    /// This method uses the C1 extension proposed by Sibson in
    /// "A brief description of natural neighbor interpolation, R. Sibson, 1981"
    ///
    /// It is also worth looking at Julia Flötotto's thesis
    /// A Coordinate System associated to a Point Cloud issued from a Manifold: Definition, Properties and Applications
    /// chapter 4.1 and 4.4
    pub fn interpolate_gradient<I, G>(
        &self,
        i: I,
        g: G,
        flatness: <V as HasPosition>::Scalar,
        position: Point2<<V as HasPosition>::Scalar>,
    ) -> Option<<V as HasPosition>::Scalar>
    where
        I: Fn(VertexHandle<V, DE, UE, F>) -> <V as HasPosition>::Scalar,
        G: Fn(VertexHandle<V, DE, UE, F>) -> [<V as HasPosition>::Scalar; 2],
    {
        let nns = &mut *self.weight_buffer.borrow_mut();
        self.get_weights(position, nns);
        if nns.is_empty() {
            return None;
        }

        // Variable names should make more sense after looking into the paper!
        // Roughly speaking, this approach works by blending a smooth c1 approximation into the
        // regular natural neighbor interpolation ("c0 contribution").
        // The c0 / c1 contributions are stored in sum_c0 / sum_c1 and are weighted by alpha and beta
        // respectively.
        let mut sum_c0 = zero();
        let mut sum_c1 = zero();
        let mut sum_c1_weights = zero();
        let mut alpha: <V as HasPosition>::Scalar = zero();
        let mut beta: <V as HasPosition>::Scalar = zero();

        for (handle, weight) in nns {
            let handle = self.triangulation.vertex(*handle);
            let pos_i = handle.position();
            let h_i = i(handle);
            let diff = position.sub(pos_i);
            let r_i2 = diff.length2();

            if r_i2 == zero() {
                return Some(h_i);
            }

            let r_i = r_i2.powf(flatness);
            let c1_weight_i = *weight / r_i;
            let grad_i = g(handle);
            let zeta_i = h_i + diff.dot(grad_i.into());

            alpha = alpha + c1_weight_i * r_i2;
            beta = beta + *weight * r_i2;
            sum_c1_weights = sum_c1_weights + c1_weight_i;
            sum_c1 = sum_c1 + zeta_i * c1_weight_i;
            sum_c0 = sum_c0 + h_i * *weight;
        }
        alpha = alpha / sum_c1_weights;
        sum_c1 = sum_c1 / sum_c1_weights;
        let result = (alpha * sum_c0 + beta * sum_c1) / (alpha + beta);

        Some(result)
    }

    /// Calculates the natural neighbor weights corresponding to a given position.
    ///
    /// The weight of a natural neighbor n is defined as the size of the intersection of two areas:
    ///  - The existing voronoi cell of the natural neighbor
    ///  - The cell that would be created if a vertex was created at the given position.
    ///
    /// The area of this intersection can, surprisingly, be calculated without doing the actual insertion.
    ///
    /// Parameters:
    ///  - position refers to the position for which the weights should be calculated
    ///  - nns: A list of edges that connect the natural neighbors (e.g. `nns[0].to() == nns[1].from()`).
    ///  - result: Stores the resulting NNs (as vertex handle) and their weights.
    ///
    /// # Visualization
    ///
    /// Refer to these two .svg files for more detail on how the algorithm works, either by looking them up
    /// directly or by running `cargo doc --document-private-items` and looking at the documentation of this
    /// function.
    ///
    /// ## Insertion cell
    ///
    /// This .svg displays the *insertion cell* (thick orange line) which is the voronoi cell that gets
    /// created if the query point (red dot) would be inserted.
    /// Each point of the insertion cell lies on a circumcenter of a triangle formed by `position` and two
    /// adjacent natural neighbors (red dots with their index shown inside).
    #[doc = include_str!("../../images/natural_neighbor_insertion_cell.svg")]
    ///
    /// ## Inner loop
    ///
    /// This .svg illustrates the inner loop of the algorithm (see code below). The goal is to calculate
    /// the weight of natural neighbor with index 4 which is proportional to the area of the orange polygon.
    /// `last_edge`, `stop_edge`, `first`, `c0`, `c1` `c2` and `last` refer to variable names (see code below).
    #[doc = include_str!("../../images/natural_neighbor_polygon.svg")]
    fn get_natural_neighbor_weights(
        &self,
        position: Point2<<V as HasPosition>::Scalar>,
        nns: &[FixedDirectedEdgeHandle],
        result: &mut Vec<(FixedVertexHandle, <V as HasPosition>::Scalar)>,
    ) {
        result.clear();

        if nns.is_empty() {
            return;
        }

        if nns.len() == 1 {
            let edge = self.triangulation.directed_edge(nns[0]);
            result.push((edge.from().fix(), one()));
            return;
        }

        if nns.len() == 2 {
            let [e0, e1] = [
                self.triangulation.directed_edge(nns[0]),
                self.triangulation.directed_edge(nns[1]),
            ];
            let [v0, v1] = [e0.from(), e1.from()];
            let [w0, w1] =
                two_point_interpolation::<DelaunayTriangulation<V, DE, UE, F>>(v0, v1, position);

            result.push((v0.fix(), w0));
            result.push((v1.fix(), w1));
            return;
        }

        // Get insertion cell vertices. The "insertion cell" refers to the voronoi cell that would be
        // created if "position" would be inserted.
        // These insertion cells happen to lie on the circumcenter of `position` and any two adjacent
        // natural neighbors (e.g. [position, nn[2].position(), nn[3].position()]).
        //
        // `images/natural_neighbor_insertion_cell.svg` depicts the cell as thick orange line.
        let mut insertion_cell = self.insert_cell_buffer.borrow_mut();
        insertion_cell.clear();
        for cur_nn in nns {
            let cur_nn = self.triangulation.directed_edge(*cur_nn);

            let [from, to] = cur_nn.positions();
            insertion_cell.push(
                math::circumcenter([
                    to.sub(position),
                    from.sub(position),
                    Point2::new(zero(), zero()),
                ])
                .0,
            );
        }

        let mut total_area = zero(); // Used to normalize weights at the end

        let mut last_edge = self.triangulation.directed_edge(*nns.last().unwrap());
        let mut last = *insertion_cell.last().unwrap();

        for (stop_edge, first) in core::iter::zip(nns.iter(), &*insertion_cell) {
            // Main loop
            //
            // Refer to images/natural_neighbor_polygon.svg for some visual aid.
            //
            // The outer loops calculates the weight of an individual natural neighbor.
            // To do this, it calculates the intersection area of the insertion cell with the cell of the
            // current natural neighbor.
            // This intersection is a convex polygon with vertices `first, current0, current1 ... last`
            // (ordered ccw, `currentX` refers to the variable in the inner loop)
            //
            // The area of a convex polygon [v0 ... vN] is given by
            // 0.5 * ((v0.x * v1.y + ... vN.x * v0.y) - (v0.y * v1.x + ... vN.y * v0.x))
            //        ⮤       positive_area        ⮥   ⮤         negative_area      ⮥
            //
            // The positive and negative contributions are calculated separately to avoid precision issues.
            // The factor of 0.5 can be omitted as the weights are normalized anyway.

            // `stop_edge` is used to know when to stop the inner loop (= once the polygon is finished)
            let stop_edge = self.triangulation.directed_edge(*stop_edge);
            assert!(!stop_edge.is_outer_edge());

            let mut positive_area = first.x * last.y;
            let mut negative_area = first.y * last.x;

            loop {
                // All other polygon vertices happen lie on the circumcenter of a face adjacent to an
                // out edge of the current natural neighbor.
                //
                // The natural_neighbor_polygon.svg refers to this variable as `c0`, `c1`, and `c2`.
                let vertices = last_edge
                    .face()
                    .as_inner()
                    .unwrap()
                    .positions()
                    .map(|p| p.sub(position));

                let current = math::circumcenter(vertices).0;

                positive_area = positive_area + last.x * current.y;
                negative_area = negative_area + last.y * current.x;

                last_edge = last_edge.next().rev();
                last = current;

                if last_edge == stop_edge.rev() {
                    positive_area = positive_area + current.x * first.y;
                    negative_area = negative_area + current.y * first.x;
                    break;
                }
            }

            let polygon_area = positive_area - negative_area;

            total_area = total_area + polygon_area;
            result.push((stop_edge.from().fix(), polygon_area));

            last = *first;
            last_edge = stop_edge;
        }

        for tuple in result {
            tuple.1 = tuple.1 / total_area;
        }
    }

    /// Estimates and returns the gradient for a every vertex in this triangulation.
    ///
    /// This assumes that the triangulation models some kind of height field.
    /// The gradient is calculated from the weighted and normalized average of the normals of all triangles
    /// adjacent to the given vertex (weighted by triangle size).
    ///
    /// NOTE: This is not Sibson's gradient estimator
    pub fn estimate_gradients<I>(
        &self,
        i: I,
    ) -> impl Fn(VertexHandle<V, DE, UE, F>) -> [<V as HasPosition>::Scalar; 2]
    where
        I: Fn(VertexHandle<V, DE, UE, F>) -> <V as HasPosition>::Scalar,
    {
        let grads = self
            .triangulation
            .vertices()
            .map(|v| self.estimate_gradient(v, &i))
            .collect::<Vec<_>>();

        move |v: VertexHandle<V, DE, UE, F>| grads[v.index()]
    }

    /// Estimates and returns the gradient for a single vertex in this triangulation.
    ///
    /// This assumes that the triangulation models some kind of height field.
    /// The gradient is calculated from the weighted and normalized average of the normals of all triangles
    /// adjacent to the given vertex (weighted by triangle size).
    ///
    /// NOTE: This is not Sibson's gradient estimator
    pub fn estimate_gradient<I>(
        &self,
        v: VertexHandle<V, DE, UE, F>,
        i: I,
    ) -> [<V as HasPosition>::Scalar; 2]
    where
        I: Fn(VertexHandle<V, DE, UE, F>) -> <V as HasPosition>::Scalar,
    {
        let v_2d = v.position();
        let v_pos = [v_2d.x, v_2d.y, i(v)];

        let neighbor_positions = {
            v.out_edges()
                .map(|e| {
                    let pos = e.to().position();
                    [pos.x, pos.y, i(e.to())]
                })
                .collect::<Vec<_>>()
        };
        let mut final_normal: [<V as HasPosition>::Scalar; 3] = [zero(); 3];
        for index in 0..neighbor_positions.len() {
            let p0 = neighbor_positions[index];
            let p1 = neighbor_positions[(index + 1) % neighbor_positions.len()];

            let d0 = [p0[0] - v_pos[0], p0[1] - v_pos[1], p0[2] - v_pos[2]];
            let d1 = [p1[0] - v_pos[0], p1[1] - v_pos[1], p1[2] - v_pos[2]];

            let normal = [
                d0[1] * d1[2] - d0[2] * d1[1],
                d0[2] * d1[0] - d0[0] * d1[2],
                d0[0] * d1[1] - d0[1] * d1[0],
            ];

            // this should be true for every normal
            // given that the height field assumption holds
            if normal[2] > zero() {
                final_normal = [
                    final_normal[0] + normal[0],
                    final_normal[1] + normal[1],
                    final_normal[2] + normal[2],
                ];
            }
        }

        // Calculate gradient from normal
        if final_normal[2] != zero() {
            [
                -final_normal[0] / final_normal[2],
                -final_normal[1] / final_normal[2],
            ]
        } else {
            [zero(), zero()]
        }
    }
}

fn get_natural_neighbor_edges<T>(
    triangulation: &T,
    inspect_buffer: &mut Vec<FixedDirectedEdgeHandle>,
    position: Point2<<T::Vertex as HasPosition>::Scalar>,
    result: &mut Vec<FixedDirectedEdgeHandle>,
) where
    T: Triangulation,
    <T::Vertex as HasPosition>::Scalar: Float,
{
    inspect_buffer.clear();
    result.clear();
    match triangulation.locate(position) {
        PositionInTriangulation::OnFace(face) => {
            for edge in triangulation
                .face(face)
                .adjacent_edges()
                .into_iter()
                .rev()
                .map(|e| e.rev())
            {
                inspect_flips(triangulation, result, inspect_buffer, edge.fix(), position);
            }
        }
        PositionInTriangulation::OnEdge(edge) => {
            let edge = triangulation.directed_edge(edge);

            if edge.is_part_of_convex_hull() {
                result.extend([edge.fix(), edge.fix().rev()]);
                return;
            }

            for edge in [edge, edge.rev()] {
                inspect_flips(triangulation, result, inspect_buffer, edge.fix(), position);
            }
        }
        PositionInTriangulation::OnVertex(fixed_handle) => {
            let vertex = triangulation.vertex(fixed_handle);
            result.push(
                vertex
                    .out_edge()
                    .map(|e| e.fix())
                    .unwrap_or(FixedDirectedEdgeHandle::new(0)),
            )
        }
        _ => {}
    }
    result.reverse();
}

/// Identifies natural neighbors.
///
/// To do so, this function "simulates" the insertion of a vertex (located at `position) and keeps track of which
/// edges would need to be flipped. A vertex is a natural neighbor if it happens to be part of an edge that would
/// require to be flipped.
///
/// Similar to function `legalize_edge` (which is used for *actual* insertions).
fn inspect_flips<T>(
    triangulation: &T,
    result: &mut Vec<FixedDirectedEdgeHandle>,
    buffer: &mut Vec<FixedDirectedEdgeHandle>,
    edge_to_validate: FixedDirectedEdgeHandle,
    position: Point2<<T::Vertex as HasPosition>::Scalar>,
) where
    T: Triangulation,
{
    buffer.clear();
    buffer.push(edge_to_validate);

    while let Some(edge) = buffer.pop() {
        let edge = triangulation.directed_edge(edge);

        let v2 = edge.opposite_vertex();
        let v1 = edge.from();

        let mut should_flip = false;

        if let Some(v2) = v2 {
            let v0 = edge.to().position();
            let v1 = v1.position();
            let v3 = position;
            debug_assert!(math::is_ordered_ccw(v2.position(), v1, v0));
            should_flip = math::contained_in_circumference(v2.position(), v1, v0, v3);

            if should_flip {
                let e1 = edge.next().fix().rev();
                let e2 = edge.prev().fix().rev();

                buffer.push(e1);
                buffer.push(e2);
            }
        }

        if !should_flip {
            result.push(edge.fix().rev());
        }
    }
}

fn two_point_interpolation<'a, T>(
    v0: VertexHandle<'a, T::Vertex, T::DirectedEdge, T::UndirectedEdge, T::Face>,
    v1: VertexHandle<'a, T::Vertex, T::DirectedEdge, T::UndirectedEdge, T::Face>,
    position: Point2<<T::Vertex as HasPosition>::Scalar>,
) -> [<T::Vertex as HasPosition>::Scalar; 2]
where
    T: Triangulation,
    <T::Vertex as HasPosition>::Scalar: Float,
{
    let projection = math::project_point(v0.position(), v1.position(), position);
    let rel = projection.relative_position();
    let one: <T::Vertex as HasPosition>::Scalar = 1.0.into();
    [one - rel, rel]
}

#[cfg(test)]
mod test {
    use approx::assert_ulps_eq;

    use crate::test_utilities::{random_points_in_range, random_points_with_seed, SEED, SEED2};
    use crate::{DelaunayTriangulation, HasPosition, InsertionError, Point2, Triangulation};
    use alloc::vec;
    use alloc::vec::Vec;

    struct PointWithHeight {
        position: Point2<f64>,
        height: f64,
    }

    impl PointWithHeight {
        fn new(position: Point2<f64>, height: f64) -> Self {
            Self { position, height }
        }
    }

    impl HasPosition for PointWithHeight {
        type Scalar = f64;

        fn position(&self) -> Point2<Self::Scalar> {
            self.position
        }
    }

    #[test]
    fn test_natural_neighbors() -> Result<(), InsertionError> {
        let vertices = random_points_with_seed(50, SEED);
        let t = DelaunayTriangulation::<_>::bulk_load(vertices)?;

        let mut nn_edges = Vec::new();

        let mut buffer = Vec::new();
        let query_point = Point2::new(0.5, 0.2);
        super::get_natural_neighbor_edges(&t, &mut buffer, query_point, &mut nn_edges);

        assert!(nn_edges.len() >= 3);

        for edge in &nn_edges {
            let edge = t.directed_edge(*edge);
            assert!(edge.side_query(query_point).is_on_left_side());
        }

        let mut nns = Vec::new();
        let natural_neighbor = &t.natural_neighbor();
        for v in t.vertices() {
            natural_neighbor.get_weights(v.position(), &mut nns);
            let expected = vec![(v.fix(), 1.0f64)];
            assert_eq!(nns, expected);
        }

        Ok(())
    }

    #[test]
    fn test_small_interpolation() -> Result<(), InsertionError> {
        let mut t = DelaunayTriangulation::<_>::new();
        // Create symmetric triangle - the weights should be mirrored
        t.insert(PointWithHeight::new(Point2::new(0.0, 2.0f64.sqrt()), 0.0))?;
        let v1 = t.insert(PointWithHeight::new(Point2::new(-1.0, -1.0), 0.0))?;
        let v2 = t.insert(PointWithHeight::new(Point2::new(1.0, -1.0), 0.0))?;

        let mut result = Vec::new();
        t.natural_neighbor()
            .get_weights(Point2::new(0.0, 0.0), &mut result);

        assert_eq!(result.len(), 3);
        let mut v1_weight = None;
        let mut v2_weight = None;
        for (v, weight) in result {
            if v == v1 {
                v1_weight = Some(weight);
            }
            if v == v2 {
                v2_weight = Some(weight);
            } else {
                assert!(weight > 0.0);
            }
        }

        assert!(v1_weight.is_some());
        assert!(v2_weight.is_some());
        assert_ulps_eq!(v1_weight.unwrap(), v2_weight.unwrap());

        Ok(())
    }

    #[test]
    fn test_quad_interpolation() -> Result<(), InsertionError> {
        let mut t = DelaunayTriangulation::<_>::new();
        // Insert points into the corners of the unit square. The weights at the origin should then
        // all be 0.25
        t.insert(Point2::new(1.0, 1.0))?;
        t.insert(Point2::new(1.0, -1.0))?;
        t.insert(Point2::new(-1.0, 1.0))?;
        t.insert(Point2::new(-1.0, -1.0))?;

        let mut result = Vec::new();
        t.natural_neighbor()
            .get_weights(Point2::new(0.0, 0.0), &mut result);

        assert_eq!(result.len(), 4);
        for (_, weight) in result {
            assert_ulps_eq!(weight, 0.25);
        }

        Ok(())
    }

    #[test]
    fn test_constant_height_interpolation() -> Result<(), InsertionError> {
        let mut t = DelaunayTriangulation::<_>::new();
        let vertices = random_points_with_seed(50, SEED);
        let fixed_height = 1.5;
        for v in vertices {
            t.insert(PointWithHeight::new(v, fixed_height))?;
        }

        for v in random_points_in_range(1.5, 50, SEED2) {
            let value = t.natural_neighbor().interpolate(|p| p.data().height, v);
            if let Some(value) = value {
                assert_ulps_eq!(value, 1.5);
            }
        }

        Ok(())
    }

    #[test]
    fn test_slope_interpolation() -> Result<(), InsertionError> {
        // Insert vertices v with
        // v.x in [-1.0 .. 1.0]
        // and
        // v.height = v.x .
        //
        // The expected side profile of the triangulation (YZ plane through y = 0.0) looks like this:
        //
        //
        //  ↑           /⇖x = 1, height = 1
        //  height     /
        //            /
        //  x→       / <---- x = -1, height = -1
        let mut t = DelaunayTriangulation::<_>::new();
        let grid_size = 32;
        let scale = 1.0 / grid_size as f64;
        for x in -grid_size..=grid_size {
            for y in -grid_size..=grid_size {
                let coords = Point2::new(x as f64, y as f64).mul(scale);
                t.insert(PointWithHeight::new(coords, coords.x))?;
            }
        }
        let query_points = random_points_with_seed(50, SEED);

        let check = |point, expected| {
            let value = t
                .natural_neighbor()
                .interpolate(|v| v.data().height, point)
                .unwrap();
            assert_ulps_eq!(value, expected, epsilon = 0.001);
        };

        // Check inside points
        for point in query_points {
            check(point, point.x);
        }

        // Check positions on the boundary
        check(Point2::new(-1.0, 0.0), -1.0);
        check(Point2::new(-1.0, 0.2), -1.0);
        check(Point2::new(-1.0, 0.5), -1.0);
        check(Point2::new(-1.0, -1.0), -1.0);
        check(Point2::new(1.0, 0.0), 1.0);
        check(Point2::new(1.0, 0.2), 1.0);
        check(Point2::new(1.0, 0.5), 1.0);
        check(Point2::new(1.0, -1.0), 1.0);

        for x in [-1.0, -0.8, -0.5, 0.0, 0.3, 0.7, 1.0] {
            check(Point2::new(x, 1.0), x);
            check(Point2::new(x, -1.0), x);
        }

        let expect_none = |point| {
            assert!(t
                .natural_neighbor()
                .interpolate(|v| v.data().height, point)
                .is_none());
        };

        // Check positions outside the triangulation.
        for x in [-5.0f64, -4.0, 3.0, 2.0] {
            expect_none(Point2::new(x, 0.5));
            expect_none(Point2::new(x, -0.5));
            expect_none(Point2::new(x, 0.1));
        }

        Ok(())
    }

    #[test]
    fn test_parabola_interpolation() -> Result<(), InsertionError> {
        // Insert vertices into a regular grid and assign a height of r*r (r = distance from origin).
        let mut t = DelaunayTriangulation::<_>::new();
        let grid_size = 32;
        let scale = 1.0 / grid_size as f64;
        for x in -grid_size..=grid_size {
            for y in -grid_size..=grid_size {
                let coords = Point2::new(x as f64, y as f64).mul(scale);
                t.insert(PointWithHeight::new(coords, coords.length2()))?;
            }
        }
        let query_points = random_points_with_seed(50, SEED);

        for point in query_points {
            let value = t
                .natural_neighbor()
                .interpolate(|v| v.data().height, point)
                .unwrap();
            let r2 = point.length2();
            assert_ulps_eq!(value, r2, epsilon = 0.001);
        }

        Ok(())
    }

    #[test]
    fn test_nan_issue() -> Result<(), InsertionError> {
        let vertices = vec![
            Point2 {
                x: 2273118.4147972693,
                y: 6205168.575915335,
            },
            Point2 {
                x: 2273118.2119929367,
                y: 6205168.6854697745,
            },
            Point2 {
                x: 2273118.1835989403,
                y: 6205168.653506873,
            },
            Point2 {
                x: 2273118.2647345643,
                y: 6205169.082690307,
            },
            Point2 {
                x: 2273118.105938253,
                y: 6205168.163882839,
            },
            Point2 {
                x: 2273117.7264146665,
                y: 6205168.718028998,
            },
            Point2 {
                x: 2273118.0946678743,
                y: 6205169.148656867,
            },
            Point2 {
                x: 2273118.3779900977,
                y: 6205168.1644559605,
            },
            Point2 {
                x: 2273117.71105166,
                y: 6205168.459413756,
            },
            Point2 {
                x: 2273118.30732556,
                y: 6205169.130634655,
            },
        ];

        let delaunay = DelaunayTriangulation::<_>::bulk_load(vertices)?;

        let nns = delaunay.natural_neighbor();
        let result: f64 = nns
            .interpolate(|_| 1.0, Point2::new(2273118.2, 6205168.666666667))
            .unwrap();

        assert!(!result.is_nan());
        Ok(())
    }

    #[test]
    fn test_gradient_estimation_planar() -> Result<(), InsertionError> {
        let points = vec![
            PointWithHeight::new(Point2::new(0., 0.), 0.),
            PointWithHeight::new(Point2::new(1., 0.), 0.),
            PointWithHeight::new(Point2::new(1., 1.), 1.),
            PointWithHeight::new(Point2::new(0., 1.), 1.),
            PointWithHeight::new(Point2::new(0.5, 0.5), 0.5),
        ];

        let t = DelaunayTriangulation::<_>::bulk_load_stable(points).unwrap();
        let nn = t.natural_neighbor();

        let v = nn
            .triangulation
            .locate_vertex(Point2::new(0.5, 0.5))
            .unwrap();

        let grad = nn.estimate_gradient(v, |v| v.data().height);

        assert!(grad == [0., 1.0]);

        Ok(())
    }

    #[test]
    fn test_gradient_estimation_flat() -> Result<(), InsertionError> {
        let points = vec![
            PointWithHeight::new(Point2::new(0., 0.), 1.),
            PointWithHeight::new(Point2::new(1., 0.), 1.),
            PointWithHeight::new(Point2::new(1., 1.), 1.),
            PointWithHeight::new(Point2::new(0., 1.), 1.),
            PointWithHeight::new(Point2::new(0.5, 0.5), 0.),
        ];

        let t = DelaunayTriangulation::<_>::bulk_load_stable(points).unwrap();
        let nn = t.natural_neighbor();

        let v = nn
            .triangulation
            .locate_vertex(Point2::new(0.5, 0.5))
            .unwrap();

        let grad = nn.estimate_gradient(v, |v| v.data().height);

        assert!(grad == [0., 0.]);

        Ok(())
    }
}