OneCAD/crates/geom/src/bspline.rs

//! B-spline machinery shared by NURBS and T-spline surfaces: homogeneous points, the
//! univariate basis (Cox–de Boor on half-open intervals, with derivatives), Boehm knot
//! insertion, knot insertion into a single basis function, Bézier extraction, and a tensor
//! NURBS surface.
//!
//! References: Piegl & Tiller, *The NURBS Book* (2nd ed.) — §2.2 (basis definition, eq. 2.5),
//! §2.3 (derivatives, eq. 2.9), A2.1 (span search), §5.2 (knot insertion, eq. 5.10 / A5.1),
//! §1.4 (de Casteljau); Sederberg, Cardon, Finnigan, North, Zheng, Lyche 2004, "T-spline
//! simplification and local refinement", §3 (knot insertion into one blending function).
//!
//! Input is hostile: every constructor checks finiteness and sizes and returns
//! [`SplineError`] instead of panicking; no function here indexes with an unchecked number.

use std::ops::{Add, AddAssign, Mul, Sub};

use crate::Vec3;

/// Highest polynomial degree accepted in either direction.
pub const MAX_SPLINE_DEGREE: usize = 7;
/// Most knots accepted in one knot vector.
pub const MAX_KNOTS: usize = 1 << 14;
/// Most control points accepted in one tensor surface.
pub const MAX_SURFACE_POINTS: usize = 1 << 20;

/// Why a spline could not be built or used.
#[derive(Debug, Clone, PartialEq, thiserror::Error)]
pub enum SplineError {
    #[error("invalid spline: {0}")]
    Invalid(String),
    #[error("not supported yet: {0}")]
    Unsupported(String),
    #[error("spline too large: {0}")]
    TooLarge(String),
}

pub type SplineResult<T> = Result<T, SplineError>;

pub(crate) fn invalid<T>(msg: impl Into<String>) -> SplineResult<T> {
    Err(SplineError::Invalid(msg.into()))
}

/// A homogeneous control point `(x·w, y·w, z·w, w)`.
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct HPoint {
    pub x: f64,
    pub y: f64,
    pub z: f64,
    pub w: f64,
}

impl HPoint {
    pub const ZERO: HPoint = HPoint { x: 0.0, y: 0.0, z: 0.0, w: 0.0 };

    /// The point `p` with weight `w`.
    pub fn weighted(p: Vec3, w: f64) -> HPoint {
        HPoint { x: p.x * w, y: p.y * w, z: p.z * w, w }
    }
    /// The Cartesian point, if the weight is usable.
    pub fn project(self) -> Option<Vec3> {
        let p = Vec3::new(self.x / self.w, self.y / self.w, self.z / self.w);
        (self.w.abs() > f64::MIN_POSITIVE && p.is_finite()).then_some(p)
    }
    /// The weighted coordinates `(x·w, y·w, z·w)`.
    pub fn xyz(self) -> Vec3 {
        Vec3::new(self.x, self.y, self.z)
    }
    pub fn is_finite(self) -> bool {
        self.x.is_finite() && self.y.is_finite() && self.z.is_finite() && self.w.is_finite()
    }
}

impl Add for HPoint {
    type Output = HPoint;
    fn add(self, o: HPoint) -> HPoint {
        HPoint { x: self.x + o.x, y: self.y + o.y, z: self.z + o.z, w: self.w + o.w }
    }
}
impl Sub for HPoint {
    type Output = HPoint;
    fn sub(self, o: HPoint) -> HPoint {
        HPoint { x: self.x - o.x, y: self.y - o.y, z: self.z - o.z, w: self.w - o.w }
    }
}
impl Mul<f64> for HPoint {
    type Output = HPoint;
    fn mul(self, k: f64) -> HPoint {
        HPoint { x: self.x * k, y: self.y * k, z: self.z * k, w: self.w * k }
    }
}
impl AddAssign for HPoint {
    fn add_assign(&mut self, o: HPoint) {
        *self = *self + o;
    }
}

/// A parameter direction: along s (u, the first) or along t (v, the second).
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
pub enum Dir {
    S,
    T,
}

impl Dir {
    /// `s` for [`Dir::S`], `t` for [`Dir::T`].
    pub fn pick<X>(self, s: X, t: X) -> X {
        match self {
            Dir::S => s,
            Dir::T => t,
        }
    }
}

/// Homogeneous control points from finite points and positive weights, one each.
pub fn weighted_points(points: &[Vec3], weights: &[f64]) -> SplineResult<Vec<HPoint>> {
    if points.len() != weights.len() {
        return invalid("one weight per control point");
    }
    if points.iter().any(|p| !p.is_finite()) {
        return invalid("a control point is not finite");
    }
    if weights.iter().any(|w| !(w.is_finite() && *w > 0.0)) {
        return invalid("weights must be positive numbers");
    }
    Ok(points.iter().zip(weights).map(|(p, w)| HPoint::weighted(*p, *w)).collect())
}

