use crate::traits::FloatScalar;

use super::{find_interval, validate_sorted, InterpError};

/// Evaluate the cubic Hermite segment at parameter `t ∈ [0, 1]`.
///
/// `h` is the segment width, `y0`/`y1` the endpoint values, `dy0`/`dy1` the
/// endpoint derivatives. Shared by the fixed-size and dynamic interpolants.
fn hermite_segment<T: FloatScalar>(t: T, h: T, y0: T, dy0: T, y1: T, dy1: T) -> T {
    let t2 = t * t;
    let t3 = t2 * t;
    let two = T::one() + T::one();
    let three = two + T::one();

    // Hermite basis: h00 = 2t³ - 3t² + 1, h10 = t³ - 2t² + t
    //                h01 = -2t³ + 3t², h11 = t³ - t²
    let h00 = two * t3 - three * t2 + T::one();
    let h10 = t3 - two * t2 + t;
    let h01 = (T::zero() - two) * t3 + three * t2;
    let h11 = t3 - t2;

    h00 * y0 + h10 * h * dy0 + h01 * y1 + h11 * h * dy1
}

/// Evaluate the cubic Hermite segment and its derivative at `t ∈ [0, 1]`.
fn hermite_segment_with_derivative<T: FloatScalar>(
    t: T,
    h: T,
    y0: T,
    dy0: T,
    y1: T,
    dy1: T,
) -> (T, T) {
    let t2 = t * t;
    let two = T::one() + T::one();
    let three = two + T::one();
    let six = three + three;

    let val = hermite_segment(t, h, y0, dy0, y1, dy1);

    // d/dx = (1/h) d/dt of the basis
    // h00' = 6t² - 6t, h10' = 3t² - 4t + 1
    // h01' = -6t² + 6t, h11' = 3t² - 2t
    let dh00 = six * t2 - six * t;
    let dh10 = three * t2 - (two + two) * t + T::one();
    let dh01 = (T::zero() - six) * t2 + six * t;
    let dh11 = three * t2 - two * t;

    let dval = (dh00 * y0 + dh10 * h * dy0 + dh01 * y1 + dh11 * h * dy1) / h;

    (val, dval)
}

/// Cubic Hermite interpolant (fixed-size, stack-allocated).
///
/// Uses user-supplied derivatives at each knot. Each segment is a cubic
/// polynomial matching both value and first derivative at its endpoints.
/// Requires at least 2 points.
///
/// # Example
///
/// ```
/// use numeris::interp::HermiteInterp;
///
/// // Interpolate sin(x) with known derivatives cos(x)
/// let xs = [0.0_f64, 1.0, 2.0];
/// let ys = [0.0_f64.sin(), 1.0_f64.sin(), 2.0_f64.sin()];
/// let dys = [0.0_f64.cos(), 1.0_f64.cos(), 2.0_f64.cos()];
/// let interp = HermiteInterp::new(xs, ys, dys).unwrap();
/// let mid = 0.5;
/// assert!((interp.eval(mid) - mid.sin()).abs() < 0.002);
/// ```
#[derive(Debug, Clone)]
pub struct HermiteInterp<T, const N: usize> {
    xs: [T; N],
    ys: [T; N],
    dys: [T; N],
}

impl<T: FloatScalar, const N: usize> HermiteInterp<T, N> {
    /// Construct a Hermite interpolant from knots and derivatives.
    pub fn new(xs: [T; N], ys: [T; N], dys: [T; N]) -> Result<Self, InterpError> {
        if N < 2 {
            return Err(InterpError::TooFewPoints);
        }
        validate_sorted(&xs)?;
        Ok(Self { xs, ys, dys })
    }

    /// Evaluate the interpolant at `x`.
    pub fn eval(&self, x: T) -> T {
        let i = find_interval(&self.xs, x);
        let h = self.xs[i + 1] - self.xs[i];
        let t = (x - self.xs[i]) / h;
        hermite_segment(
            t,
            h,
            self.ys[i],
            self.dys[i],
            self.ys[i + 1],
            self.dys[i + 1],
        )
    }

    /// Evaluate the interpolant and its derivative at `x`.
    pub fn eval_derivative(&self, x: T) -> (T, T) {
        let i = find_interval(&self.xs, x);
        let h = self.xs[i + 1] - self.xs[i];
        let t = (x - self.xs[i]) / h;
        hermite_segment_with_derivative(
            t,
            h,
            self.ys[i],
            self.dys[i],
            self.ys[i + 1],
            self.dys[i + 1],
        )
    }

    /// The knot x-values.
    pub fn xs(&self) -> &[T; N] {
        &self.xs
    }

    /// The knot y-values.
    pub fn ys(&self) -> &[T; N] {
        &self.ys
    }
}

// ---------- Dynamic variant ----------

#[cfg(feature = "alloc")]
extern crate alloc;
#[cfg(feature = "alloc")]
use alloc::vec::Vec;

/// Cubic Hermite interpolant (heap-allocated, runtime-sized).
///
/// Dynamic counterpart of [`HermiteInterp`]. Requires at least 2 points.
///
/// # Example
///
/// ```
/// use numeris::interp::DynHermiteInterp;
///
/// let interp = DynHermiteInterp::new(
///     vec![0.0_f64, 1.0, 2.0],
///     vec![0.0, 1.0, 0.0],
///     vec![1.0, 0.0, -1.0],
/// ).unwrap();
/// assert!((interp.eval(0.5) - 0.625).abs() < 1e-14);
/// ```
#[cfg(feature = "alloc")]
#[derive(Debug, Clone)]
pub struct DynHermiteInterp<T> {
    xs: Vec<T>,
    ys: Vec<T>,
    dys: Vec<T>,
}

#[cfg(feature = "alloc")]
impl<T: FloatScalar> DynHermiteInterp<T> {
    /// Construct a Hermite interpolant from knots and derivatives.
    pub fn new(xs: Vec<T>, ys: Vec<T>, dys: Vec<T>) -> Result<Self, InterpError> {
        if xs.len() != ys.len() || xs.len() != dys.len() {
            return Err(InterpError::LengthMismatch);
        }
        if xs.len() < 2 {
            return Err(InterpError::TooFewPoints);
        }
        validate_sorted(&xs)?;
        Ok(Self { xs, ys, dys })
    }

    /// Evaluate the interpolant at `x`.
    pub fn eval(&self, x: T) -> T {
        let i = find_interval(&self.xs, x);
        let h = self.xs[i + 1] - self.xs[i];
        let t = (x - self.xs[i]) / h;
        hermite_segment(
            t,
            h,
            self.ys[i],
            self.dys[i],
            self.ys[i + 1],
            self.dys[i + 1],
        )
    }

    /// Evaluate the interpolant and its derivative at `x`.
    pub fn eval_derivative(&self, x: T) -> (T, T) {
        let i = find_interval(&self.xs, x);
        let h = self.xs[i + 1] - self.xs[i];
        let t = (x - self.xs[i]) / h;
        hermite_segment_with_derivative(
            t,
            h,
            self.ys[i],
            self.dys[i],
            self.ys[i + 1],
            self.dys[i + 1],
        )
    }

    /// The knot x-values.
    pub fn xs(&self) -> &[T] {
        &self.xs
    }

    /// The knot y-values.
    pub fn ys(&self) -> &[T] {
        &self.ys
    }
}
