scirs2-interpolate 0.4.1

//! Bezier curve and surface implementations
//!
//! This module provides functionality for Bezier curves and surfaces, which
//! are parametric curves and surfaces commonly used in computer graphics and
//! computer-aided design.
//!
//! A Bezier curve is defined by control points, where the curve is guaranteed
//! to pass through the first and last control points, but generally not through
//! the intermediate control points which instead "pull" the curve toward them.

use crate::error::{InterpolateError, InterpolateResult};
use scirs2_core::ndarray::{Array1, Array2, ArrayView1, ArrayView2};
use scirs2_core::numeric::{Float, FromPrimitive};
use std::fmt::Debug;

/// Bezier curve defined by control points
///
/// A Bezier curve is a parametric curve defined by a set of control points.
/// The curve passes through the first and last control points, and is influenced
/// by (but does not necessarily pass through) the intermediate control points.
#[derive(Debug, Clone)]
pub struct BezierCurve<F: Float + FromPrimitive + Debug> {
    /// Control points defining the curve (n x dim)
    control_points: Array2<F>,
    /// Degree of the Bezier curve (number of control points - 1)
    degree: usize,
}

impl<F: Float + FromPrimitive + Debug + std::fmt::Display> BezierCurve<F> {
    /// Create a new Bezier curve from control points
    ///
    /// # Arguments
    ///
    /// * `control_points` - 2D array of shape (n, dim) containing n control points in dim-dimensional space
    ///
    /// # Returns
    ///
    /// A new `BezierCurve` object
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_core::ndarray::array;
    /// use scirs2_interpolate::bezier::BezierCurve;
    ///
    /// // Create a 2D cubic Bezier curve (degree 3) with 4 control points
    /// let control_points = array![
    ///     [0.0, 0.0],  // Start point
    ///     [1.0, 2.0],  // Control point 1
    ///     [3.0, 2.0],  // Control point 2
    ///     [4.0, 0.0]   // End point
    /// ];
    ///
    /// let curve = BezierCurve::new(&control_points.view()).expect("Operation failed");
    ///
    /// // Evaluate the curve at parameter t = 0.5 (midpoint)
    /// let point = curve.evaluate(0.5).expect("Operation failed");
    /// ```
    pub fn new(controlpoints: &ArrayView2<F>) -> InterpolateResult<Self> {
        if controlpoints.is_empty() {
            return Err(InterpolateError::invalid_input(
                "Control _points array cannot be empty".to_string(),
            ));
        }

        let degree = controlpoints.shape()[0] - 1;
        Ok(BezierCurve {
            control_points: controlpoints.to_owned(),
            degree,
        })
    }

    /// Get the control points of the Bezier curve
    pub fn control_points(&self) -> &Array2<F> {
        &self.control_points
    }

    /// Get the degree of the Bezier curve
    pub fn degree(&self) -> usize {
        self.degree
    }

    /// Evaluate the Bezier curve at parameter value t
    ///
    /// # Arguments
    ///
    /// * `t` - Parameter value in [0, 1]
    ///
    /// # Returns
    ///
    /// A point on the Bezier curve at parameter t
    pub fn evaluate(&self, t: F) -> InterpolateResult<Array1<F>> {
        if t < F::zero() || t > F::one() {
            return Err(InterpolateError::OutOfBounds(format!(
                "Parameter t must be in [0, 1], got {}",
                t
            )));
        }

        // Handle the boundary cases
        if t == F::zero() {
            return Ok(self.control_points.row(0).to_owned());
        }
        if t == F::one() {
            return Ok(self.control_points.row(self.degree).to_owned());
        }

        // Use De Casteljau's algorithm to compute the point on the curve
        let n = self.degree + 1;
        let dim = self.control_points.shape()[1];
        let mut points = self.control_points.clone();

        for r in 1..n {
            for i in 0..n - r {
                for d in 0..dim {
                    points[[i, d]] = (F::one() - t) * points[[i, d]] + t * points[[i + 1, d]];
                }
            }
        }

        Ok(points.row(0).to_owned())
    }

