//! Distances between two triangle meshes: the nearest pair of points, and every pair of
//! triangles within a distance. A [`TriangleSet`] keeps a mesh's triangles under a bounding
//! volume hierarchy, so a query costs about the pairs of triangles that come near each other
//! rather than the product of the triangle counts.

use crate::compare::closest_on_triangle;
use crate::{Aabb3, Bvh, Mesh, Vec3};

/// A mesh's triangles with their B-rep faces, indexed for distance queries.
#[derive(Clone, Debug, Default)]
pub struct TriangleSet {
    tris: Vec<[Vec3; 3]>,
    face: Vec<u32>,
    bvh: Bvh,
    bounds: Aabb3,
}

/// The nearest points of two triangle sets, and the triangles they lie on.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct MeshClosest {
    pub distance: f64,
    pub a: Vec3,
    pub b: Vec3,
    pub tri_a: u32,
    pub tri_b: u32,
}

impl TriangleSet {
    /// The set of `mesh`'s triangles (ones with a vertex that is not finite are left out).
    pub fn new(mesh: &Mesh) -> TriangleSet {
        let mut tris = Vec::with_capacity(mesh.triangles.len());
        let mut face = Vec::with_capacity(mesh.triangles.len());
        let mut boxes = Vec::with_capacity(mesh.triangles.len());
        let mut bounds = Aabb3::EMPTY;
        for (k, t) in mesh.triangles.iter().enumerate() {
            let Some(p) = mesh.tri(t) else { continue };
            if !p.iter().all(|v| v.is_finite()) {
                continue;
            }
            let mut b = Aabb3::EMPTY;
            for v in p {
                b.add(v);
            }
            bounds = bounds.union(&b);
            boxes.push(b);
            tris.push(p);
            face.push(mesh.tri_face.get(k).copied().unwrap_or(0));
        }
        TriangleSet { bvh: Bvh::new(&boxes), tris, face, bounds }
    }

    /// The box around every triangle.
    pub fn bounds(&self) -> Aabb3 {
        self.bounds
    }

    pub fn len(&self) -> usize {
        self.tris.len()
    }

    pub fn is_empty(&self) -> bool {
        self.tris.is_empty()
    }

    /// Triangle `t`'s corners.
    pub fn triangle(&self, t: u32) -> Option<[Vec3; 3]> {
        self.tris.get(t as usize).copied()
    }

    /// The B-rep face triangle `t` came from.
    pub fn face(&self, t: u32) -> Option<u32> {
        self.face.get(t as usize).copied()
    }

    /// The nearest points of this set and `other`, if they come within `limit` of each other
    /// (`f64::INFINITY`: always). Triangles that cross count as distance 0, at a point where
    /// they meet.
    pub fn closest(&self, other: &TriangleSet, limit: f64) -> Option<MeshClosest> {
        let mut best: Option<MeshClosest> = None;
        let bound = std::cell::Cell::new(if limit.is_nan() { f64::INFINITY } else { limit });
        let mut found = |i: u32, j: u32| {
            let (Some(x), Some(y)) = (self.tris.get(i as usize), other.tris.get(j as usize)) else { return };
            let (d, a, b) = triangle_distance(x, y);
            if d <= bound.get() && best.is_none_or(|m| d < m.distance) {
                best = Some(MeshClosest { distance: d, a, b, tri_a: i, tri_b: j });
                bound.set(d);
            }
        };
        self.bvh.visit_pairs(&other.bvh, |x, y| box_gap(x, y) <= bound.get(), &mut found);
        best
    }

    /// Calls `f` with every pair of triangles, one from this set and one from `other`, that come
    /// within `d` of each other, with their nearest points.
    pub fn within(&self, other: &TriangleSet, d: f64, mut f: impl FnMut(MeshClosest)) {
        if !(d >= 0.0) {
            return;
        }
        self.bvh.visit_pairs(
            &other.bvh,
            |x, y| box_gap(x, y) <= d,
            |i, j| {
                let (Some(x), Some(y)) = (self.tris.get(i as usize), other.tris.get(j as usize)) else { return };
                let (dist, a, b) = triangle_distance(x, y);
                if dist <= d {
                    f(MeshClosest { distance: dist, a, b, tri_a: i, tri_b: j });
                }
            },
        );
    }