/// Which one-sided limit a basis evaluation takes at a knot. Basis functions live on half-open
/// spans `[uᵢ, uᵢ₊₁)` ([`Side::Right`]); at the right end of a domain the left limit on
/// `(uᵢ, uᵢ₊₁]` ([`Side::Left`]) keeps the closing span.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Side {
    Right,
    Left,
}

/// Check a knot vector: finite, non-decreasing, not too long.
pub fn check_knots(knots: &[f64]) -> SplineResult<()> {
    if knots.len() > MAX_KNOTS {
        return Err(SplineError::TooLarge(format!("{} knots (at most {MAX_KNOTS})", knots.len())));
    }
    if knots.iter().any(|k| !k.is_finite()) {
        return invalid("a knot is not a finite number");
    }
    if knots.windows(2).any(|w| matches!(w, [a, b] if b < a)) {
        return invalid("knots must not decrease");
    }
    Ok(())
}

/// One B-spline basis function of degree `p = knots.len() − 2` given by its local knot vector
/// `[ξ₀ … ξₚ₊₁]`, and its derivatives up to order `n`: element `k` is the `k`-th derivative at
/// `u`. Cox–de Boor recursion (NURBS Book eq. 2.5) with the 0/0 = 0 convention, derivatives by
/// eq. 2.9 applied repeatedly (`dᵏNᵢ,ₚ = p·(dᵏ⁻¹Nᵢ,ₚ₋₁/(uᵢ₊ₚ−uᵢ) − dᵏ⁻¹Nᵢ₊₁,ₚ₋₁/(uᵢ₊ₚ₊₁−uᵢ₊₁))`).
/// A knot vector shorter than two knots gives zeros.
pub fn local_basis(knots: &[f64], u: f64, side: Side, n: usize) -> Vec<f64> {
    let mut out = vec![0.0; n + 1];
    let Some(p) = knots.len().checked_sub(2) else { return out };
    if p > MAX_SPLINE_DEGREE || !u.is_finite() {
        return out;
    }
    let kn = |i: usize| knots.get(i).copied().unwrap_or(0.0);
    let ratio = |num: f64, den: f64| if den > 0.0 { num / den } else { 0.0 };
    // table[k][j]: k-th derivative of N_{j,q} for the current degree q (j = 0..=p−q), updated
    // in place: k descending (level k reads level k−1 of the previous degree), j ascending
    // (N_{j,q} reads N_{j+1,q−1}, not yet overwritten). Derivatives above p are zero.
    const W: usize = MAX_SPLINE_DEGREE + 1;
    let levels = n.min(p);
    let mut table = [[0.0f64; W]; W];
    for (j, slot) in table[0].iter_mut().enumerate().take(p + 1) {
        let (a, b) = (kn(j), kn(j + 1));
        let inside = match side {
            Side::Right => a <= u && u < b,
            Side::Left => a < u && u <= b,
        };
        *slot = if inside { 1.0 } else { 0.0 };
    }
    for q in 1..=p {
        for k in (0..=levels).rev() {
            for j in 0..=p - q {
                let (d1, d2) = (kn(j + q) - kn(j), kn(j + q + 1) - kn(j + 1));
                let at = |k: usize, j: usize| table.get(k).and_then(|r| r.get(j)).copied().unwrap_or(0.0);
                let v = if k == 0 {
                    ratio(u - kn(j), d1) * at(0, j) + ratio(kn(j + q + 1) - u, d2) * at(0, j + 1)
                } else {
                    q as f64 * (ratio(at(k - 1, j), d1) - ratio(at(k - 1, j + 1), d2))
                };
                if let Some(slot) = table.get_mut(k).and_then(|r| r.get_mut(j)) {
                    *slot = v;
                }
            }
        }
    }
    for (k, o) in out.iter_mut().enumerate().take(levels + 1) {
        *o = table.get(k).map_or(0.0, |r| r[0]);
    }
    out
}

/// The coefficients `(c₁, c₂)` of knot insertion into one basis function: inserting `k`
/// (with `ξ₀ ≤ k ≤ ξₚ₊₁`) into `N[ξ₀…ξₚ₊₁]` gives `c₁·N[first p+2 merged knots] +
/// c₂·N[last p+2 merged knots]` (Sederberg et al. 2004, §3; the single-function case of the
/// NURBS Book eq. 5.10). `None` when `k` is outside the support or the knots are too few.
pub fn split_basis(knots: &[f64], k: f64) -> Option<(f64, f64)> {
    let p = knots.len().checked_sub(2)?;
    let (x0, x1, xp, xl) = (*knots.first()?, *knots.get(1)?, *knots.get(p)?, *knots.get(p + 1)?);
    if !(k >= x0 && k <= xl) {
        return None;
    }
    let c1 = if k >= xp { 1.0 } else { (k - x0) / (xp - x0) };
    let c2 = if k <= x1 { 1.0 } else { (xl - k) / (xl - x1) };
    Some((c1, c2))
}