    /// Evaluate the Bezier curve at multiple parameter values
    ///
    /// # Arguments
    ///
    /// * `t_values` - Array of parameter values, each in [0, 1]
    ///
    /// # Returns
    ///
    /// Array of points on the Bezier curve at the given parameter values
    pub fn evaluate_array(&self, tvalues: &ArrayView1<F>) -> InterpolateResult<Array2<F>> {
        let n_points = tvalues.len();
        let dim = self.control_points.shape()[1];
        let mut result = Array2::zeros((n_points, dim));

        for (i, &t) in tvalues.iter().enumerate() {
            let point = self.evaluate(t)?;
            for d in 0..dim {
                result[[i, d]] = point[d];
            }
        }

        Ok(result)
    }

    /// Compute the derivative of the Bezier curve
    ///
    /// # Returns
    ///
    /// A new Bezier curve representing the derivative of this curve
    pub fn derivative(&self) -> InterpolateResult<Self> {
        if self.degree == 0 {
            // Derivative of a constant curve is a zero curve
            let dim = self.control_points.shape()[1];
            let zero_point = Array2::zeros((1, dim));
            return Self::new(&zero_point.view());
        }

        let dim = self.control_points.shape()[1];
        let mut deriv_control_points = Array2::zeros((self.degree, dim));

        // Derivative control points are scaled differences between original control points
        let n = F::from_usize(self.degree).expect("Operation failed");
        for i in 0..self.degree {
            for d in 0..dim {
                deriv_control_points[[i, d]] =
                    n * (self.control_points[[i + 1, d]] - self.control_points[[i, d]]);
            }
        }

        Self::new(&deriv_control_points.view())
    }

    /// Split the Bezier curve at parameter t
    ///
    /// # Arguments
    ///
    /// * `t` - Parameter value in [0, 1] at which to split the curve
    ///
    /// # Returns
    ///
    /// A tuple of two Bezier curves: (left part, right part)
    pub fn split(&self, t: F) -> InterpolateResult<(Self, Self)> {
        if t < F::zero() || t > F::one() {
            return Err(InterpolateError::OutOfBounds(format!(
                "Parameter t must be in [0, 1], got {}",
                t
            )));
        }

        if t == F::zero() {
            // Special case: split at beginning
            let left_point = self.control_points.row(0).to_owned();
            let left_point = left_point.insert_axis(scirs2_core::ndarray::Axis(0));
            let left = Self::new(&left_point.view())?;
            let right = self.clone();
            return Ok((left, right));
        }

        if t == F::one() {
            // Special case: split at end
            let right_point = self.control_points.row(self.degree).to_owned();
            let right_point = right_point.insert_axis(scirs2_core::ndarray::Axis(0));
            let right = Self::new(&right_point.view())?;
            let left = self.clone();
            return Ok((left, right));
        }

        // Use De Casteljau's algorithm to compute the new control points
        let n = self.degree + 1;
        let dim = self.control_points.shape()[1];

        // Initialize the triangle of points
        let mut triangle = vec![self.control_points.clone()];

        for r in 1..n {
            let mut new_points = Array2::zeros((n - r, dim));
            let prev_points = &triangle[r - 1];

            for i in 0..n - r {
                for d in 0..dim {
                    new_points[[i, d]] =
                        (F::one() - t) * prev_points[[i, d]] + t * prev_points[[i + 1, d]];
                }
            }

            triangle.push(new_points);
        }

        // Extract the control points for the left and right curves
        let mut left_control_points = Array2::zeros((n, dim));
        let mut right_control_points = Array2::zeros((n, dim));

        for (i, triangle_row) in triangle.iter().enumerate() {
            // Left curve: diagonal from top-left
            left_control_points.row_mut(i).assign(&triangle_row.row(0));

            // Right curve: diagonal from bottom-right
            right_control_points
                .row_mut(n - 1 - i)
                .assign(&triangle_row.row(n - 1 - i));
        }

        let left = Self::new(&left_control_points.view())?;
        let right = Self::new(&right_control_points.view())?;

        Ok((left, right))
    }
}

/// Bezier surface defined by a grid of control points
///
/// A Bezier surface is a tensor product surface defined by a grid of
/// control points. It can be thought of as a 2D extension of a Bezier curve.
#[derive(Debug, Clone)]
pub struct BezierSurface<F: Float + FromPrimitive + Debug> {
    /// Control points defining the surface (n x m x dim)
    control_points: Array2<F>,
    /// Number of control points in the u direction
    nu: usize,
    /// Number of control points in the v direction
    nv: usize,
    /// Dimension of the control points
    dim: usize,
}

