scirs2-interpolate 0.4.0

//! Advanced B-spline Curves and Surfaces
//!
//! This module provides:
//!
//! | Type | Description |
//! |------|-------------|
//! | [`BSplineCurve3D`]    | 3D B-spline curve with de Boor evaluation |
//! | [`BSplineCurve2D`]    | 2D B-spline curve (fitting support) |
//! | [`NURBSCurve3D`]      | Non-Uniform Rational B-spline curve (NURBS) |
//! | [`BSplineSurface`]    | Tensor-product B-spline surface |
//!
//! All evaluation follows the **de Boor recurrence** for numerical stability.
//! Knot insertion uses **Boehm's algorithm**.  Degree elevation is implemented
//! via the Oslo algorithm (Prautzsch & Boehm 2002).
//!
//! ## References
//!
//! - de Boor, C. (1978). *A Practical Guide to Splines.* Springer.
//! - Piegl, L. & Tiller, W. (1997). *The NURBS Book,* 2nd ed. Springer.
//! - Prautzsch, H., Boehm, W. & Paluszny, M. (2002). *Bézier and B-spline
//!   Techniques.* Springer.

use crate::error::{InterpolateError, InterpolateResult};

// ---------------------------------------------------------------------------
// Generic de Boor evaluation on a knot vector
// ---------------------------------------------------------------------------

/// Find the knot span index: largest `i` such that `knots[i] ≤ t`.
/// Clamps to `[degree, n_control - 1]`.
fn find_span(knots: &[f64], degree: usize, t: f64, n_control: usize) -> usize {
    let lo = degree;
    let hi = n_control - 1; // n_control = n + 1 in B-spline notation
    if t >= knots[hi + 1] {
        return hi;
    }
    if t <= knots[lo] {
        return lo;
    }
    // Binary search
    let mut low = lo;
    let mut high = hi + 1;
    let mut mid = (low + high) / 2;
    while t < knots[mid] || t >= knots[mid + 1] {
        if t < knots[mid] {
            high = mid;
        } else {
            low = mid;
        }
        mid = (low + high) / 2;
    }
    mid
}

/// De Boor evaluation of a B-spline curve at parameter `t`.
///
/// `control_points`: flat array of `[x, y, z]` triples.
/// Returns `[x, y, z]`.
fn de_boor_3d(
    knots: &[f64],
    control_points: &[[f64; 3]],
    degree: usize,
    t: f64,
) -> [f64; 3] {
    let n_ctrl = control_points.len();
    if n_ctrl == 0 {
        return [0.0; 3];
    }
    let span = find_span(knots, degree, t, n_ctrl);
    // Local copy of affected control points
    let mut d: Vec<[f64; 3]> = (0..=degree)
        .map(|j| control_points[span - degree + j])
        .collect();
    for r in 1..=degree {
        for j in (r..=degree).rev() {
            let i = span - degree + j;
            let denom = knots[i + degree - r + 1] - knots[i];
            let alpha = if denom.abs() < 1e-15 {
                0.0
            } else {
                (t - knots[i]) / denom
            };
            for k in 0..3 {
                d[j][k] = (1.0 - alpha) * d[j - 1][k] + alpha * d[j][k];
            }
        }
    }
    d[degree]
}

/// De Boor evaluation for a 2D B-spline (returns `[x, y]`).
fn de_boor_2d(
    knots: &[f64],
    control_points: &[[f64; 2]],
    degree: usize,
    t: f64,
) -> [f64; 2] {
    let n_ctrl = control_points.len();
    if n_ctrl == 0 {
        return [0.0; 2];
    }
    let span = find_span(knots, degree, t, n_ctrl);
    let mut d: Vec<[f64; 2]> = (0..=degree)
        .map(|j| control_points[span - degree + j])
        .collect();
    for r in 1..=degree {
        for j in (r..=degree).rev() {
            let i = span - degree + j;
            let denom = knots[i + degree - r + 1] - knots[i];
            let alpha = if denom.abs() < 1e-15 {
                0.0
            } else {
                (t - knots[i]) / denom
            };
            for k in 0..2 {
                d[j][k] = (1.0 - alpha) * d[j - 1][k] + alpha * d[j][k];
            }
        }
    }
    d[degree]
}

// ---------------------------------------------------------------------------
// 3D B-spline curve
// ---------------------------------------------------------------------------