/// The two knot vectors of a split basis function (see [`split_basis`]): `k` merged into
/// `knots` in order, then the first and the last `knots.len()` entries.
pub fn split_knots<T: Copy + PartialOrd>(knots: &[T], k: T) -> [Vec<T>; 2] {
    let at = knots.iter().position(|&x| x > k).unwrap_or(knots.len());
    let mut merged = knots.to_vec();
    merged.insert(at, k);
    let n = knots.len();
    [merged.get(..n).map(<[T]>::to_vec).unwrap_or_default(), merged.get(1..).map(<[T]>::to_vec).unwrap_or_default()]
}

/// The Bernstein coefficients of one basis function `N[knots]` on `[a, b]`: `N(u) =
/// Σ cᵢ Bᵢ,ₚ((u−a)/(b−a))` there. `[a, b]` must not contain a knot in its interior (the
/// function is one polynomial on it). Found by inserting `a` and `b` into the function until
/// every surviving piece is a Bernstein polynomial `N[a…a, b…b]` (Sederberg et al. 2004, §3;
/// NURBS Book §5.4, decomposition into Bézier segments).
pub fn basis_bezier(knots: &[f64], a: f64, b: f64) -> SplineResult<Vec<f64>> {
    let Some(p) = knots.len().checked_sub(2) else { return invalid("a basis function needs at least two knots") };
    if p > MAX_SPLINE_DEGREE {
        return Err(SplineError::Unsupported(format!("degree {p} (at most {MAX_SPLINE_DEGREE})")));
    }
    check_knots(knots)?;
    if !(a.is_finite() && b.is_finite() && a < b) {
        return invalid("a Bézier interval must be finite and non-empty");
    }
    if knots.iter().any(|&x| x > a && x < b) {
        return invalid("a knot lies inside the Bézier interval");
    }
    let mut out = vec![0.0; p + 1];
    let mut work = vec![(knots.to_vec(), 1.0)];
    let mut steps = 0usize;
    while let Some((kn, c)) = work.pop() {
        steps += 1;
        if steps > 1 << 12 {
            return Err(SplineError::TooLarge("Bézier extraction did not converge".into()));
        }
        let (Some(&lo), Some(&hi)) = (kn.first(), kn.last()) else { continue };
        if hi <= a || lo >= b || c == 0.0 {
            continue;
        }
        let insert = if kn.iter().any(|&x| x < a) {
            Some(a)
        } else if kn.iter().any(|&x| x > b) {
            Some(b)
        } else {
            None
        };
        match insert {
            Some(k) => {
                let (c1, c2) = split_basis(&kn, k).ok_or_else(|| SplineError::Invalid("knot insertion outside the support".into()))?;
                let [first, second] = split_knots(&kn, k);
                work.push((first, c * c1));
                work.push((second, c * c2));
            }
            None => {
                // Knots a×(p+1−i), b×(i+1): the Bernstein polynomial Bᵢ,ₚ.
                let nb = kn.iter().filter(|&&x| x == b).count();
                if let Some(slot) = nb.checked_sub(1).and_then(|i| out.get_mut(i)) {
                    *slot += c;
                }
            }
        }
    }
    Ok(out)
}

/// The span index `k` with `knots[k] ≤ u < knots[k+1]` among the valid spans `p ≤ k < n`
/// (`n` control points), the last span closed at the domain end (NURBS Book A2.1).
pub fn find_span(knots: &[f64], p: usize, n: usize, u: f64) -> Option<usize> {
    if n <= p || knots.len() != n + p + 1 || !u.is_finite() {
        return None;
    }
    let (lo, hi) = (*knots.get(p)?, *knots.get(n)?);
    if u < lo || u > hi {
        return None;
    }
    // The last non-empty span in [p, n−1] that starts at or before u.
    let mut k = p;
    for i in p..n {
        if let (Some(&a), Some(&b)) = (knots.get(i), knots.get(i + 1))
            && a <= u
            && a < b
        {
            k = i;
        }
    }
    Some(k)
}

/// Insert the knot `u` once into a B-spline with degree `p`, knots and homogeneous control
/// points (Boehm; NURBS Book eq. 5.10 / A5.1). `u` must lie in the domain `[knots[p],
/// knots[n]]`; the curve is unchanged.
pub fn insert_knot(p: usize, knots: &[f64], ctrl: &[HPoint], u: f64) -> SplineResult<(Vec<f64>, Vec<HPoint>)> {
    let n = ctrl.len();
    if n <= p || knots.len() != n + p + 1 {
        return invalid("knot and control point counts do not match the degree");
    }
    if knots.len() >= MAX_KNOTS {
        return Err(SplineError::TooLarge("too many knots".into()));
    }
    check_knots(knots)?;
    let k = find_span(knots, p, n, u).ok_or_else(|| SplineError::Invalid(format!("knot {u} is outside the domain")))?;
    let kn = |i: usize| knots.get(i).copied().unwrap_or(0.0);
    let pt = |i: usize| ctrl.get(i).copied().unwrap_or(HPoint::ZERO);
    let mut q = Vec::with_capacity(n + 1);
    for i in 0..=n {
        q.push(if i + p <= k {
            pt(i)
        } else if i > k {
            pt(i - 1)
        } else {
            let den = kn(i + p) - kn(i);
            let alpha = if den > 0.0 { (u - kn(i)) / den } else { 1.0 };
            pt(i) * alpha + pt(i - 1) * (1.0 - alpha)
        });
    }
    let mut nk = knots.to_vec();
    nk.insert(k + 1, u);
    Ok((nk, q))
}

