wbtopology 0.1.2

//! Fast Delaunay triangulation utilities.
//!
//! This module implements a fast sweep-hull style triangulator adapted from
//! the Delaunator family of algorithms.
//!
//! Attribution:
//! - Mapbox Delaunator (JavaScript): https://github.com/mapbox/delaunator
//! - mourner/delaunator-rs (Rust port): https://github.com/mourner/delaunator-rs
//!
//! The implementation here stays intentionally close to the upstream crate and
//! only adapts the public entrypoint to return
//! [`crate::triangulation::DelaunayTriangulation`].

use std::cmp::Ordering;

use crate::geom::Coord;
use crate::triangulation::DelaunayTriangulation;

const EMPTY: usize = usize::MAX;
const DEFAULT_EPSILON: f64 = f64::EPSILON * 2.0;

#[derive(Clone, Copy, PartialEq, Default)]
struct Point2 {
    x: f64,
    y: f64,
}

impl From<&Point2> for robust::Coord<f64> {
    fn from(point: &Point2) -> robust::Coord<f64> {
        robust::Coord::<f64> {
            x: point.x,
            y: point.y,
        }
    }
}

impl Point2 {
    #[inline]
    fn dist2(&self, other: &Self) -> f64 {
        let dx = self.x - other.x;
        let dy = self.y - other.y;
        dx * dx + dy * dy
    }

    #[inline]
    fn orient(&self, q: &Self, r: &Self) -> f64 {
        robust::orient2d(self.into(), q.into(), r.into())
    }

    #[inline]
    fn circumdelta(&self, b: &Self, c: &Self) -> (f64, f64) {
        let dx = b.x - self.x;
        let dy = b.y - self.y;
        let ex = c.x - self.x;
        let ey = c.y - self.y;

        let bl = dx * dx + dy * dy;
        let cl = ex * ex + ey * ey;
        let d = 0.5 / (dx * ey - dy * ex);

        let x = (ey * bl - dy * cl) * d;
        let y = (dx * cl - ex * bl) * d;
        (x, y)
    }

    #[inline]
    fn circumradius2(&self, b: &Self, c: &Self) -> f64 {
        let (x, y) = self.circumdelta(b, c);
        x * x + y * y
    }

    #[inline]
    fn circumcenter(&self, b: &Self, c: &Self) -> Self {
        let (x, y) = self.circumdelta(b, c);
        Self {
            x: self.x + x,
            y: self.y + y,
        }
    }

    #[inline]
    fn in_circle(&self, b: &Self, c: &Self, p: &Self) -> bool {
        let dx = self.x - p.x;
        let dy = self.y - p.y;
        let ex = b.x - p.x;
        let ey = b.y - p.y;
        let fx = c.x - p.x;
        let fy = c.y - p.y;

        let ap = dx * dx + dy * dy;
        let bp = ex * ex + ey * ey;
        let cp = fx * fx + fy * fy;

        dx * (ey * cp - bp * fy) - dy * (ex * cp - bp * fx) + ap * (ex * fy - ey * fx) < 0.0
    }

    #[inline]
    fn nearly_equals(&self, p: &Self, epsilon: f64) -> bool {
        let eps = if epsilon.is_finite() {
            epsilon.abs().max(DEFAULT_EPSILON)
        } else {
            DEFAULT_EPSILON
        };
        f64_abs(self.x - p.x) <= eps && f64_abs(self.y - p.y) <= eps
    }
}

#[inline]
fn next_halfedge(i: usize) -> usize {
    if i % 3 == 2 {
        i - 2
    } else {
        i + 1
    }
}

#[inline]
fn prev_halfedge(i: usize) -> usize {
    if i % 3 == 0 {
        i + 2
    } else {
        i - 1
    }
}

#[derive(Debug, Clone)]
struct FastTriangulation {
    triangles: Vec<usize>,
    halfedges: Vec<usize>,
    hull: Vec<usize>,
}