impl<F: Float + FromPrimitive + Debug + std::fmt::Display> BezierSurface<F> {
    /// Create a new Bezier surface from a grid of control points
    ///
    /// # Arguments
    ///
    /// * `control_points` - 2D array of shape (nu*nv, dim) containing control points in row-major order
    /// * `nu` - Number of control points in the u direction
    /// * `nv` - Number of control points in the v direction
    ///
    /// # Returns
    ///
    /// A new `BezierSurface` object
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_core::ndarray::array;
    /// use scirs2_interpolate::bezier::BezierSurface;
    ///
    /// // Create a 3x3 grid of control points for a 3D Bezier surface
    /// let control_points = array![
    ///     [0.0, 0.0, 0.0],  // (0,0)
    ///     [1.0, 0.0, 1.0],  // (0,1)
    ///     [2.0, 0.0, 0.0],  // (0,2)
    ///     [0.0, 1.0, 1.0],  // (1,0)
    ///     [1.0, 1.0, 2.0],  // (1,1)
    ///     [2.0, 1.0, 1.0],  // (1,2)
    ///     [0.0, 2.0, 0.0],  // (2,0)
    ///     [1.0, 2.0, 1.0],  // (2,1)
    ///     [2.0, 2.0, 0.0],  // (2,2)
    /// ];
    ///
    /// let surface = BezierSurface::new(&control_points.view(), 3, 3).expect("Operation failed");
    ///
    /// // Evaluate the surface at parameters (u,v) = (0.5, 0.5)
    /// let point = surface.evaluate(0.5, 0.5).expect("Operation failed");
    /// ```
    pub fn new(controlpoints: &ArrayView2<F>, nu: usize, nv: usize) -> InterpolateResult<Self> {
        if controlpoints.is_empty() {
            return Err(InterpolateError::invalid_input(
                "Control _points array cannot be empty".to_string(),
            ));
        }

        if nu == 0 || nv == 0 {
            return Err(InterpolateError::invalid_input(
                "Number of control _points in each direction must be positive".to_string(),
            ));
        }

        if controlpoints.shape()[0] != nu * nv {
            return Err(InterpolateError::invalid_input(format!(
                "Expected {} control _points for a {}x{} grid, got {}",
                nu * nv,
                nu,
                nv,
                controlpoints.shape()[0]
            )));
        }

        let dim = controlpoints.shape()[1];

        Ok(BezierSurface {
            control_points: controlpoints.to_owned(),
            nu,
            nv,
            dim,
        })
    }

    /// Get the control points of the Bezier surface
    pub fn control_points(&self) -> &Array2<F> {
        &self.control_points
    }

    /// Get the dimensions of the control point grid
    pub fn grid_dimensions(&self) -> (usize, usize) {
        (self.nu, self.nv)
    }

    /// Evaluate the Bezier surface at parameters (u, v)
    ///
    /// # Arguments
    ///
    /// * `u` - Parameter value in the first direction, in [0, 1]
    /// * `v` - Parameter value in the second direction, in [0, 1]
    ///
    /// # Returns
    ///
    /// A point on the Bezier surface at parameters (u, v)
    pub fn evaluate(&self, u: F, v: F) -> InterpolateResult<Array1<F>> {
        if u < F::zero() || u > F::one() || v < F::zero() || v > F::one() {
            return Err(InterpolateError::OutOfBounds(format!(
                "Parameters (u,v) must be in [0, 1]x[0, 1], got ({}, {})",
                u, v
            )));
        }

        // Evaluate using tensor product of Bernstein polynomials
        let mut result = Array1::zeros(self.dim);

        // Precompute Bernstein polynomials for u and v
        let u_bernstein = compute_bernstein_all(u, self.nu - 1)?;
        let v_bernstein = compute_bernstein_all(v, self.nv - 1)?;

        // Tensor product evaluation
        for i in 0..self.nu {
            for j in 0..self.nv {
                let idx = i * self.nv + j;
                let weight = u_bernstein[i] * v_bernstein[j];

                for d in 0..self.dim {
                    result[d] = result[d] + weight * self.control_points[[idx, d]];
                }
            }
        }

        Ok(result)
    }