/// Bernstein polynomial `Bᵢ,ₚ(x)`.
pub fn bernstein(p: usize, i: usize, x: f64) -> f64 {
    if i > p {
        return 0.0;
    }
    let mut c = 1.0;
    for m in 0..i {
        c = c * (p - m) as f64 / (m + 1) as f64;
    }
    c * x.powi(i as i32) * (1.0 - x).powi((p - i) as i32)
}

/// A Bézier curve's point at `x ∈ [0, 1]` (de Casteljau, NURBS Book §1.4).
pub fn bezier_point(ctrl: &[HPoint], x: f64) -> HPoint {
    subdivide(ctrl, x).0.last().copied().unwrap_or(HPoint::ZERO)
}

/// The control points of the piece of a Bézier curve over `[a, b] ⊂ [0, 1]` (two de Casteljau
/// subdivisions).
pub fn bezier_segment(ctrl: &[HPoint], a: f64, b: f64) -> SplineResult<Vec<HPoint>> {
    if !(a.is_finite() && b.is_finite() && 0.0 <= a && a < b && b <= 1.0) {
        return invalid("a Bézier segment must satisfy 0 ≤ a < b ≤ 1");
    }
    // Left part [0, b], then its right part from a/b.
    let left = subdivide(ctrl, b).0;
    Ok(subdivide(&left, a / b).1)
}

/// Split a Bézier curve at `x` (de Casteljau): control points of `[0, x]` and `[x, 1]`.
fn subdivide(ctrl: &[HPoint], x: f64) -> (Vec<HPoint>, Vec<HPoint>) {
    let mut left = Vec::with_capacity(ctrl.len());
    let mut right = Vec::with_capacity(ctrl.len());
    let mut pts = ctrl.to_vec();
    while let (Some(&f), Some(&l)) = (pts.first(), pts.last()) {
        left.push(f);
        right.push(l);
        pts = pts.windows(2).map(|w| if let [a, b] = w { *a * (1.0 - x) + *b * x } else { HPoint::ZERO }).collect();
    }
    right.reverse();
    (left, right)
}

/// A rational tensor Bézier patch over the parameter rectangle `s × t`: control point `(i, j)`
/// (i along s, j along t) at `ctrl[j·(degree[0]+1) + i]`.
#[derive(Clone, Debug, PartialEq)]
pub struct BezierPatch {
    pub s: [f64; 2],
    pub t: [f64; 2],
    pub degree: [usize; 2],
    pub ctrl: Vec<HPoint>,
}

impl BezierPatch {
    /// Control point `(i, j)`.
    pub fn point(&self, i: usize, j: usize) -> HPoint {
        self.ctrl.get(j * (self.degree[0] + 1) + i).copied().unwrap_or(HPoint::ZERO)
    }
    /// Homogeneous point at global parameters `(s, t)`.
    pub fn eval_h(&self, s: f64, t: f64) -> HPoint {
        let x = (s - self.s[0]) / (self.s[1] - self.s[0]);
        let y = (t - self.t[0]) / (self.t[1] - self.t[0]);
        let mut acc = HPoint::ZERO;
        for j in 0..=self.degree[1] {
            let bj = bernstein(self.degree[1], j, y);
            for i in 0..=self.degree[0] {
                acc += self.point(i, j) * (bernstein(self.degree[0], i, x) * bj);
            }
        }
        acc
    }
    /// Point at global parameters `(s, t)`.
    pub fn eval(&self, s: f64, t: f64) -> Option<Vec3> {
        self.eval_h(s, t).project()
    }
    /// The control points of one side as a Bézier curve: `0` t = t₀ (s rising), `1` s = s₁
    /// (t rising), `2` t = t₁ (s rising), `3` s = s₀ (t rising).
    pub fn side(&self, side: usize) -> Vec<HPoint> {
        let [ps, pt] = self.degree;
        match side {
            0 => (0..=ps).map(|i| self.point(i, 0)).collect(),
            1 => (0..=pt).map(|j| self.point(ps, j)).collect(),
            2 => (0..=ps).map(|i| self.point(i, pt)).collect(),
            _ => (0..=pt).map(|j| self.point(0, j)).collect(),
        }
    }
}