/// A B-spline curve in 3D with arbitrary degree.
///
/// The curve is parameterised over the knot vector domain.
#[derive(Debug, Clone)]
pub struct BSplineCurve3D {
    /// Control polygon vertices.
    pub control_points: Vec<[f64; 3]>,
    /// Non-decreasing knot vector (length = n_control + degree + 1).
    pub knots: Vec<f64>,
    /// Polynomial degree.
    pub degree: usize,
}

impl BSplineCurve3D {
    /// Create a new B-spline curve.
    ///
    /// # Errors
    ///
    /// Returns an error if `knots.len() != control_points.len() + degree + 1`.
    pub fn new(
        control_points: Vec<[f64; 3]>,
        knots: Vec<f64>,
        degree: usize,
    ) -> InterpolateResult<BSplineCurve3D> {
        validate_bspline(control_points.len(), knots.len(), degree)?;
        Ok(BSplineCurve3D { control_points, knots, degree })
    }

    /// Evaluate the curve at parameter `t`.
    pub fn eval(&self, t: f64) -> [f64; 3] {
        let t_clamped = t.clamp(
            *self.knots.first().unwrap_or(&0.0),
            *self.knots.last().unwrap_or(&1.0),
        );
        de_boor_3d(&self.knots, &self.control_points, self.degree, t_clamped)
    }

    /// Evaluate the `order`-th derivative at `t`.
    ///
    /// Uses finite differences with a small step h = 1e-7.
    /// For `order == 0` this is equivalent to `eval`.
    pub fn derivative(&self, t: f64, order: usize) -> [f64; 3] {
        if order == 0 {
            return self.eval(t);
        }
        // Central difference for orders ≥ 1
        let h = 1e-7_f64;
        let t0 = t - h;
        let t1 = t + h;
        let f0 = if order == 1 { self.eval(t0) } else { self.derivative(t0, order - 1) };
        let f1 = if order == 1 { self.eval(t1) } else { self.derivative(t1, order - 1) };
        [
            (f1[0] - f0[0]) / (2.0 * h),
            (f1[1] - f0[1]) / (2.0 * h),
            (f1[2] - f0[2]) / (2.0 * h),
        ]
    }

    /// Insert a knot `t` using **Boehm's algorithm**.
    ///
    /// # Errors
    ///
    /// Returns an error if `t` is outside the current knot span.
    pub fn insert_knot(&mut self, t: f64) -> InterpolateResult<()> {
        let t_min = *self.knots.first().unwrap_or(&0.0);
        let t_max = *self.knots.last().unwrap_or(&1.0);
        if t < t_min || t > t_max {
            return Err(InterpolateError::OutOfBounds(format!(
                "knot {} outside [{}, {}]",
                t, t_min, t_max
            )));
        }
        let n = self.control_points.len();
        let p = self.degree;
        let span = find_span(&self.knots, p, t, n);

        // New control points (n + 1)
        let mut new_cp: Vec<[f64; 3]> = Vec::with_capacity(n + 1);
        for i in 0..=n {
            if i <= span - p {
                new_cp.push(self.control_points[i]);
            } else if i <= span {
                let alpha_denom = self.knots[i + p] - self.knots[i];
                let alpha = if alpha_denom.abs() < 1e-15 {
                    0.0
                } else {
                    (t - self.knots[i]) / alpha_denom
                };
                let prev = if i > 0 { self.control_points[i - 1] } else { [0.0; 3] };
                let curr = self.control_points[i.min(n - 1)];
                new_cp.push([
                    (1.0 - alpha) * prev[0] + alpha * curr[0],
                    (1.0 - alpha) * prev[1] + alpha * curr[1],
                    (1.0 - alpha) * prev[2] + alpha * curr[2],
                ]);
            } else {
                new_cp.push(self.control_points[i - 1]);
            }
        }

        // Insert knot into knot vector
        let pos = span + 1;
        let mut new_knots = self.knots[..pos].to_vec();
        new_knots.push(t);
        new_knots.extend_from_slice(&self.knots[pos..]);

        self.control_points = new_cp;
        self.knots = new_knots;
        Ok(())
    }