impl FastTriangulation {
    fn new(n: usize) -> Self {
        let max_triangles = if n > 2 { 2 * n - 5 } else { 0 };
        Self {
            triangles: Vec::with_capacity(max_triangles * 3),
            halfedges: Vec::with_capacity(max_triangles * 3),
            hull: Vec::new(),
        }
    }

    fn add_triangle(&mut self, i0: usize, i1: usize, i2: usize, a: usize, b: usize, c: usize) -> usize {
        let t = self.triangles.len();

        self.triangles.push(i0);
        self.triangles.push(i1);
        self.triangles.push(i2);

        self.halfedges.push(a);
        self.halfedges.push(b);
        self.halfedges.push(c);

        if a != EMPTY {
            self.halfedges[a] = t;
        }
        if b != EMPTY {
            self.halfedges[b] = t + 1;
        }
        if c != EMPTY {
            self.halfedges[c] = t + 2;
        }

        t
    }

    fn legalize(&mut self, a: usize, points: &[Point2], hull: &mut Hull) -> usize {
        let b = self.halfedges[a];
        let ar = prev_halfedge(a);

        if b == EMPTY {
            return ar;
        }

        let al = next_halfedge(a);
        let bl = prev_halfedge(b);

        let p0 = self.triangles[ar];
        let pr = self.triangles[a];
        let pl = self.triangles[al];
        let p1 = self.triangles[bl];

        if points[p0].in_circle(&points[pr], &points[pl], &points[p1]) {
            self.triangles[a] = p1;
            self.triangles[b] = p0;

            let hbl = self.halfedges[bl];
            let har = self.halfedges[ar];

            if hbl == EMPTY {
                let mut e = hull.start;
                loop {
                    if hull.tri[e] == bl {
                        hull.tri[e] = a;
                        break;
                    }
                    e = hull.prev[e];
                    if e == hull.start {
                        break;
                    }
                }
            }

            self.halfedges[a] = hbl;
            self.halfedges[b] = har;
            self.halfedges[ar] = bl;

            if hbl != EMPTY {
                self.halfedges[hbl] = a;
            }
            if har != EMPTY {
                self.halfedges[har] = b;
            }
            if bl != EMPTY {
                self.halfedges[bl] = ar;
            }

            let br = next_halfedge(b);
            self.legalize(a, points, hull);
            return self.legalize(br, points, hull);
        }

        ar
    }
}

struct Hull {
    prev: Vec<usize>,
    next: Vec<usize>,
    tri: Vec<usize>,
    hash: Vec<usize>,
    start: usize,
    center: Point2,
}

impl Hull {
    fn new(n: usize, center: Point2, i0: usize, i1: usize, i2: usize, points: &[Point2]) -> Self {
        let hash_len = f64_sqrt(n as f64) as usize;

        let mut hull = Self {
            prev: vec![0; n],
            next: vec![0; n],
            tri: vec![0; n],
            hash: vec![EMPTY; hash_len.max(1)],
            start: i0,
            center,
        };

        hull.next[i0] = i1;
        hull.prev[i2] = i1;
        hull.next[i1] = i2;
        hull.prev[i0] = i2;
        hull.next[i2] = i0;
        hull.prev[i1] = i0;

        hull.tri[i0] = 0;
        hull.tri[i1] = 1;
        hull.tri[i2] = 2;

        hull.hash_edge(&points[i0], i0);
        hull.hash_edge(&points[i1], i1);
        hull.hash_edge(&points[i2], i2);

        hull
    }

    fn hash_key(&self, p: &Point2) -> usize {
        let dx = p.x - self.center.x;
        let dy = p.y - self.center.y;

        let q = dx / (f64_abs(dx) + f64_abs(dy));
        let a = (if dy > 0.0 { 3.0 - q } else { 1.0 + q }) / 4.0;

        let len = self.hash.len();
        (f64_floor((len as f64) * a) as usize) % len
    }

