use bone_types::{Aabb3, ChordHeightTolerance, Parameter, Point3, Tolerance, Vec3};

use crate::KernelError;
use crate::closest::ClosestPoint3;
use crate::curve3::Curve3;
use crate::nurbs::{
    Degree, KnotVector, RationalControlPoint, evaluate_homogeneous, project, project_derivative,
};
use uom::si::length::millimeter;

const MAX_DEPTH: u32 = 16;
const SEED_SAMPLES: u32 = 128;
const PROJECTION_STEPS: u32 = 24;

#[derive(Clone, Debug, PartialEq)]
pub struct NurbsCurve3 {
    degree: Degree,
    knots: KnotVector,
    control: Vec<RationalControlPoint>,
    homogeneous: Vec<[f64; 4]>,
}

impl NurbsCurve3 {
    pub fn new(
        degree: Degree,
        knots: KnotVector,
        control: Vec<RationalControlPoint>,
    ) -> Result<Self, KernelError> {
        if control.len() < degree.get() + 1
            || knots.values().len() != control.len() + degree.get() + 1
            || !knots.is_clamped(degree)
        {
            return Err(KernelError::DegenerateNurbs);
        }
        let homogeneous = control.iter().map(|point| point.homogeneous()).collect();
        Ok(Self {
            degree,
            knots,
            control,
            homogeneous,
        })
    }

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

    #[must_use]
    pub fn knots(&self) -> &KnotVector {
        &self.knots
    }

    #[must_use]
    pub fn control(&self) -> &[RationalControlPoint] {
        &self.control
    }

    #[must_use]
    fn param_at(&self, t: f64) -> f64 {
        let (lo, hi) = self.knots.domain(self.degree);
        lo + (hi - lo) * t
    }

    #[must_use]
    fn span(&self) -> f64 {
        let (lo, hi) = self.knots.domain(self.degree);
        hi - lo
    }

    fn refine(
        &self,
        t0: f64,
        t1: f64,
        p0: Point3,
        p1: Point3,
        chord: ChordHeightTolerance,
        depth: u32,
    ) -> Vec<Point3> {
        let tm = f64::midpoint(t0, t1);
        let pm = self.evaluate(Parameter::new(tm));
        let chord_mid = Point3::from_mm(
            f64::midpoint(p0.coords_mm().0, p1.coords_mm().0),
            f64::midpoint(p0.coords_mm().1, p1.coords_mm().1),
            f64::midpoint(p0.coords_mm().2, p1.coords_mm().2),
        );
        if depth == 0 || (pm - chord_mid).norm_mm() <= chord.millimeters() {
            vec![p1]
        } else {
            let mut head = self.refine(t0, tm, p0, pm, chord, depth - 1);
            head.extend(self.refine(tm, t1, pm, p1, chord, depth - 1));
            head
        }
    }

    #[must_use]
    fn seed(&self, p: Point3) -> f64 {
        (0..=SEED_SAMPLES)
            .map(|i| {
                let t = f64::from(i) / f64::from(SEED_SAMPLES);
                (t, (self.evaluate(Parameter::new(t)) - p).norm_mm())
            })
            .min_by(|a, b| a.1.total_cmp(&b.1))
            .map_or(0.0, |(t, _)| t)
    }

    #[must_use]
    fn project(&self, p: Point3, seed: f64, tolerance: Tolerance) -> f64 {
        let stepped = (0..PROJECTION_STEPS).try_fold(seed, |t, _| {
            let point = self.evaluate(Parameter::new(t));
            let tangent = self.derivative(Parameter::new(t));
            let tangent_sq = tangent.dot_mm2(tangent);
            if tangent_sq <= f64::EPSILON {
                return Err(t);
            }
            let next = (t - (point - p).dot_mm2(tangent) / tangent_sq).clamp(0.0, 1.0);
            if (self.evaluate(Parameter::new(next)) - point).norm_mm() <= tolerance.value() {
                Err(next)
            } else {
                Ok(next)
            }
        });
        match stepped {
            Ok(t) | Err(t) => t,
        }
    }
}

impl Curve3 for NurbsCurve3 {
    fn evaluate(&self, t: Parameter) -> Point3 {
        let (value, _) = evaluate_homogeneous(
            self.degree.get(),
            self.knots.values(),
            &self.homogeneous,
            self.param_at(t.value()),
        );
        project(value)
    }

    fn derivative(&self, t: Parameter) -> Vec3 {
        let (value, derivative) = evaluate_homogeneous(
            self.degree.get(),
            self.knots.values(),
            &self.homogeneous,
            self.param_at(t.value()),
        );
        project_derivative(value, derivative) * self.span()
    }

    fn bounding_box(&self) -> Aabb3 {
        Aabb3::from_points(self.control.iter().map(RationalControlPoint::position))
            .unwrap_or_else(|| Aabb3::from_corners(Point3::origin(), Point3::origin()))
    }

    fn closest_point(&self, p: Point3, tolerance: Tolerance) -> ClosestPoint3 {
        let t = self.project(p, self.seed(p), tolerance);
        let point = self.evaluate(Parameter::new(t));
        ClosestPoint3::new(
            Parameter::new(t),
            point,
            bone_types::Length::new::<millimeter>((point - p).norm_mm()),
        )
    }

    fn tessellate(&self, tolerance: ChordHeightTolerance) -> Vec<Point3> {
        let p0 = self.evaluate(Parameter::new(0.0));
        let p1 = self.evaluate(Parameter::new(1.0));
        let mut points = vec![p0];
        points.extend(self.refine(0.0, 1.0, p0, p1, tolerance, MAX_DEPTH));
        points
    }
}

impl core::fmt::Display for NurbsCurve3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "nurbs_curve3{{ degree={}, control_points={} }}",
            self.degree.get(),
            self.control.len(),
        )
    }
}