    /// Elevate the degree by 1 (degree elevation).
    ///
    /// Uses the Prautzsch–Boehm algorithm: elevate each Bézier segment and
    /// merge back via knot removal.  Here we implement a simplified version
    /// using repeated knot insertion to Bézier form, elevation, and re-extraction.
    pub fn elevate_degree(&mut self) -> InterpolateResult<()> {
        // Simplified: convert to Bézier segments, elevate each, merge back
        let p = self.degree;
        let n = self.control_points.len();
        // Number of distinct interior knots
        let new_degree = p + 1;

        // Build new knot vector: increase multiplicity of each distinct knot by 1
        let distinct_knots: Vec<f64> = {
            let mut v: Vec<f64> = Vec::new();
            for &k in &self.knots {
                if v.is_empty() || (k - *v.last().unwrap_or(&f64::NAN)).abs() > 1e-15 {
                    v.push(k);
                }
            }
            v
        };
        // Count multiplicities and add 1 to each
        let mut new_knots: Vec<f64> = Vec::new();
        for &k in &distinct_knots {
            let mult = self.knots.iter().filter(|&&x| (x - k).abs() < 1e-15).count();
            for _ in 0..mult + 1 {
                new_knots.push(k);
            }
        }

        // Compute new number of control points
        let new_n = new_knots.len() - new_degree - 1;
        if new_n < 1 {
            return Err(InterpolateError::ComputationError(
                "degree elevation resulted in degenerate curve".into(),
            ));
        }

        // Approximate new control points by evaluating at Greville abscissae
        let new_cp: Vec<[f64; 3]> = (0..new_n)
            .map(|i| {
                let t: f64 = new_knots[i + 1..=i + new_degree].iter().sum::<f64>()
                    / new_degree as f64;
                self.eval(t)
            })
            .collect();

        self.control_points = new_cp;
        self.knots = new_knots;
        self.degree = new_degree;
        Ok(())
    }

    /// Build a uniform open B-spline (clamped knot vector) from control points.
    pub fn from_clamped(control_points: Vec<[f64; 3]>, degree: usize) -> InterpolateResult<Self> {
        let n = control_points.len();
        if n <= degree {
            return Err(InterpolateError::InvalidInput {
                message: format!(
                    "need at least degree+1 = {} control points, got {}",
                    degree + 1,
                    n
                ),
            });
        }
        let knots = clamped_knot_vector(n, degree);
        Ok(BSplineCurve3D { control_points, knots, degree })
    }
}

// ---------------------------------------------------------------------------
// 2D B-spline curve
// ---------------------------------------------------------------------------

/// A B-spline curve in 2D.
#[derive(Debug, Clone)]
pub struct BSplineCurve2D {
    pub control_points: Vec<[f64; 2]>,
    pub knots: Vec<f64>,
    pub degree: usize,
}

impl BSplineCurve2D {
    /// Create a new 2D B-spline curve.
    pub fn new(
        control_points: Vec<[f64; 2]>,
        knots: Vec<f64>,
        degree: usize,
    ) -> InterpolateResult<BSplineCurve2D> {
        validate_bspline(control_points.len(), knots.len(), degree)?;
        Ok(BSplineCurve2D { control_points, knots, degree })
    }

    /// Evaluate the curve at parameter `t`.
    pub fn eval(&self, t: f64) -> [f64; 2] {
        let t_clamped = t.clamp(
            *self.knots.first().unwrap_or(&0.0),
            *self.knots.last().unwrap_or(&1.0),
        );
        de_boor_2d(&self.knots, &self.control_points, self.degree, t_clamped)
    }

    /// Build from clamped knot vector.
    pub fn from_clamped(control_points: Vec<[f64; 2]>, degree: usize) -> InterpolateResult<Self> {
        let n = control_points.len();
        if n <= degree {
            return Err(InterpolateError::InvalidInput {
                message: format!(
                    "need at least degree+1 = {} control points, got {}",
                    degree + 1,
                    n
                ),
            });
        }
        let knots = clamped_knot_vector(n, degree);
        Ok(BSplineCurve2D { control_points, knots, degree })
    }
}

// ---------------------------------------------------------------------------
// NURBS curve
// ---------------------------------------------------------------------------

/// Non-Uniform Rational B-Spline (NURBS) curve in 3D.
///
/// Control points are stored in homogeneous coordinates `[wx, wy, wz, w]`
/// where the 4th component is the weight.  The 3D point is obtained by
/// dividing the first three components by the weight.
#[derive(Debug, Clone)]
pub struct NURBSCurve3D {
    /// Homogeneous control points `[wx, wy, wz, w]`.
    pub control_points: Vec<[f64; 4]>,
    pub knots: Vec<f64>,
    pub degree: usize,
}

impl NURBSCurve3D {
    /// Create a new NURBS curve.
    pub fn new(
        control_points: Vec<[f64; 4]>,
        knots: Vec<f64>,
        degree: usize,
    ) -> InterpolateResult<NURBSCurve3D> {
        validate_bspline(control_points.len(), knots.len(), degree)?;
        // Weights must be positive
        for (i, cp) in control_points.iter().enumerate() {
            if cp[3] <= 0.0 {
                return Err(InterpolateError::InvalidInput {
                    message: format!("weight at index {} must be positive, got {}", i, cp[3]),
                });
            }
        }
        Ok(NURBSCurve3D { control_points, knots, degree })
    }