    /// Evaluate the Bezier surface at multiple parameter pairs
    ///
    /// # Arguments
    ///
    /// * `u_values` - Array of u parameter values, each in [0, 1]
    /// * `v_values` - Array of v parameter values, each in [0, 1]
    /// * `grid` - If true, evaluate on a grid of u×v points; otherwise, evaluate at pairs `(u[i], v[i])`
    ///
    /// # Returns
    ///
    /// Array of points on the Bezier surface at the given parameters
    pub fn evaluate_array(
        &self,
        u_values: &ArrayView1<F>,
        v_values: &ArrayView1<F>,
        grid: bool,
    ) -> InterpolateResult<Array2<F>> {
        if grid {
            // Evaluate on a grid of u×v points
            let nu = u_values.len();
            let nv = v_values.len();
            let mut result = Array2::zeros((nu * nv, self.dim));

            for (i, &u) in u_values.iter().enumerate() {
                for (j, &v) in v_values.iter().enumerate() {
                    let point = self.evaluate(u, v)?;
                    let idx = i * nv + j;
                    for d in 0..self.dim {
                        result[[idx, d]] = point[d];
                    }
                }
            }

            Ok(result)
        } else {
            // Evaluate at pairs (u[i], v[i])
            if u_values.len() != v_values.len() {
                return Err(InterpolateError::invalid_input(
                    "When grid=false, u_values and v_values must have the same length".to_string(),
                ));
            }

            let n_points = u_values.len();
            let mut result = Array2::zeros((n_points, self.dim));

            for i in 0..n_points {
                let point = self.evaluate(u_values[i], v_values[i])?;
                for d in 0..self.dim {
                    result[[i, d]] = point[d];
                }
            }

            Ok(result)
        }
    }

    /// Compute the partial derivative of the Bezier surface with respect to u
    ///
    /// # Returns
    ///
    /// A new Bezier surface representing the partial derivative ∂S/∂u
    pub fn derivative_u(&self) -> InterpolateResult<Self> {
        if self.nu <= 1 {
            // Derivative of a constant surface is a zero surface
            let zero_points = Array2::zeros((self.nv, self.dim));
            return Self::new(&zero_points.view(), 1, self.nv);
        }

        // Compute control points for the derivative surface
        let mut deriv_control_points = Array2::zeros(((self.nu - 1) * self.nv, self.dim));
        let degree_u = F::from_usize(self.nu - 1).expect("Operation failed");

        for i in 0..self.nu - 1 {
            for j in 0..self.nv {
                let idx_curr = i * self.nv + j;
                let idx_next = (i + 1) * self.nv + j;
                let idx_deriv = i * self.nv + j;

                for d in 0..self.dim {
                    deriv_control_points[[idx_deriv, d]] = degree_u
                        * (self.control_points[[idx_next, d]] - self.control_points[[idx_curr, d]]);
                }
            }
        }

        Self::new(&deriv_control_points.view(), self.nu - 1, self.nv)
    }

    /// Compute the partial derivative of the Bezier surface with respect to v
    ///
    /// # Returns
    ///
    /// A new Bezier surface representing the partial derivative ∂S/∂v
    pub fn derivative_v(&self) -> InterpolateResult<Self> {
        if self.nv <= 1 {
            // Derivative of a constant surface is a zero surface
            let zero_points = Array2::zeros((self.nu, self.dim));
            return Self::new(&zero_points.view(), self.nu, 1);
        }

        // Compute control points for the derivative surface
        let mut deriv_control_points = Array2::zeros((self.nu * (self.nv - 1), self.dim));
        let degree_v = F::from_usize(self.nv - 1).expect("Operation failed");

        for i in 0..self.nu {
            for j in 0..self.nv - 1 {
                let idx_curr = i * self.nv + j;
                let idx_next = i * self.nv + (j + 1);
                let idx_deriv = i * (self.nv - 1) + j;

                for d in 0..self.dim {
                    deriv_control_points[[idx_deriv, d]] = degree_v
                        * (self.control_points[[idx_next, d]] - self.control_points[[idx_curr, d]]);
                }
            }
        }

        Self::new(&deriv_control_points.view(), self.nu, self.nv - 1)
    }
}

