//! Points on a surface–surface intersection edge, on our own surface evaluators.
//!
//! truck stores an intersection edge as a leader curve (a polyline or B-spline near the
//! intersection) and its two surfaces, and finds each point by projecting the leader point onto
//! each surface in turn from no starting guess: a nearest-point search per surface per point.
//! Here the intersection is solved exactly at a set of leader parameters, and a cubic B-spline
//! is fitted through those points together with both surfaces' parameters there (a smooth
//! leader with its curves on both surfaces). Each point is then refined from the B-spline by
//! Newton's method on the four unknowns (u1, v1, u2, v2): S1(u1, v1) = S2(u2, v2), held on the
//! plane through the B-spline point normal to the B-spline (Barnhill & Kersey, "A marching
//! method for parametric surface/surface intersection", CAGD 7, 1990; Patrikalakis & Maekawa,
//! "Shape Interrogation for CAD/CAM", 2002, ch. 5). Every point is independent once seeded. Where the surfaces are close to
//! tangent along the edge (the system becomes singular) or Newton does not converge, the caller
//! uses truck's evaluation.

use solidcraft_geom::Vec3;
use solidcraft_geom::bspline::CurveN;
use solidcraft_geom::tess::EvalSurface;
use truck_meshalgo::prelude::*;
use truck_modeling as mt;

use super::uv::{self, Domain};
use crate::body::from_p3;

/// Leader parameters at which both surfaces' parameters are found up front.
const SEEDS: usize = 32;
/// Newton iterations per point (two or three suffice from a seed).
const MAX_ITER: usize = 8;
/// Below this sine of the angle between the surface normals the intersection is treated as
/// near-tangent: points there are left to truck.
const MIN_SIN: f64 = 0.05;

/// An intersection edge ready for point evaluation.
pub(crate) struct Intersection {
    surfaces: [EvalSurface; 2],
    domains: [Domain; 2],
    /// The smooth leader over the curve's parameter: the intersection point (x, y, z) and both
    /// surfaces' parameters (u1, v1, u2, v2), periodic directions unwrapped along it.
    leader: CurveN<7>,
    /// Convergence tolerance (mm).
    eps: f64,
}

impl Intersection {
    /// `None` unless `curve` is an intersection curve whose surfaces have evaluators and whose
    /// leader projects onto both.
    pub(crate) fn new(curve: &mt::Curve) -> Option<Self> {
        let mt::Curve::IntersectionCurve(ic) = curve else { return None };
        let surfaces = [super::surface::eval_surface(ic.surface0())?, super::surface::eval_surface(ic.surface1())?];
        let domains = [uv::domain(&surfaces[0]), uv::domain(&surfaces[1])];
        let (t0, t1) = curve.range_tuple();
        if !(t0.is_finite() && t1.is_finite() && t1 > t0) {
            return None;
        }
        let leader = |t: f64| match ic.leader() {
            mt::Leader::Polyline(l) => (from_p3(l.subs(t)), from_v3(l.der(t))),
            mt::Leader::BSpline(l) => (from_p3(l.subs(t)), from_v3(l.der(t))),
        };
        // The leader lies within the curve's tolerance of both surfaces.
        let accept = (ic.tolerance() * 100.0).max(1e-6);
        let scale = (0..=SEEDS).map(|k| leader(t0 + (t1 - t0) * k as f64 / SEEDS as f64).0.len()).fold(1.0, f64::max);
        let mut it = Intersection { surfaces, domains, leader: CurveN { degree: 1, knots: Vec::new(), ctrl: Vec::new() }, eps: scale * 1e-10 };
        let (mut params, mut points) = (Vec::with_capacity(SEEDS + 1), Vec::with_capacity(SEEDS + 1));
        let mut prev: Option<[f64; 4]> = None;
        for k in 0..=SEEDS {
            let t = t0 + (t1 - t0) * k as f64 / SEEDS as f64;
            let (p, d) = leader(t);
            let mut q = [0.0; 4];
            for (i, (s, dom)) in it.surfaces.iter().zip(&it.domains).enumerate() {
                let hint = prev.map(|w| [w[2 * i], w[2 * i + 1]]);
                let mut uv = uv::project(s, dom, p, hint, accept)?;
                if let Some(h) = hint {
                    for d in 0..2 {
                        uv[d] = uv::unwrap(uv[d], h[d], dom.period[d]);
                    }
                }
                q[2 * i] = uv[0];
                q[2 * i + 1] = uv[1];
            }
            // Exactly on the intersection where Newton converges (near-tangent seeds keep the
            // leader point and its projections).
            let (p, q) = d.normalized().and_then(|dir| it.newton(p, dir, q)).unwrap_or((p, q));
            params.push(t);
            points.push([p.x, p.y, p.z, q[0], q[1], q[2], q[3]]);
            prev = Some(q);
        }
        it.leader = CurveN::interpolate(&params, &points, 3).ok()?;
        Some(it)
    }