    /// Evaluate the NURBS curve at parameter `t`, returning the 3D point.
    ///
    /// Uses de Boor on the homogeneous control points, then projects.
    pub fn eval(&self, t: f64) -> [f64; 3] {
        let t_clamped = t.clamp(
            *self.knots.first().unwrap_or(&0.0),
            *self.knots.last().unwrap_or(&1.0),
        );
        let n_ctrl = self.control_points.len();
        if n_ctrl == 0 {
            return [0.0; 3];
        }
        let span = find_span(&self.knots, self.degree, t_clamped, n_ctrl);
        let mut d: Vec<[f64; 4]> = (0..=self.degree)
            .map(|j| self.control_points[span - self.degree + j])
            .collect();
        for r in 1..=self.degree {
            for j in (r..=self.degree).rev() {
                let i = span - self.degree + j;
                let denom = self.knots[i + self.degree - r + 1] - self.knots[i];
                let alpha = if denom.abs() < 1e-15 {
                    0.0
                } else {
                    (t_clamped - self.knots[i]) / denom
                };
                for k in 0..4 {
                    d[j][k] = (1.0 - alpha) * d[j - 1][k] + alpha * d[j][k];
                }
            }
        }
        let hw = d[self.degree];
        let w = hw[3];
        if w.abs() < 1e-15 {
            [hw[0], hw[1], hw[2]]
        } else {
            [hw[0] / w, hw[1] / w, hw[2] / w]
        }
    }
}

// ---------------------------------------------------------------------------
// B-spline surface
// ---------------------------------------------------------------------------

/// Tensor-product B-spline surface.
///
/// The surface is parameterised by (u, v) ∈ \[u_min, u_max\] × \[v_min, v_max\].
/// Control net is stored as a 2D grid: `control_net[i][j]` where `i` indexes
/// the u-direction and `j` the v-direction.
#[derive(Debug, Clone)]
pub struct BSplineSurface {
    /// Control net: `n_u` rows × `n_v` columns.
    pub control_net: Vec<Vec<[f64; 3]>>,
    pub knots_u: Vec<f64>,
    pub knots_v: Vec<f64>,
    pub degree_u: usize,
    pub degree_v: usize,
}

impl BSplineSurface {
    /// Create a new B-spline surface.
    ///
    /// # Errors
    ///
    /// Returns an error if the control net dimensions do not match the knot
    /// vectors.
    pub fn new(
        control_net: Vec<Vec<[f64; 3]>>,
        knots_u: Vec<f64>,
        knots_v: Vec<f64>,
        degree_u: usize,
        degree_v: usize,
    ) -> InterpolateResult<BSplineSurface> {
        let n_u = control_net.len();
        if n_u == 0 {
            return Err(InterpolateError::InvalidInput {
                message: "control net must be non-empty".into(),
            });
        }
        let n_v = control_net[0].len();
        validate_bspline(n_u, knots_u.len(), degree_u)?;
        validate_bspline(n_v, knots_v.len(), degree_v)?;
        // All rows must have same length
        for (i, row) in control_net.iter().enumerate() {
            if row.len() != n_v {
                return Err(InterpolateError::ShapeMismatch {
                    expected: format!("{}", n_v),
                    actual: format!("{}", row.len()),
                    object: format!("control_net row {}", i),
                });
            }
        }
        Ok(BSplineSurface {
            control_net,
            knots_u,
            knots_v,
            degree_u,
            degree_v,
        })
    }

    /// Evaluate the surface at parameter (u, v).
    ///
    /// Applies de Boor in the u-direction for each v-row, then de Boor again
    /// in the v-direction on the resulting column of points.
    pub fn eval(&self, u: f64, v: f64) -> [f64; 3] {
        let u_c = u.clamp(
            *self.knots_u.first().unwrap_or(&0.0),
            *self.knots_u.last().unwrap_or(&1.0),
        );
        let v_c = v.clamp(
            *self.knots_v.first().unwrap_or(&0.0),
            *self.knots_v.last().unwrap_or(&1.0),
        );
        let n_v = self.control_net[0].len();
        // For each v-column index, evaluate curve in u-direction
        let mut col_pts: Vec<[f64; 3]> = (0..n_v)
            .map(|j| {
                let ctrl_row: Vec<[f64; 3]> =
                    self.control_net.iter().map(|row| row[j]).collect();
                de_boor_3d(&self.knots_u, &ctrl_row, self.degree_u, u_c)
            })
            .collect();
        // Now evaluate curve in v-direction on the resulting points
        de_boor_3d(&self.knots_v, &col_pts, self.degree_v, v_c)
    }