    fn hash_edge(&mut self, p: &Point2, i: usize) {
        let key = self.hash_key(p);
        self.hash[key] = i;
    }

    fn find_visible_edge(&self, p: &Point2, points: &[Point2]) -> (usize, bool) {
        let mut start = 0usize;
        let key = self.hash_key(p);
        let len = self.hash.len();

        for j in 0..len {
            start = self.hash[(key + j) % len];
            if start != EMPTY && self.next[start] != EMPTY {
                break;
            }
        }

        if start == EMPTY {
            return (EMPTY, false);
        }

        start = self.prev[start];
        let mut e = start;
        while p.orient(&points[e], &points[self.next[e]]) <= 0.0 {
            e = self.next[e];
            if e == start {
                return (EMPTY, false);
            }
        }

        (e, e == start)
    }
}

fn calc_bbox_center(points: &[Point2]) -> Point2 {
    let mut min_x = f64::INFINITY;
    let mut min_y = f64::INFINITY;
    let mut max_x = f64::NEG_INFINITY;
    let mut max_y = f64::NEG_INFINITY;

    for p in points {
        min_x = min_x.min(p.x);
        min_y = min_y.min(p.y);
        max_x = max_x.max(p.x);
        max_y = max_y.max(p.y);
    }

    Point2 {
        x: (min_x + max_x) / 2.0,
        y: (min_y + max_y) / 2.0,
    }
}

fn find_closest_point(points: &[Point2], p0: &Point2) -> Option<usize> {
    let mut min_dist = f64::INFINITY;
    let mut index = 0usize;

    for (i, p) in points.iter().enumerate() {
        let d = p0.dist2(p);
        if d > 0.0 && d < min_dist {
            index = i;
            min_dist = d;
        }
    }

    if min_dist == f64::INFINITY {
        None
    } else {
        Some(index)
    }
}

fn find_seed_triangle(points: &[Point2]) -> Option<(usize, usize, usize)> {
    let bbox_center = calc_bbox_center(points);
    let i0 = find_closest_point(points, &bbox_center)?;
    let p0 = &points[i0];

    let i1 = find_closest_point(points, p0)?;
    let p1 = &points[i1];

    let mut min_radius = f64::INFINITY;
    let mut i2 = 0usize;
    for (i, p) in points.iter().enumerate() {
        if i == i0 || i == i1 {
            continue;
        }
        let r = p0.circumradius2(p1, p);
        if r < min_radius {
            i2 = i;
            min_radius = r;
        }
    }

    if min_radius == f64::INFINITY {
        None
    } else {
        Some(if p0.orient(p1, &points[i2]) > 0.0 {
            (i0, i2, i1)
        } else {
            (i0, i1, i2)
        })
    }
}

fn sortf(values: &mut [(usize, f64)]) {
    values.sort_unstable_by(|&(_, da), &(_, db)| da.partial_cmp(&db).unwrap_or(Ordering::Equal));
}

fn handle_collinear_points(points: &[Point2]) -> FastTriangulation {
    let Point2 { x, y } = points.first().copied().unwrap_or_default();

    let mut dist: Vec<_> = points
        .iter()
        .enumerate()
        .map(|(i, p)| {
            let mut d = p.x - x;
            if d == 0.0 {
                d = p.y - y;
            }
            (i, d)
        })
        .collect();
    sortf(&mut dist);

    let mut triangulation = FastTriangulation::new(0);
    let mut d0 = f64::NEG_INFINITY;
    for (i, distance) in dist {
        if distance > d0 {
            triangulation.hull.push(i);
            d0 = distance;
        }
    }

    triangulation
}

