use bone_types::Point3;

use crate::KernelError;

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct Degree(usize);

impl Degree {
    pub fn new(value: usize) -> Result<Self, KernelError> {
        if value == 0 {
            return Err(KernelError::DegenerateNurbs);
        }
        Ok(Self(value))
    }

    #[must_use]
    pub const fn get(self) -> usize {
        self.0
    }
}

#[derive(Clone, Debug, PartialEq)]
pub struct KnotVector(Vec<f64>);

impl KnotVector {
    pub fn new(values: Vec<f64>) -> Result<Self, KernelError> {
        let monotone = values.windows(2).all(|pair| pair[0] <= pair[1]);
        let finite = values.iter().all(|value| value.is_finite());
        let spread = values
            .first()
            .zip(values.last())
            .is_some_and(|(lo, hi)| lo < hi);
        if values.len() < 2 || !finite || !monotone || !spread {
            return Err(KernelError::DegenerateNurbs);
        }
        Ok(Self(values))
    }

    #[must_use]
    pub fn values(&self) -> &[f64] {
        &self.0
    }

    #[must_use]
    pub(crate) fn domain(&self, degree: Degree) -> (f64, f64) {
        (
            self.0[degree.get()],
            self.0[self.0.len() - 1 - degree.get()],
        )
    }

    #[must_use]
    pub(crate) fn is_clamped(&self, degree: Degree) -> bool {
        let p = degree.get();
        let n = self.0.len();
        if n < 2 * (p + 1) {
            return false;
        }
        self.0[p] <= self.0[0] && self.0[n - 1] <= self.0[n - 1 - p]
    }
}

#[derive(Copy, Clone, Debug, PartialEq)]
pub struct RationalControlPoint {
    position: Point3,
    weight: f64,
}

impl RationalControlPoint {
    pub fn new(
        position: Point3,
        weight: f64,
        tolerance: bone_types::Tolerance,
    ) -> Result<Self, KernelError> {
        if !weight.is_finite() || weight <= tolerance.value() {
            return Err(KernelError::DegenerateNurbs);
        }
        Ok(Self { position, weight })
    }

    #[must_use]
    pub const fn position(&self) -> Point3 {
        self.position
    }

    #[must_use]
    pub const fn weight(self) -> f64 {
        self.weight
    }

    #[must_use]
    pub(crate) fn homogeneous(self) -> [f64; 4] {
        let (x, y, z) = self.position.coords_mm();
        [
            x * self.weight,
            y * self.weight,
            z * self.weight,
            self.weight,
        ]
    }
}

pub(crate) fn count_to_f64(value: usize) -> f64 {
    u32::try_from(value).map_or(f64::INFINITY, f64::from)
}

fn add(a: [f64; 4], b: [f64; 4]) -> [f64; 4] {
    [a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]]
}

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

fn scale(a: [f64; 4], s: f64) -> [f64; 4] {
    [a[0] * s, a[1] * s, a[2] * s, a[3] * s]
}

fn bisect_span(low: usize, high: usize, u: f64, knots: &[f64]) -> usize {
    let mid = low.midpoint(high);
    if u < knots[mid] {
        bisect_span(low, mid, u, knots)
    } else if u >= knots[mid + 1] {
        bisect_span(mid, high, u, knots)
    } else {
        mid
    }
}

pub(crate) fn find_span(last: usize, degree: usize, u: f64, knots: &[f64]) -> usize {
    if u >= knots[last + 1] {
        last
    } else if u <= knots[degree] {
        degree
    } else {
        bisect_span(degree, last + 1, u, knots)
    }
}

pub(crate) fn basis_functions(span: usize, u: f64, degree: usize, knots: &[f64]) -> Vec<f64> {
    let seed = (
        vec![1.0_f64],
        vec![0.0_f64; degree + 1],
        vec![0.0_f64; degree + 1],
    );
    (1..=degree)
        .fold(seed, |(lower, mut left, mut right), j| {
            left[j] = u - knots[span + 1 - j];
            right[j] = knots[span + j] - u;
            let (mut next, saved) =
                (0..j).fold((Vec::with_capacity(j + 1), 0.0), |(mut out, saved), r| {
                    let denom = right[r + 1] + left[j - r];
                    let temp = if denom == 0.0 { 0.0 } else { lower[r] / denom };
                    out.push(saved + right[r + 1] * temp);
                    (out, left[j - r] * temp)
                });
            next.push(saved);
            (next, left, right)
        })
        .0
}

pub(crate) fn evaluate_homogeneous(
    degree: usize,
    knots: &[f64],
    control: &[[f64; 4]],
    u: f64,
) -> ([f64; 4], [f64; 4]) {
    let last = control.len() - 1;
    let span = find_span(last, degree, u, knots);
    let basis = basis_functions(span, u, degree, knots);
    let value = (0..=degree).fold([0.0; 4], |acc, k| {
        add(acc, scale(control[span - degree + k], basis[k]))
    });
    if degree == 0 {
        return (value, [0.0; 4]);
    }
    let reduced = &knots[1..knots.len() - 1];
    let derivative_control: Vec<[f64; 4]> = (0..last)
        .map(|i| {
            let denom = knots[i + degree + 1] - knots[i + 1];
            if denom == 0.0 {
                [0.0; 4]
            } else {
                scale(
                    sub(control[i + 1], control[i]),
                    count_to_f64(degree) / denom,
                )
            }
        })
        .collect();
    let reduced_last = derivative_control.len() - 1;
    let span_d = find_span(reduced_last, degree - 1, u, reduced);
    let basis_d = basis_functions(span_d, u, degree - 1, reduced);
    let derivative = (0..degree).fold([0.0; 4], |acc, k| {
        add(
            acc,
            scale(derivative_control[span_d - (degree - 1) + k], basis_d[k]),
        )
    });
    (value, derivative)
}

pub(crate) fn project(value: [f64; 4]) -> Point3 {
    Point3::from_mm(
        value[0] / value[3],
        value[1] / value[3],
        value[2] / value[3],
    )
}

pub(crate) fn project_derivative(value: [f64; 4], derivative: [f64; 4]) -> bone_types::Vec3 {
    let w = value[3];
    let dw = derivative[3];
    bone_types::Vec3::from_mm(
        (derivative[0] * w - value[0] * dw) / (w * w),
        (derivative[1] * w - value[1] * dw) / (w * w),
        (derivative[2] * w - value[2] * dw) / (w * w),
    )
}