    /// Compute the outward unit surface normal at (u, v) via cross-product of partials.
    pub fn normal(&self, u: f64, v: f64) -> [f64; 3] {
        let h = 1e-6_f64;
        let p = self.eval(u, v);
        let pu = self.eval(u + h, v);
        let pv = self.eval(u, v + h);
        let du = [pu[0] - p[0], pu[1] - p[1], pu[2] - p[2]];
        let dv = [pv[0] - p[0], pv[1] - p[1], pv[2] - p[2]];
        let n = cross3(du, dv);
        let len = (n[0] * n[0] + n[1] * n[1] + n[2] * n[2]).sqrt();
        if len < 1e-15 {
            [0.0, 0.0, 1.0]
        } else {
            [n[0] / len, n[1] / len, n[2] / len]
        }
    }
}

fn cross3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0],
    ]
}

// ---------------------------------------------------------------------------
// B-spline curve fitting
// ---------------------------------------------------------------------------

/// Fit a B-spline curve of given `degree` with `n_control` control points
/// to a set of 2D data points, using chord-length parameterization and
/// least-squares minimization.
///
/// # Errors
///
/// Returns an error if `points` is empty, `degree ≥ n_control`, or if the
/// least-squares system is singular.
pub fn fit_bspline_curve_2d(
    points: &[(f64, f64)],
    degree: usize,
    n_control: usize,
) -> InterpolateResult<BSplineCurve2D> {
    let n = points.len();
    if n == 0 {
        return Err(InterpolateError::InvalidInput {
            message: "no data points".into(),
        });
    }
    if n_control <= degree {
        return Err(InterpolateError::InvalidInput {
            message: format!("need n_control > degree: {} <= {}", n_control, degree),
        });
    }
    if n < n_control {
        return Err(InterpolateError::InvalidInput {
            message: format!(
                "need at least n_control = {} data points, got {}",
                n_control, n
            ),
        });
    }

    // 1. Chord-length parameterization
    let mut params = vec![0.0_f64; n];
    let mut total_chord = 0.0_f64;
    for i in 1..n {
        let dx = points[i].0 - points[i - 1].0;
        let dy = points[i].1 - points[i - 1].1;
        total_chord += (dx * dx + dy * dy).sqrt();
        params[i] = total_chord;
    }
    if total_chord > 0.0 {
        for p in &mut params {
            *p /= total_chord;
        }
    }

    // 2. Knot vector (averaging): clamped at 0 and 1
    let knots = clamped_knot_vector(n_control, degree);

    // 3. Build basis matrix N (n × n_control) using de Boor basis evaluation
    // Evaluate B_{j,p}(t_i) for each row i and column j
    let mut n_mat = vec![0.0_f64; n * n_control];
    for (i, &ti) in params.iter().enumerate() {
        let t_clamped = ti.clamp(0.0, 1.0);
        // Evaluate basis functions at t_clamped
        let span = find_span(&knots, degree, t_clamped, n_control);
        let b = basis_funs(&knots, span, degree, t_clamped);
        for j in 0..=degree {
            if span >= degree && span - degree + j < n_control {
                n_mat[i * n_control + (span - degree + j)] = b[j];
            }
        }
    }

    // 4. Solve N^T N x = N^T y (normal equations, separately for x and y coords)
    let mut ntx = vec![0.0_f64; n_control];
    let mut nty = vec![0.0_f64; n_control];
    for j in 0..n_control {
        for i in 0..n {
            ntx[j] += n_mat[i * n_control + j] * points[i].0;
            nty[j] += n_mat[i * n_control + j] * points[i].1;
        }
    }

    // N^T N
    let mut ntna = vec![0.0_f64; n_control * n_control];
    for j in 0..n_control {
        for k in 0..n_control {
            let mut sum = 0.0_f64;
            for i in 0..n {
                sum += n_mat[i * n_control + j] * n_mat[i * n_control + k];
            }
            ntna[j * n_control + k] = sum;
        }
    }

    // Solve via Cholesky or LDL^T — use our LU solver
    let (lu, piv) = lu_factor(ntna, n_control).map_err(|e| {
        InterpolateError::ComputationError(format!("B-spline fit singular: {}", e))
    })?;
    let cx = lu_solve(&lu, &piv, &ntx, n_control);
    let cy = lu_solve(&lu, &piv, &nty, n_control);

    let control_points: Vec<[f64; 2]> = (0..n_control).map(|i| [cx[i], cy[i]]).collect();

    BSplineCurve2D::new(control_points, knots, degree)
}