/// A rational tensor-product B-spline (NURBS) surface. Control point `(i, j)` (i along u, j
/// along v) is `ctrl[j·count_u + i]`; the domain is `[knots_u[p], knots_u[n]] × …`.
#[derive(Clone, Debug, PartialEq)]
pub struct NurbsSurface {
    degree: [usize; 2],
    knots: [Vec<f64>; 2],
    count: [usize; 2],
    ctrl: Vec<HPoint>,
}

impl NurbsSurface {
    /// A surface from Cartesian points and positive weights (one per point, u fastest).
    pub fn new(degree: [usize; 2], knots: [Vec<f64>; 2], points: &[Vec3], weights: &[f64]) -> SplineResult<NurbsSurface> {
        Self::from_homogeneous(degree, knots, weighted_points(points, weights)?)
    }

    /// A surface from homogeneous control points (u fastest).
    pub fn from_homogeneous(degree: [usize; 2], knots: [Vec<f64>; 2], ctrl: Vec<HPoint>) -> SplineResult<NurbsSurface> {
        let mut count = [0; 2];
        for d in 0..2 {
            let (p, kn) = (degree[d], &knots[d]);
            if p == 0 || p > MAX_SPLINE_DEGREE {
                return Err(SplineError::Unsupported(format!("degree {p} (1 to {MAX_SPLINE_DEGREE})")));
            }
            check_knots(kn)?;
            let n = kn.len().checked_sub(p + 1).filter(|&n| n > p).ok_or_else(|| SplineError::Invalid("too few knots for the degree".into()))?;
            if !(kn.get(p) < kn.get(n)) {
                return invalid("the surface's domain is empty");
            }
            count[d] = n;
        }
        if count[0].checked_mul(count[1]).is_none_or(|c| c > MAX_SURFACE_POINTS) {
            return Err(SplineError::TooLarge(format!("more than {MAX_SURFACE_POINTS} control points")));
        }
        if ctrl.len() != count[0] * count[1] {
            return invalid(format!("{} control points, expected {}", ctrl.len(), count[0] * count[1]));
        }
        if ctrl.iter().any(|h| !h.is_finite() || !(h.w > 0.0)) {
            return invalid("control points must be finite with positive weights");
        }
        Ok(NurbsSurface { degree, knots, count, ctrl })
    }

    pub fn degree(&self) -> [usize; 2] {
        self.degree
    }
    pub fn knots(&self) -> &[Vec<f64>; 2] {
        &self.knots
    }
    /// Control point counts along u and v.
    pub fn count(&self) -> [usize; 2] {
        self.count
    }
    pub fn homogeneous(&self) -> &[HPoint] {
        &self.ctrl
    }
    /// Control point `(i, j)`.
    pub fn point(&self, i: usize, j: usize) -> HPoint {
        self.ctrl.get(j * self.count[0] + i).copied().unwrap_or(HPoint::ZERO)
    }

    /// The parameter domain `[[u₀, u₁], [v₀, v₁]]`.
    pub fn domain(&self) -> [[f64; 2]; 2] {
        let d = |k: usize| {
            let (p, n, kn) = (self.degree[k], self.count[k], &self.knots[k]);
            [kn.get(p).copied().unwrap_or(0.0), kn.get(n).copied().unwrap_or(0.0)]
        };
        [d(0), d(1)]
    }

    /// The point at `(u, v)`, which must lie in the domain.
    pub fn eval(&self, u: f64, v: f64) -> SplineResult<Vec3> {
        let dom = self.domain();
        let mut spans = [0; 2];
        let mut basis: [Vec<f64>; 2] = [Vec::new(), Vec::new()];
        for (d, x) in [u, v].into_iter().enumerate() {
            let (p, n, kn) = (self.degree[d], self.count[d], &self.knots[d]);
            let span = find_span(kn, p, n, x).ok_or_else(|| SplineError::Invalid(format!("({u}, {v}) is outside the domain")))?;
            let side = if x >= dom[d][1] { Side::Left } else { Side::Right };
            spans[d] = span;
            basis[d] = (span - p..=span).map(|i| kn.get(i..i + p + 2).map_or(0.0, |k| local_basis(k, x, side, 0)[0])).collect();
        }
        let mut acc = HPoint::ZERO;
        for (b, nj) in basis[1].iter().enumerate() {
            for (a, ni) in basis[0].iter().enumerate() {
                acc += self.point(spans[0] - self.degree[0] + a, spans[1] - self.degree[1] + b) * (ni * nj);
            }
        }
        acc.project().ok_or_else(|| SplineError::Invalid("the surface's weights vanish there".into()))
    }