/// Compute the Bernstein polynomial of degree n at parameter t
///
/// The Bernstein polynomial is defined as:
/// B_{i,n}(t) = (n choose i) * t^i * (1-t)^(n-i)
///
/// # Arguments
///
/// * `t` - Parameter value in [0, 1]
/// * `i` - Index of the Bernstein polynomial
/// * `n` - Degree of the Bernstein polynomial
///
/// # Returns
///
/// The value of the Bernstein polynomial at parameter t
#[allow(dead_code)]
pub fn bernstein<F: Float + FromPrimitive + std::fmt::Display>(
    t: F,
    i: usize,
    n: usize,
) -> InterpolateResult<F> {
    if i > n {
        return Err(InterpolateError::invalid_input(format!(
            "Index i={} must be <= degree n={}",
            i, n
        )));
    }

    if t < F::zero() || t > F::one() {
        return Err(InterpolateError::OutOfBounds(format!(
            "Parameter t must be in [0, 1], got {}",
            t
        )));
    }

    // Handle boundary cases for numerical stability
    if (i == 0 && t == F::zero()) || (i == n && t == F::one()) {
        return Ok(F::one());
    }
    if (i == 0 && t == F::one()) || (i == n && t == F::zero()) {
        return Ok(F::zero());
    }

    // Compute binomial coefficient (n choose i)
    let mut binomial = F::one();
    for j in 0..i {
        binomial = binomial * F::from_usize(n - j).expect("Operation failed")
            / F::from_usize(j + 1).expect("Operation failed");
    }

    // Compute Bernstein polynomial
    let ti = t.powi(i as i32);
    let one_minus_t = F::one() - t;
    let one_minus_t_pow = one_minus_t.powi((n - i) as i32);

    Ok(binomial * ti * one_minus_t_pow)
}