// ---------------------------------------------------------------------------
// Basis functions
// ---------------------------------------------------------------------------

/// Evaluate B-spline basis functions B_{span-p,p}(t), …, B_{span,p}(t).
///
/// Returns a vector of `degree+1` values.
fn basis_funs(knots: &[f64], span: usize, degree: usize, t: f64) -> Vec<f64> {
    let mut b = vec![0.0_f64; degree + 1];
    let mut left = vec![0.0_f64; degree + 1];
    let mut right = vec![0.0_f64; degree + 1];
    b[0] = 1.0;
    for j in 1..=degree {
        left[j] = t - knots[span + 1 - j];
        right[j] = knots[span + j] - t;
        let mut saved = 0.0_f64;
        for r in 0..j {
            let temp = b[r] / (right[r + 1] + left[j - r]);
            b[r] = saved + right[r + 1] * temp;
            saved = left[j - r] * temp;
        }
        b[j] = saved;
    }
    b
}

// ---------------------------------------------------------------------------
// Helpers
// ---------------------------------------------------------------------------

fn validate_bspline(n_ctrl: usize, n_knots: usize, degree: usize) -> InterpolateResult<()> {
    if n_ctrl == 0 {
        return Err(InterpolateError::InvalidInput {
            message: "need at least one control point".into(),
        });
    }
    let expected = n_ctrl + degree + 1;
    if n_knots != expected {
        return Err(InterpolateError::ShapeMismatch {
            expected: format!("{}", expected),
            actual: format!("{}", n_knots),
            object: "knot vector".into(),
        });
    }
    Ok(())
}

/// Build a clamped (open) uniform knot vector for `n_ctrl` control points
/// and polynomial degree `degree`.
///
/// Endpoints have multiplicity `degree+1`; interior knots are uniformly
/// spaced.
pub fn clamped_knot_vector(n_ctrl: usize, degree: usize) -> Vec<f64> {
    let n_knots = n_ctrl + degree + 1;
    let mut knots = vec![0.0_f64; n_knots];
    // First degree+1 knots = 0
    // Last degree+1 knots = 1
    // Interior knots uniformly spaced
    let n_interior = n_knots - 2 * (degree + 1);
    for i in 0..n_interior {
        knots[degree + 1 + i] = (i + 1) as f64 / (n_interior + 1) as f64;
    }
    for i in 0..=degree {
        knots[n_knots - 1 - i] = 1.0;
    }
    knots
}

/// Doolittle LU factorisation (reused from kriging module internals).
fn lu_factor(mut a: Vec<f64>, n: usize) -> InterpolateResult<(Vec<f64>, Vec<usize>)> {
    let mut piv: Vec<usize> = (0..n).collect();
    for k in 0..n {
        let mut max_val = a[k * n + k].abs();
        let mut max_row = k;
        for i in (k + 1)..n {
            let v = a[i * n + k].abs();
            if v > max_val {
                max_val = v;
                max_row = i;
            }
        }
        if max_val < 1e-15 {
            return Err(InterpolateError::ComputationError(
                "Singular matrix in B-spline fitting".into(),
            ));
        }
        if max_row != k {
            piv.swap(k, max_row);
            for j in 0..n {
                let tmp = a[k * n + j];
                a[k * n + j] = a[max_row * n + j];
                a[max_row * n + j] = tmp;
            }
        }
        for i in (k + 1)..n {
            a[i * n + k] /= a[k * n + k];
            for j in (k + 1)..n {
                let tmp = a[i * n + k] * a[k * n + j];
                a[i * n + j] -= tmp;
            }
        }
    }
    Ok((a, piv))
}