    /// The same surface with the knot `x` inserted once in direction `dir` ([`Dir::S`] = u).
    pub fn insert_knot(&self, dir: Dir, x: f64) -> SplineResult<NurbsSurface> {
        let d = dir.pick(0, 1);
        let (p, kn) = (self.degree[d], &self.knots[d]);
        let [nu, nv] = self.count;
        let (lines, len) = if d == 0 { (nv, nu) } else { (nu, nv) };
        let at = |line: usize, k: usize| if d == 0 { self.point(k, line) } else { self.point(line, k) };
        let mut new_knots = Vec::new();
        let mut rows: Vec<Vec<HPoint>> = Vec::with_capacity(lines);
        for line in 0..lines {
            let row: Vec<HPoint> = (0..len).map(|k| at(line, k)).collect();
            let (nk, nr) = insert_knot(p, kn, &row, x)?;
            new_knots = nk;
            rows.push(nr);
        }
        let mut knots = self.knots.clone();
        knots[d] = new_knots;
        let new_len = len + 1;
        let mut ctrl = Vec::with_capacity(lines * new_len);
        if d == 0 {
            for row in &rows {
                ctrl.extend_from_slice(row);
            }
        } else {
            for j in 0..new_len {
                for row in &rows {
                    ctrl.push(row.get(j).copied().unwrap_or(HPoint::ZERO));
                }
            }
        }
        NurbsSurface::from_homogeneous(self.degree, knots, ctrl)
    }

    /// The same surface on the same domain with clamped knot vectors (`p+1` equal knots at
    /// each end, so the boundary curves are given by the boundary rows of control points):
    /// insert each domain end until it has multiplicity `p`, then drop the basis functions
    /// that vanish on the domain (NURBS Book §5.2 knot insertion; a basis function's last span
    /// does not depend on its first knot, so that knot can be moved to the domain end).
    pub fn clamped(&self) -> SplineResult<NurbsSurface> {
        let mut s = self.clone();
        for dir in [Dir::S, Dir::T] {
            let d = dir.pick(0, 1);
            let p = s.degree[d];
            for end in 0..2 {
                let a = s.domain()[d][end];
                while s.knots[d].iter().filter(|&&x| x == a).count() < p {
                    s = s.insert_knot(dir, a)?;
                }
                s = s.trim_end(d, end, a)?;
            }
        }
        Ok(s)
    }