/// Compute all Bernstein polynomials of degree n at parameter t
///
/// # Arguments
///
/// * `t` - Parameter value in [0, 1]
/// * `n` - Degree of the Bernstein polynomials
///
/// # Returns
///
/// An array containing all n+1 Bernstein polynomials of degree n at parameter t
#[allow(dead_code)]
pub fn compute_bernstein_all<F: Float + FromPrimitive>(
    t: F,
    n: usize,
) -> InterpolateResult<Array1<F>> {
    let mut result = Array1::zeros(n + 1);

    // Handle boundary cases for numerical stability
    if t == F::zero() {
        result[0] = F::one();
        return Ok(result);
    }
    if t == F::one() {
        result[n] = F::one();
        return Ok(result);
    }

    // Use more numerically stable recursive formula
    result[0] = F::one();
    let one_minus_t = F::one() - t;

    for i in 1..=n {
        let mut j = i;
        while j > 0 {
            result[j] = one_minus_t * result[j] + t * result[j - 1];
            j -= 1;
        }
        result[0] = one_minus_t * result[0];
    }

    Ok(result)
}

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;
    use scirs2_core::ndarray::array;

    #[test]
    fn test_bernstein_polynomials() {
        // For degree 1, we should have B_{0,1}(t) = 1-t and B_{1,1}(t) = t
        assert_relative_eq!(bernstein(0.0, 0, 1).expect("Operation failed"), 1.0);
        assert_relative_eq!(bernstein(0.0, 1, 1).expect("Operation failed"), 0.0);
        assert_relative_eq!(bernstein(1.0, 0, 1).expect("Operation failed"), 0.0);
        assert_relative_eq!(bernstein(1.0, 1, 1).expect("Operation failed"), 1.0);
        assert_relative_eq!(bernstein(0.5, 0, 1).expect("Operation failed"), 0.5);
        assert_relative_eq!(bernstein(0.5, 1, 1).expect("Operation failed"), 0.5);

        // Test degree 2
        assert_relative_eq!(bernstein(0.5, 0, 2).expect("Operation failed"), 0.25);
        assert_relative_eq!(bernstein(0.5, 1, 2).expect("Operation failed"), 0.5);
        assert_relative_eq!(bernstein(0.5, 2, 2).expect("Operation failed"), 0.25);
    }

    #[test]
    fn test_compute_bernstein_all() {
        // For degree 1 at t=0.5, we should have [0.5, 0.5]
        let result = compute_bernstein_all(0.5, 1).expect("Operation failed");
        assert_eq!(result.len(), 2);
        assert_relative_eq!(result[0], 0.5);
        assert_relative_eq!(result[1], 0.5);

        // Test degree 2 at t=0.5
        let result = compute_bernstein_all(0.5, 2).expect("Operation failed");
        assert_eq!(result.len(), 3);
        assert_relative_eq!(result[0], 0.25);
        assert_relative_eq!(result[1], 0.5);
        assert_relative_eq!(result[2], 0.25);

        // Sum of all Bernstein polynomials should be 1
        assert_relative_eq!(result.sum(), 1.0);
    }

    #[test]
    fn test_bezier_curve() {
        // Create a linear Bezier curve (straight line)
        let control_points = array![[0.0, 0.0], [1.0, 1.0]];
        let curve = BezierCurve::new(&control_points.view()).expect("Operation failed");

        // Test evaluation
        let p0 = curve.evaluate(0.0).expect("Operation failed");
        let p1 = curve.evaluate(1.0).expect("Operation failed");
        let p_mid = curve.evaluate(0.5).expect("Operation failed");

        assert_relative_eq!(p0[0], 0.0);
        assert_relative_eq!(p0[1], 0.0);
        assert_relative_eq!(p1[0], 1.0);
        assert_relative_eq!(p1[1], 1.0);
        assert_relative_eq!(p_mid[0], 0.5);
        assert_relative_eq!(p_mid[1], 0.5);

        // Test quadratic Bezier curve
        let control_points = array![[0.0, 0.0], [1.0, 2.0], [2.0, 0.0]];
        let curve = BezierCurve::new(&control_points.view()).expect("Operation failed");

        let p_mid = curve.evaluate(0.5).expect("Operation failed");
        assert_relative_eq!(p_mid[0], 1.0);
        assert_relative_eq!(p_mid[1], 1.0);
    }

    #[test]
    fn test_bezier_curve_derivative() {
        // For a quadratic curve, the derivative should be a linear curve
        let control_points = array![[0.0, 0.0], [1.0, 2.0], [2.0, 0.0]];
        let curve = BezierCurve::new(&control_points.view()).expect("Operation failed");

        let deriv = curve.derivative().expect("Operation failed");
        assert_eq!(deriv.degree(), 1); // Linear

        // The derivative control points should be 2*([1,2]-[0,0]) and 2*([2,0]-[1,2])
        let deriv_cp = deriv.control_points();
        assert_relative_eq!(deriv_cp[[0, 0]], 2.0);
        assert_relative_eq!(deriv_cp[[0, 1]], 4.0);
        assert_relative_eq!(deriv_cp[[1, 0]], 2.0);
        assert_relative_eq!(deriv_cp[[1, 1]], -4.0);
    }

    #[test]
    fn test_bezier_curve_split() {
        // Split a quadratic curve at t=0.5
        let control_points = array![[0.0, 0.0], [1.0, 2.0], [2.0, 0.0]];
        let curve = BezierCurve::new(&control_points.view()).expect("Operation failed");

        let (left, right) = curve.split(0.5).expect("Operation failed");

        // Check degrees
        assert_eq!(left.degree(), 2);
        assert_eq!(right.degree(), 2);

        // Check end points
        let left_start = left.evaluate(0.0).expect("Operation failed");
        let left_end = left.evaluate(1.0).expect("Operation failed");
        let right_start = right.evaluate(0.0).expect("Operation failed");
        let right_end = right.evaluate(1.0).expect("Operation failed");

        assert_relative_eq!(left_start[0], 0.0);
        assert_relative_eq!(left_start[1], 0.0);
        assert_relative_eq!(left_end[0], 1.0);
        assert_relative_eq!(left_end[1], 1.0);
        assert_relative_eq!(right_start[0], 1.0);
        assert_relative_eq!(right_start[1], 1.0);
        assert_relative_eq!(right_end[0], 2.0);
        assert_relative_eq!(right_end[1], 0.0);

        // The point at t=0.5 on the original curve should be the same as
        // the end point of the left curve and the start point of the right curve
        let mid = curve.evaluate(0.5).expect("Operation failed");
        assert_relative_eq!(mid[0], left_end[0]);
        assert_relative_eq!(mid[1], left_end[1]);
        assert_relative_eq!(mid[0], right_start[0]);
        assert_relative_eq!(mid[1], right_start[1]);
    }

    #[test]
    fn test_bezier_surface() {
        // Create a simple bilinear Bezier surface (plane)
        let control_points = array![
            [0.0, 0.0, 0.0], // (0,0)
            [1.0, 0.0, 0.0], // (0,1)
            [0.0, 1.0, 0.0], // (1,0)
            [1.0, 1.0, 0.0], // (1,1)
        ];
        let surface = BezierSurface::new(&control_points.view(), 2, 2).expect("Operation failed");

        // Test evaluation at corners
        let p00 = surface.evaluate(0.0, 0.0).expect("Operation failed");
        let p01 = surface.evaluate(0.0, 1.0).expect("Operation failed");
        let p10 = surface.evaluate(1.0, 0.0).expect("Operation failed");
        let p11 = surface.evaluate(1.0, 1.0).expect("Operation failed");

        assert_relative_eq!(p00[0], 0.0);
        assert_relative_eq!(p00[1], 0.0);
        assert_relative_eq!(p00[2], 0.0);

        assert_relative_eq!(p01[0], 1.0);
        assert_relative_eq!(p01[1], 0.0);
        assert_relative_eq!(p01[2], 0.0);

        assert_relative_eq!(p10[0], 0.0);
        assert_relative_eq!(p10[1], 1.0);
        assert_relative_eq!(p10[2], 0.0);

        assert_relative_eq!(p11[0], 1.0);
        assert_relative_eq!(p11[1], 1.0);
        assert_relative_eq!(p11[2], 0.0);

        // Test evaluation at center
        let p_mid = surface.evaluate(0.5, 0.5).expect("Operation failed");
        assert_relative_eq!(p_mid[0], 0.5);
        assert_relative_eq!(p_mid[1], 0.5);
        assert_relative_eq!(p_mid[2], 0.0);
    }

    #[test]
    fn test_bezier_surface_derivatives() {
        // Create a biquadratic Bezier surface
        let control_points = array![
            [0.0, 0.0, 0.0], // (0,0)
            [1.0, 0.0, 1.0], // (0,1)
            [2.0, 0.0, 0.0], // (0,2)
            [0.0, 1.0, 1.0], // (1,0)
            [1.0, 1.0, 2.0], // (1,1)
            [2.0, 1.0, 1.0], // (1,2)
            [0.0, 2.0, 0.0], // (2,0)
            [1.0, 2.0, 1.0], // (2,1)
            [2.0, 2.0, 0.0], // (2,2)
        ];
        let surface = BezierSurface::new(&control_points.view(), 3, 3).expect("Operation failed");

        // Get partial derivatives
        let deriv_u = surface.derivative_u().expect("Operation failed");
        let deriv_v = surface.derivative_v().expect("Operation failed");

        // Check dimensions
        assert_eq!(deriv_u.grid_dimensions(), (2, 3)); // Degree 1 in u, 2 in v
        assert_eq!(deriv_v.grid_dimensions(), (3, 2)); // Degree 2 in u, 1 in v

        // Evaluate derivatives at (u,v) = (0.5, 0.5)
        let du = deriv_u.evaluate(0.5, 0.5).expect("Operation failed");
        let dv = deriv_v.evaluate(0.5, 0.5).expect("Operation failed");

        // The partial derivatives should be non-zero for this non-flat surface
        assert_eq!(du.len(), 3); // 3D point
        assert_eq!(dv.len(), 3);

        // Check that the derivatives have reasonable magnitudes
        let du_magnitude = (du[0] * du[0] + du[1] * du[1] + du[2] * du[2]).sqrt();
        let dv_magnitude = (dv[0] * dv[0] + dv[1] * dv[1] + dv[2] * dv[2]).sqrt();

        assert!(du_magnitude > 0.1); // Should be non-zero
        assert!(dv_magnitude > 0.1); // Should be non-zero

        // Verify the surface itself evaluates correctly at the center
        let center = surface.evaluate(0.5, 0.5).expect("Operation failed");
        assert_eq!(center.len(), 3);

        // The center point should be approximately at (1, 1, z) for some z value
        assert_relative_eq!(center[0], 1.0, epsilon = 0.1);
        assert_relative_eq!(center[1], 1.0, epsilon = 0.1);
    }
}