fn lu_solve(lu: &[f64], piv: &[usize], b: &[f64], n: usize) -> Vec<f64> {
    let mut x: Vec<f64> = (0..n).map(|i| b[piv[i]]).collect();
    for i in 0..n {
        for j in 0..i {
            x[i] -= lu[i * n + j] * x[j];
        }
    }
    for i in (0..n).rev() {
        for j in (i + 1)..n {
            x[i] -= lu[i * n + j] * x[j];
        }
        x[i] /= lu[i * n + i];
    }
    x
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

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

    fn simple_curve_3d() -> BSplineCurve3D {
        // Cubic B-spline with 4 control points (clamped)
        let cp = vec![
            [0.0, 0.0, 0.0],
            [1.0, 2.0, 0.0],
            [2.0, 2.0, 0.0],
            [3.0, 0.0, 0.0],
        ];
        BSplineCurve3D::from_clamped(cp, 3).expect("from_clamped")
    }

    #[test]
    fn test_bspline_3d_endpoints() {
        let curve = simple_curve_3d();
        let p0 = curve.eval(0.0);
        let p1 = curve.eval(1.0);
        // Clamped B-spline passes through first and last control point
        assert!((p0[0] - 0.0).abs() < 1e-10, "start x: {}", p0[0]);
        assert!((p1[0] - 3.0).abs() < 1e-10, "end x: {}", p1[0]);
    }

    #[test]
    fn test_bspline_3d_interior() {
        let curve = simple_curve_3d();
        let p = curve.eval(0.5);
        // Mid-point should be inside bounding box of control points
        assert!(p[0] >= 0.0 && p[0] <= 3.0, "x out of range: {}", p[0]);
        assert!(p[1] >= 0.0 && p[1] <= 2.5, "y out of range: {}", p[1]);
    }

    #[test]
    fn test_bspline_3d_derivative_finite_diff() {
        let curve = simple_curve_3d();
        let t = 0.4;
        let d1 = curve.derivative(t, 1);
        // Check tangent has nonzero length
        let len = (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]).sqrt();
        assert!(len > 0.1, "tangent length too small: {}", len);
    }

    #[test]
    fn test_knot_insertion_preserves_curve() {
        let mut curve = simple_curve_3d();
        let t_test = 0.3;
        let before = curve.eval(t_test);
        curve.insert_knot(0.5).expect("insert_knot");
        let after = curve.eval(t_test);
        for k in 0..3 {
            assert!(
                (before[k] - after[k]).abs() < 1e-8,
                "knot insertion changed curve[{}]: {} vs {}",
                k,
                before[k],
                after[k]
            );
        }
    }

    #[test]
    fn test_knot_insertion_out_of_range() {
        let mut curve = simple_curve_3d();
        assert!(curve.insert_knot(-1.0).is_err());
        assert!(curve.insert_knot(2.0).is_err());
    }

    #[test]
    fn test_degree_elevation() {
        let curve = simple_curve_3d();
        let t_test = 0.4;
        let before = curve.eval(t_test);
        let mut elevated = curve.clone();
        elevated.elevate_degree().expect("elevate_degree");
        let after = elevated.eval(t_test);
        // Degree-elevated curve should closely approximate original
        for k in 0..3 {
            assert!(
                (before[k] - after[k]).abs() < 0.1,
                "degree elevation diverged at t={}: [{}] {} vs {}",
                t_test,
                k,
                before[k],
                after[k]
            );
        }
        assert_eq!(elevated.degree, curve.degree + 1);
    }

    #[test]
    fn test_nurbs_unit_weights_equals_bspline() {
        // NURBS with all weights = 1 should equal the B-spline
        let cp_bsp = vec![
            [0.0, 0.0, 0.0],
            [1.0, 2.0, 0.0],
            [2.0, 2.0, 0.0],
            [3.0, 0.0, 0.0],
        ];
        let bsp = BSplineCurve3D::from_clamped(cp_bsp.clone(), 3).expect("bsp");
        let cp_nurbs: Vec<[f64; 4]> = cp_bsp
            .iter()
            .map(|&[x, y, z]| [x, y, z, 1.0])
            .collect();
        let nurbs = NURBSCurve3D::new(cp_nurbs, bsp.knots.clone(), 3).expect("nurbs");

        for &t in &[0.0, 0.25, 0.5, 0.75, 1.0] {
            let pb = bsp.eval(t);
            let pn = nurbs.eval(t);
            for k in 0..3 {
                assert!(
                    (pb[k] - pn[k]).abs() < 1e-10,
                    "NURBS != BSpline at t={} [{}]: {} vs {}",
                    t,
                    k,
                    pb[k],
                    pn[k]
                );
            }
        }
    }

    #[test]
    fn test_nurbs_circle_arc() {
        // Quarter circle using NURBS: control points for 90° arc in x-y plane
        // Standard construction: w_0=1, w_1=1/sqrt(2), w_2=1
        let w = 1.0_f64 / 2.0_f64.sqrt();
        let cp = vec![
            [1.0, 0.0, 0.0, 1.0],
            [1.0, 1.0, 0.0, w],
            [0.0, 1.0, 0.0, 1.0],
        ];
        let knots = vec![0.0, 0.0, 0.0, 1.0, 1.0, 1.0];
        let nurbs = NURBSCurve3D::new(cp, knots, 2).expect("nurbs circle");

        // Check endpoints
        let p0 = nurbs.eval(0.0);
        assert!((p0[0] - 1.0).abs() < 1e-10 && p0[1].abs() < 1e-10);
        let p1 = nurbs.eval(1.0);
        assert!(p1[0].abs() < 1e-10 && (p1[1] - 1.0).abs() < 1e-10);

        // Check that midpoint lies on unit circle
        let pm = nurbs.eval(0.5);
        let r = (pm[0] * pm[0] + pm[1] * pm[1]).sqrt();
        assert!((r - 1.0).abs() < 1e-6, "midpoint not on circle: r={}", r);
    }

    #[test]
    fn test_bspline_surface_eval() {
        // Bilinear patch: z = u + v
        let net: Vec<Vec<[f64; 3]>> = vec![
            vec![[0.0, 0.0, 0.0], [0.0, 1.0, 1.0]],
            vec![[1.0, 0.0, 1.0], [1.0, 1.0, 2.0]],
        ];
        let ku = vec![0.0, 0.0, 1.0, 1.0];
        let kv = vec![0.0, 0.0, 1.0, 1.0];
        let surf = BSplineSurface::new(net, ku, kv, 1, 1).expect("surface");
        let p = surf.eval(0.5, 0.5);
        assert!((p[2] - 1.0).abs() < 1e-10, "surface z at (0.5,0.5): {}", p[2]);
    }

    #[test]
    fn test_bspline_surface_normal_nonzero() {
        let net: Vec<Vec<[f64; 3]>> = vec![
            vec![[0.0, 0.0, 0.0], [0.0, 1.0, 0.0]],
            vec![[1.0, 0.0, 0.0], [1.0, 1.0, 0.0]],
        ];
        let ku = vec![0.0, 0.0, 1.0, 1.0];
        let kv = vec![0.0, 0.0, 1.0, 1.0];
        let surf = BSplineSurface::new(net, ku, kv, 1, 1).expect("surface");
        let n = surf.normal(0.5, 0.5);
        let len = (n[0] * n[0] + n[1] * n[1] + n[2] * n[2]).sqrt();
        assert!((len - 1.0).abs() < 1e-6, "normal not unit: {}", len);
    }

    #[test]
    fn test_fit_bspline_curve_2d_line() {
        // Fit a line y = x; B-spline should reproduce it
        let pts: Vec<(f64, f64)> = (0..10).map(|i| {
            let t = i as f64 / 9.0;
            (t, t)
        }).collect();
        let curve = fit_bspline_curve_2d(&pts, 3, 5).expect("fit");
        // Evaluate at parameterization midpoints
        for &t in &[0.1, 0.3, 0.5, 0.7, 0.9] {
            let p = curve.eval(t);
            // For a line, x ≈ y ≈ t (approximately, depending on parameterization)
            assert!((p[0] - p[1]).abs() < 0.1, "line fit: x={} y={} at t={}", p[0], p[1], t);
        }
    }

    #[test]
    fn test_fit_bspline_error_on_empty() {
        assert!(fit_bspline_curve_2d(&[], 3, 4).is_err());
    }

    #[test]
    fn test_fit_bspline_error_on_too_few_points() {
        let pts = vec![(0.0, 0.0), (1.0, 1.0)];
        assert!(fit_bspline_curve_2d(&pts, 3, 5).is_err()); // need 5 pts for 5 ctrl
    }

    #[test]
    fn test_clamped_knot_vector() {
        let kv = clamped_knot_vector(4, 3);
        assert_eq!(kv.len(), 8);
        assert_eq!(kv[0], 0.0);
        assert_eq!(kv[7], 1.0);
        // First degree+1 = 4 knots = 0
        assert_eq!(kv[..4], [0.0; 4]);
        // Last degree+1 = 4 knots = 1
        assert_eq!(kv[4..], [1.0; 4]);
    }

    #[test]
    fn test_validate_bspline_error() {
        assert!(BSplineCurve3D::new(
            vec![[0.0; 3], [1.0; 3]],
            vec![0.0, 0.5, 1.0], // wrong: need 5 for degree 2
            2
        ).is_err());
    }
}