    /// Drop the basis functions that vanish on the domain past the knot `a` (multiplicity ≥ p)
    /// at the start (`end == 0`) or the end of direction `d`.
    fn trim_end(&self, d: usize, end: usize, a: f64) -> SplineResult<NurbsSurface> {
        let p = self.degree[d];
        let kn = &self.knots[d];
        let n = self.count[d];
        let (first, last) = match end {
            0 => {
                let last_a = kn.iter().rposition(|&x| x == a).ok_or_else(|| SplineError::Invalid("clamp".into()))?;
                // Functions i with i + p + 1 ≤ last_a vanish on [a, …).
                (last_a.saturating_sub(p), n)
            }
            _ => {
                let first_a = kn.iter().position(|&x| x == a).ok_or_else(|| SplineError::Invalid("clamp".into()))?;
                // Functions i with i ≥ first_a vanish on (…, a].
                (0, first_a.min(n))
            }
        };
        if last <= first + p {
            return invalid("clamp: too few control points remain");
        }
        let mut knots = self.knots.clone();
        let mut nk: Vec<f64> = kn.get(first..last + p + 1).map(<[f64]>::to_vec).unwrap_or_default();
        match end {
            0 => nk.first_mut().into_iter().for_each(|x| *x = a),
            _ => nk.last_mut().into_iter().for_each(|x| *x = a),
        }
        knots[d] = nk;
        let [nu, nv] = self.count;
        let mut ctrl = Vec::new();
        for j in 0..nv {
            for i in 0..nu {
                let k = if d == 0 { i } else { j };
                if k >= first && k < last {
                    ctrl.push(self.point(i, j));
                }
            }
        }
        NurbsSurface::from_homogeneous(self.degree, knots, ctrl)
    }
}

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

    /// A small deterministic generator for test data.
    pub(crate) struct Lcg(pub(crate) u64);
    impl Lcg {
        pub(crate) fn unit(&mut self) -> f64 {
            self.0 = self.0.wrapping_mul(6364136223846793005).wrapping_add(1442695040888963407);
            (self.0 >> 11) as f64 / (1u64 << 53) as f64
        }
    }

    const KNOTS: [f64; 12] = [0.0, 0.0, 0.0, 0.0, 0.3, 0.3, 0.5, 0.9, 1.0, 1.0, 1.0, 1.0];

    fn samples() -> Vec<f64> {
        (0..=200).map(|k| k as f64 / 200.0).collect()
    }

    #[test]
    fn basis_partition_of_unity_non_negative_compact_support() {
        let p = 3;
        let n = KNOTS.len() - p - 1;
        for u in samples() {
            let side = if u >= 1.0 { Side::Left } else { Side::Right };
            let mut sum = 0.0;
            for i in 0..n {
                let local = &KNOTS[i..i + p + 2];
                let b = local_basis(local, u, side, 0)[0];
                assert!(b >= -1e-15, "N{i}({u}) = {b}");
                if u < local[0] || u > local[p + 1] {
                    assert_eq!(b, 0.0, "N{i} is not zero outside its support at {u}");
                }
                sum += b;
            }
            assert!((sum - 1.0).abs() < 1e-14, "Σ N({u}) = {sum}");
        }
    }

    #[test]
    fn basis_derivatives_match_finite_differences() {
        let p = 3;
        let h = 1e-6;
        let mut worst = 0.0f64;
        for i in 0..KNOTS.len() - p - 1 {
            let local = &KNOTS[i..i + p + 2];
            for u in samples() {
                if KNOTS.iter().any(|k| (k - u).abs() < 2.0 * h) {
                    continue;
                }
                let d = local_basis(local, u, Side::Right, 2);
                let f = |x: f64| local_basis(local, x, Side::Right, 0)[0];
                let fd1 = (f(u + h) - f(u - h)) / (2.0 * h);
                let df = |x: f64| local_basis(local, x, Side::Right, 1)[1];
                let fd2 = (df(u + h) - df(u - h)) / (2.0 * h);
                worst = worst.max((d[1] - fd1).abs()).max((d[2] - fd2).abs() / 100.0);
            }
        }
        assert!(worst < 1e-5, "derivative error {worst}");
    }

    #[test]
    fn single_function_knot_insertion_is_exact() {
        let mut rng = Lcg(7);
        for _ in 0..200 {
            let mut kn: Vec<f64> = (0..5).map(|_| (rng.unit() * 8.0).round() / 8.0).collect();
            kn.sort_by(f64::total_cmp);
            if kn[0] == kn[4] {
                continue;
            }
            let k = kn[0] + (kn[4] - kn[0]) * (rng.unit() * 8.0).round() / 8.0;
            let (c1, c2) = split_basis(&kn, k).unwrap();
            let mut merged = kn.clone();
            let at = merged.iter().position(|&x| x > k).unwrap_or(5);
            merged.insert(at, k);
            for u in samples() {
                let u = kn[0] - 0.1 + u * (kn[4] - kn[0] + 0.2);
                let f = |k: &[f64]| local_basis(k, u, Side::Right, 0)[0];
                let lhs = f(&kn);
                let rhs = c1 * f(&merged[..5]) + c2 * f(&merged[1..]);
                assert!((lhs - rhs).abs() < 1e-14, "{kn:?} insert {k} at {u}: {lhs} vs {rhs}");
            }
        }
    }

    #[test]
    fn basis_bezier_coefficients_reproduce_the_function() {
        let p = 3;
        let spans: Vec<(f64, f64)> = KNOTS.windows(2).filter(|w| w[0] < w[1]).map(|w| (w[0], w[1])).collect();
        for i in 0..KNOTS.len() - p - 1 {
            let local = &KNOTS[i..i + p + 2];
            for &(a, b) in &spans {
                let c = basis_bezier(local, a, b).unwrap();
                for k in 0..=20 {
                    let x = k as f64 / 20.0;
                    let u = a + (b - a) * x;
                    let side = if x >= 1.0 { Side::Left } else { Side::Right };
                    let want = local_basis(local, u, side, 0)[0];
                    let got: f64 = c.iter().enumerate().map(|(j, cj)| cj * bernstein(p, j, x)).sum();
                    assert!((want - got).abs() < 1e-14, "N{i} on [{a}, {b}] at {u}: {want} vs {got}");
                }
            }
        }
        assert!(basis_bezier(&KNOTS[..5], 0.0, 0.5).is_err(), "a knot inside the interval is refused");
        assert!(basis_bezier(&KNOTS[..5], f64::NAN, 0.5).is_err());
    }

    fn curve(p: usize, knots: &[f64], ctrl: &[HPoint], u: f64) -> Vec3 {
        let n = ctrl.len();
        let side = if u >= knots[n] { Side::Left } else { Side::Right };
        let mut acc = HPoint::ZERO;
        for (i, c) in ctrl.iter().enumerate() {
            acc += *c * local_basis(&knots[i..i + p + 2], u, side, 0)[0];
        }
        acc.project().unwrap()
    }

    #[test]
    fn boehm_insertion_keeps_the_curve() {
        let p = 3;
        let mut rng = Lcg(3);
        let ctrl: Vec<HPoint> = (0..8).map(|_| HPoint::weighted(Vec3::new(rng.unit(), rng.unit(), rng.unit()) * 10.0, 0.5 + rng.unit())).collect();
        let (mut kn, mut pts) = (KNOTS.to_vec(), ctrl.clone());
        for u in [0.0, 0.1, 0.3, 0.3, 0.77, 1.0, 1.0] {
            (kn, pts) = insert_knot(p, &kn, &pts, u).unwrap();
        }
        let mut worst = 0.0f64;
        for u in samples() {
            worst = worst.max(curve(p, &KNOTS, &ctrl, u).dist(curve(p, &kn, &pts, u)));
        }
        assert!(worst < 1e-13, "deviation {worst}");
        assert!(insert_knot(p, &KNOTS, &ctrl, 1.5).is_err());
        assert!(insert_knot(p, &KNOTS, &ctrl, f64::NAN).is_err());
    }

    pub(crate) fn random_surface(seed: u64, ku: Vec<f64>, kv: Vec<f64>, degree: [usize; 2]) -> NurbsSurface {
        let mut rng = Lcg(seed);
        let n = (ku.len() - degree[0] - 1) * (kv.len() - degree[1] - 1);
        let pts: Vec<Vec3> = (0..n).map(|_| Vec3::new(rng.unit(), rng.unit(), rng.unit()) * 10.0).collect();
        let w: Vec<f64> = (0..n).map(|_| 0.5 + rng.unit()).collect();
        NurbsSurface::new(degree, [ku, kv], &pts, &w).unwrap()
    }

    fn max_deviation(a: &NurbsSurface, b: &NurbsSurface) -> f64 {
        let [du, dv] = a.domain();
        let mut worst = 0.0f64;
        for i in 0..=30 {
            for j in 0..=30 {
                let u = du[0] + (du[1] - du[0]) * i as f64 / 30.0;
                let v = dv[0] + (dv[1] - dv[0]) * j as f64 / 30.0;
                worst = worst.max(a.eval(u, v).unwrap().dist(b.eval(u, v).unwrap()));
            }
        }
        worst
    }

    #[test]
    fn surface_knot_insertion_and_clamping_keep_the_surface() {
        let uniform: Vec<f64> = (0..10).map(|k| k as f64).collect();
        let s = random_surface(11, uniform.clone(), (0..9).map(|k| k as f64 * 0.5).collect(), [3, 3]);
        let r = s.insert_knot(Dir::S, 4.5).unwrap().insert_knot(Dir::T, 2.25).unwrap();
        assert!(max_deviation(&s, &r) < 1e-12);
        let c = s.clamped().unwrap();
        assert_eq!(c.domain(), s.domain());
        assert_eq!(&c.knots()[0][..4], &[3.0; 4]);
        assert_eq!(&c.knots()[1][c.knots()[1].len() - 4..], &[2.5; 4]);
        let dev = max_deviation(&s, &c);
        assert!(dev < 1e-12, "clamping moved the surface by {dev}");
        // A clamped surface interpolates its corner control points.
        let corner = c.eval(3.0, 1.5).unwrap();
        assert!(corner.dist(c.point(0, 0).project().unwrap()) < 1e-12);
    }

    #[test]
    fn bezier_segments_lie_on_the_curve() {
        let mut rng = Lcg(5);
        let ctrl: Vec<HPoint> = (0..4).map(|_| HPoint::weighted(Vec3::new(rng.unit(), rng.unit(), 0.0), 0.5 + rng.unit())).collect();
        let seg = bezier_segment(&ctrl, 0.2, 0.7).unwrap();
        for k in 0..=10 {
            let x = k as f64 / 10.0;
            let want = bezier_point(&ctrl, 0.2 + 0.5 * x).project().unwrap();
            assert!(bezier_point(&seg, x).project().unwrap().dist(want) < 1e-14);
        }
        assert!(bezier_segment(&ctrl, 0.7, 0.2).is_err());
    }

    #[test]
    fn hostile_spline_input_is_refused() {
        let pts = [Vec3::ZERO; 16];
        let w = [1.0; 16];
        let k = vec![0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0];
        assert!(NurbsSurface::new([3, 3], [k.clone(), k.clone()], &pts, &w).is_ok());
        assert!(NurbsSurface::new([3, 3], [vec![0.0, f64::NAN, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0], k.clone()], &pts, &w).is_err());
        assert!(NurbsSurface::new([3, 3], [k.clone(), k.clone()], &pts, &[0.0; 16]).is_err());
        assert!(NurbsSurface::new([3, 3], [k.clone(), k.clone()], &pts[..15], &w[..15]).is_err());
        assert!(NurbsSurface::new([0, 3], [k.clone(), k.clone()], &pts, &w).is_err());
        assert!(NurbsSurface::new([9, 3], [k.clone(), k.clone()], &pts, &w).is_err());
        assert!(NurbsSurface::new([3, 3], [vec![1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0], k.clone()], &pts, &w).is_err());
        assert!(check_knots(&vec![0.0; MAX_KNOTS + 1]).is_err());
        let s = NurbsSurface::new([3, 3], [k.clone(), k], &pts, &w).unwrap();
        assert!(s.eval(2.0, 0.5).is_err());
        assert!(s.eval(f64::NAN, 0.5).is_err());
        assert_eq!(local_basis(&[], 0.5, Side::Right, 1), vec![0.0, 0.0]);
    }
}