    /// The point at curve parameter `t` and both surfaces' parameters there, or `None`
    /// (near-tangent, or Newton failed).
    pub(crate) fn eval(&self, t: f64) -> Option<(Vec3, [f64; 4])> {
        let (x, dx) = self.leader.eval(t)?;
        let dir = Vec3::new(dx[0], dx[1], dx[2]).normalized()?;
        self.newton(Vec3::new(x[0], x[1], x[2]), dir, [x[3], x[4], x[5], x[6]])
    }

    /// The two surfaces, in the order of the parameters [`Intersection::eval`] gives.
    pub(crate) fn surfaces(&self) -> &[EvalSurface; 2] {
        &self.surfaces
    }

    /// Newton from the parameters `x` on S1 = S2, held on the plane through `l` normal to `dir`:
    /// the point and its parameters, or `None` (near-tangent, singular, or not converged).
    fn newton(&self, l: Vec3, dir: Vec3, mut x: [f64; 4]) -> Option<(Vec3, [f64; 4])> {
        for _ in 0..MAX_ITER {
            let (Some((p1, u1, v1)), Some((p2, u2, v2))) = (self.surfaces[0].derivs(x[0], x[1]), self.surfaces[1].derivs(x[2], x[3])) else {
                return None;
            };
            let gap = p1 - p2;
            let plane = dir.dot(p1 - l);
            let n = u1.cross(v1).normalized()?.cross(u2.cross(v2).normalized()?);
            if n.len() < MIN_SIN {
                return None;
            }
            if gap.len() <= self.eps && plane.abs() <= self.eps {
                return Some(((p1 + p2) * 0.5, x));
            }
            // J · dx = −F, F = (S1 − S2, dir · (S1 − L)).
            let j = [[u1.x, v1.x, -u2.x, -v2.x], [u1.y, v1.y, -u2.y, -v2.y], [u1.z, v1.z, -u2.z, -v2.z], [dir.dot(u1), dir.dot(v1), 0.0, 0.0]];
            let dx = solve4(j, [-gap.x, -gap.y, -gap.z, -plane])?;
            for (xi, d) in x.iter_mut().zip(dx) {
                *xi += d;
            }
            for i in 0..2 {
                let [u, v] = self.domains[i].clamp([x[2 * i], x[2 * i + 1]]);
                x[2 * i] = u;
                x[2 * i + 1] = v;
            }
        }
        None
    }
}

fn from_v3(v: mt::Vector3) -> Vec3 {
    Vec3::new(v.x, v.y, v.z)
}