    /// The distance from `p` to the nearest of the triangles `tris` (indices into this set).
    pub fn distance_to(&self, p: Vec3, tris: &[u32]) -> f64 {
        tris.iter()
            .filter_map(|&t| self.tris.get(t as usize))
            .map(|[a, b, c]| closest_on_triangle(p, *a, *b, *c).dist(p))
            .fold(f64::INFINITY, f64::min)
    }
}

/// The gap between two boxes (0 when they meet).
pub fn box_gap(a: &Aabb3, b: &Aabb3) -> f64 {
    if a.is_empty() || b.is_empty() {
        return f64::INFINITY;
    }
    let axis = |lo1: f64, hi1: f64, lo2: f64, hi2: f64| (lo2 - hi1).max(lo1 - hi2).max(0.0);
    let d = Vec3::new(axis(a.min.x, a.max.x, b.min.x, b.max.x), axis(a.min.y, a.max.y, b.min.y, b.max.y), axis(a.min.z, a.max.z, b.min.z, b.max.z));
    d.len()
}

/// The nearest points of segments `p0`–`p1` and `q0`–`q1` (Ericson, Real-Time Collision
/// Detection, 5.1.9).
pub fn closest_on_segments(p0: Vec3, p1: Vec3, q0: Vec3, q1: Vec3) -> (Vec3, Vec3) {
    let (d1, d2, r) = (p1 - p0, q1 - q0, p0 - q0);
    let (a, e, f) = (d1.len2(), d2.len2(), d2.dot(r));
    const TINY: f64 = 1e-300;
    let (s, t) = if a <= TINY && e <= TINY {
        (0.0, 0.0)
    } else if a <= TINY {
        (0.0, (f / e).clamp(0.0, 1.0))
    } else {
        let c = d1.dot(r);
        if e <= TINY {
            ((-c / a).clamp(0.0, 1.0), 0.0)
        } else {
            let b = d1.dot(d2);
            let den = a * e - b * b;
            let mut s = if den > TINY * (a * e).max(1.0) { ((b * f - c * e) / den).clamp(0.0, 1.0) } else { 0.0 };
            let mut t = (b * s + f) / e;
            if t < 0.0 {
                t = 0.0;
                s = (-c / a).clamp(0.0, 1.0);
            } else if t > 1.0 {
                t = 1.0;
                s = ((b - c) / a).clamp(0.0, 1.0);
            }
            (s, t)
        }
    };
    (p0 + d1 * s, q0 + d2 * t)
}

/// Where segment `p`–`q` passes through triangle `t` (not when it lies in its plane).
fn segment_through(p: Vec3, q: Vec3, t: &[Vec3; 3]) -> Option<Vec3> {
    let [a, b, c] = *t;
    let n = (b - a).cross(c - a);
    let (dp, dq) = (n.dot(p - a), n.dot(q - a));
    if (dp > 0.0 && dq > 0.0) || (dp < 0.0 && dq < 0.0) || dp == dq {
        return None;
    }
    let x = p + (q - p) * (dp / (dp - dq));
    let inside = [(a, b), (b, c), (c, a)].iter().all(|(u, v)| (*v - *u).cross(x - *u).dot(n) >= 0.0);
    inside.then_some(x)
}

/// The distance between two triangles and their nearest points (0 where they cross).
pub fn triangle_distance(s: &[Vec3; 3], t: &[Vec3; 3]) -> (f64, Vec3, Vec3) {
    let edges = |x: &[Vec3; 3]| [(x[0], x[1]), (x[1], x[2]), (x[2], x[0])];
    for (p, q) in edges(s) {
        if let Some(x) = segment_through(p, q, t) {
            return (0.0, x, x);
        }
    }
    for (p, q) in edges(t) {
        if let Some(x) = segment_through(p, q, s) {
            return (0.0, x, x);
        }
    }
    let mut best = (f64::INFINITY, s[0], t[0]);
    let mut keep = |a: Vec3, b: Vec3| {
        let d = a.dist(b);
        if d < best.0 {
            best = (d, a, b);
        }
    };
    for p in s {
        keep(*p, closest_on_triangle(*p, t[0], t[1], t[2]));
    }
    for q in t {
        keep(closest_on_triangle(*q, s[0], s[1], s[2]), *q);
    }
    for (p0, p1) in edges(s) {
        for (q0, q1) in edges(t) {
            let (a, b) = closest_on_segments(p0, p1, q0, q1);
            keep(a, b);
        }
    }
    best
}