fn triangulate_fast_indices(points: &[Point2], epsilon: f64) -> FastTriangulation {
    let Some((i0, i1, i2)) = find_seed_triangle(points) else {
        return handle_collinear_points(points);
    };

    let n = points.len();
    let center = points[i0].circumcenter(&points[i1], &points[i2]);

    let mut triangulation = FastTriangulation::new(n);
    triangulation.add_triangle(i0, i1, i2, EMPTY, EMPTY, EMPTY);

    let mut dists: Vec<_> = points
        .iter()
        .enumerate()
        .map(|(i, point)| (i, point.dist2(&center)))
        .collect();
    sortf(&mut dists);

    let mut hull = Hull::new(n, center, i0, i1, i2, points);

    for (k, &(i, _)) in dists.iter().enumerate() {
        let p = &points[i];

        if k > 0 && p.nearly_equals(&points[dists[k - 1].0], epsilon) {
            continue;
        }
        if i == i0 || i == i1 || i == i2 {
            continue;
        }

        let (mut e, walk_back) = hull.find_visible_edge(p, points);
        if e == EMPTY {
            continue;
        }

        let t = triangulation.add_triangle(e, i, hull.next[e], EMPTY, EMPTY, hull.tri[e]);
        hull.tri[i] = triangulation.legalize(t + 2, points, &mut hull);
        hull.tri[e] = t;

        let mut n = hull.next[e];
        loop {
            let q = hull.next[n];
            if p.orient(&points[n], &points[q]) <= 0.0 {
                break;
            }
            let t = triangulation.add_triangle(n, i, q, hull.tri[i], EMPTY, hull.tri[n]);
            hull.tri[i] = triangulation.legalize(t + 2, points, &mut hull);
            hull.next[n] = EMPTY;
            n = q;
        }

        if walk_back {
            loop {
                let q = hull.prev[e];
                if p.orient(&points[q], &points[e]) <= 0.0 {
                    break;
                }
                let t = triangulation.add_triangle(q, i, e, EMPTY, hull.tri[e], hull.tri[q]);
                triangulation.legalize(t + 2, points, &mut hull);
                hull.tri[q] = t;
                hull.next[e] = EMPTY;
                e = q;
            }
        }

        hull.prev[i] = e;
        hull.next[i] = n;
        hull.prev[n] = i;
        hull.next[e] = i;
        hull.start = e;

        hull.hash_edge(p, i);
        hull.hash_edge(&points[e], e);
    }

    let mut e = hull.start;
    loop {
        triangulation.hull.push(e);
        e = hull.next[e];
        if e == hull.start {
            break;
        }
    }

    triangulation.triangles.shrink_to_fit();
    triangulation.halfedges.shrink_to_fit();
    triangulation
}

/// Build a fast 2D Delaunay triangulation using a sweep-hull strategy.
///
/// This routine is intended for high-throughput workflows (for example large
/// LiDAR TIN gridding). It keeps the `Coord.z` value from the input points and
/// returns triangle indices into the returned point array.
pub fn delaunay_triangulation_fast(points: &[Coord], epsilon: f64) -> DelaunayTriangulation {
    let filtered: Vec<Coord> = points
        .iter()
        .copied()
        .filter(|p| p.x.is_finite() && p.y.is_finite())
        .collect();

    if filtered.len() < 3 {
        return DelaunayTriangulation {
            points: filtered,
            triangles: vec![],
        };
    }

    let points2d: Vec<Point2> = filtered
        .iter()
        .map(|p| Point2 { x: p.x, y: p.y })
        .collect();
    let work = triangulate_fast_indices(&points2d, epsilon);

    let mut triangles = Vec::with_capacity(work.triangles.len() / 3);
    for tri in work.triangles.chunks_exact(3) {
        triangles.push([tri[0], tri[1], tri[2]]);
    }

    DelaunayTriangulation {
        points: filtered,
        triangles,
    }
}

#[inline]
fn f64_abs(value: f64) -> f64 {
    value.abs()
}

#[inline]
fn f64_floor(value: f64) -> f64 {
    value.floor()
}

#[inline]
fn f64_sqrt(value: f64) -> f64 {
    value.sqrt()
}