/// `a · x = b` for a 4 × 4 system by Gaussian elimination with partial pivoting; `None` when
/// singular.
fn solve4(mut a: [[f64; 4]; 4], mut b: [f64; 4]) -> Option<[f64; 4]> {
    let scale = a.iter().flatten().fold(0.0f64, |m, x| m.max(x.abs()));
    if !(scale > 0.0) {
        return None;
    }
    for c in 0..4 {
        let p = (c..4).max_by(|&i, &j| a[i][c].abs().total_cmp(&a[j][c].abs()))?;
        if !(a[p][c].abs() > 1e-12 * scale) {
            return None;
        }
        a.swap(c, p);
        b.swap(c, p);
        let pivot = a[c];
        for r in c + 1..4 {
            let f = a[r][c] / pivot[c];
            for (x, p) in a[r].iter_mut().zip(pivot).skip(c) {
                *x -= f * p;
            }
            b[r] -= f * b[c];
        }
    }
    let mut x = [0.0; 4];
    for c in (0..4).rev() {
        let s: f64 = (c + 1..4).map(|k| a[c][k] * x[k]).sum();
        x[c] = (b[c] - s) / a[c][c];
    }
    x.iter().all(|v| v.is_finite()).then_some(x)
}

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

    #[test]
    fn solves_a_four_by_four_system() {
        let a = [[2.0, 1.0, 0.0, 0.0], [1.0, 3.0, 1.0, 0.0], [0.0, 1.0, 4.0, 1.0], [0.0, 0.0, 1.0, 5.0]];
        let x = [1.0, -2.0, 0.5, 3.0];
        let b = [0, 1, 2, 3].map(|r| (0..4).map(|c| a[r][c] * x[c]).sum::<f64>());
        let Some(got) = solve4(a, b) else { panic!("solves") };
        for (g, w) in got.iter().zip(x) {
            assert!((g - w).abs() < 1e-12);
        }
        assert!(solve4([[1.0; 4]; 4], [1.0; 4]).is_none(), "singular");
    }

    /// Points on the intersection edges of a boolean (a cross hole: two cylinders) lie on both
    /// surfaces, on truck's curve (the parameterisation follows our smooth leader, so the same
    /// parameter may sit at a slightly different place along it), and advance along the curve.
    #[test]
    fn intersection_points_lie_on_both_surfaces_and_on_truck_s_curve() {
        let Ok(pin) = crate::cylinder(Vec3::new(5.0, 5.0, -1.0), Vec3::Z, 2.0, 6.0) else { panic!("pin") };
        let Ok(cross) = crate::cylinder(Vec3::new(5.0, -1.0, 2.0), Vec3::Y, 1.0, 12.0) else { panic!("cross") };
        let Ok(Some(body)) = crate::boolean(&pin, &cross, crate::BoolOp::Cut) else { panic!("cut") };
        let mut checked = 0;
        for e in body.solid.edge_iter() {
            let curve = e.curve();
            let Some(ix) = Intersection::new(&curve) else { continue };
            let (t0, t1) = curve.range_tuple();
            let dense: Vec<Vec3> = (0..=1000).map(|k| from_p3(curve.subs(t0 + (t1 - t0) * f64::from(k) / 1000.0))).collect();
            let near = |p: Vec3| dense.windows(2).map(|w| p.dist_to_segment(w[0], w[1])).fold(f64::MAX, f64::min);
            let mut last: Option<Vec3> = None;
            for k in 0..=50 {
                let t = t0 + (t1 - t0) * f64::from(k) / 50.0;
                let Some((p, _)) = ix.eval(t) else { continue };
                for (s, dom) in ix.surfaces.iter().zip(&ix.domains) {
                    assert!(uv::project(s, dom, p, None, 1e-8).is_some(), "t {t}: {p:?} off a surface");
                }
                assert!(near(p) < 1e-4, "t {t}: {p:?} is {} from truck's curve", near(p));
                if let Some(q) = last {
                    assert!(p.dist(q) > 0.0, "t {t}: the points advance");
                }
                last = Some(p);
                checked += 1;
            }
        }
        assert!(checked > 100, "{checked} points checked");
    }
}