#[cfg(test)]
mod tests {
    use super::*;

    fn tri(a: [f64; 3], b: [f64; 3], c: [f64; 3]) -> [Vec3; 3] {
        [Vec3::from(a), Vec3::from(b), Vec3::from(c)]
    }

    /// Distances between triangles worked out by hand: face to vertex, edge to edge, crossing.
    #[test]
    fn triangle_distances_by_hand() {
        let floor = tri([0.0, 0.0, 0.0], [10.0, 0.0, 0.0], [0.0, 10.0, 0.0]);
        // A vertex 3 above the floor's inside.
        let (d, a, b) = triangle_distance(&floor, &tri([1.0, 1.0, 3.0], [1.0, 1.0, 9.0], [2.0, 1.0, 9.0]));
        assert!((d - 3.0).abs() < 1e-12 && a.dist(Vec3::new(1.0, 1.0, 0.0)) < 1e-12 && b.dist(Vec3::new(1.0, 1.0, 3.0)) < 1e-12);
        // Two skew edges: the floor's x axis edge and a line along y at x = 5, z = 2, off the floor at y < 0.
        let (d, ..) = triangle_distance(&floor, &tri([5.0, -1.0, 2.0], [5.0, -9.0, 2.0], [5.0, -5.0, 9.0]));
        assert!((d - 5f64.sqrt()).abs() < 1e-12, "{d}");
        // A triangle through the floor.
        let (d, ..) = triangle_distance(&floor, &tri([2.0, 2.0, -1.0], [2.0, 2.0, 1.0], [3.0, 2.0, 1.0]));
        assert_eq!(d, 0.0);
        // Parallel segments overlapping along their length.
        let (p, q) = closest_on_segments(Vec3::ZERO, Vec3::X * 4.0, Vec3::new(2.0, 1.0, 0.0), Vec3::new(6.0, 1.0, 0.0));
        assert!((p.dist(q) - 1.0).abs() < 1e-12);
    }

    fn cube(at: Vec3, size: f64) -> Mesh {
        let mut m = Mesh::default();
        let c = |x: f64, y: f64, z: f64| at + Vec3::new(x, y, z) * size;
        m.positions = vec![c(0., 0., 0.), c(1., 0., 0.), c(1., 1., 0.), c(0., 1., 0.), c(0., 0., 1.), c(1., 0., 1.), c(1., 1., 1.), c(0., 1., 1.)];
        m.triangles =
            vec![[0, 2, 1], [0, 3, 2], [4, 5, 6], [4, 6, 7], [0, 1, 5], [0, 5, 4], [1, 2, 6], [1, 6, 5], [2, 3, 7], [2, 7, 6], [3, 0, 4], [3, 4, 7]];
        m.tri_face = vec![0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5];
        m
    }

    /// Two cubes apart along a diagonal: the nearest points are their facing corners.
    #[test]
    fn nearest_points_of_two_meshes() {
        let a = TriangleSet::new(&cube(Vec3::ZERO, 10.0));
        let b = TriangleSet::new(&cube(Vec3::new(13.0, 14.0, 0.0), 10.0));
        let c = a.closest(&b, f64::INFINITY);
        assert!(c.is_some_and(|c| (c.distance - 5.0).abs() < 1e-12), "{c:?}");
        assert!(a.closest(&b, 4.0).is_none(), "nothing within 4");
        let mut n = 0;
        a.within(&b, 5.5, |_| n += 1);
        assert!(n > 0);
        let far = TriangleSet::new(&cube(Vec3::new(5.0, 5.0, 5.0), 10.0));
        assert_eq!(a.closest(&far, f64::INFINITY).map(|c| c.distance), Some(0.0), "overlapping cubes cross");
    }
}