use bone_types::{Length, Parameter, Point2, Tolerance, UnitVec2, Vec2};
use uom::si::length::millimeter;

use crate::aabb::Aabb2;
use crate::closest::ClosestPoint2;
use crate::curvature::Curvature;
use crate::curve2::{Curve2, Curve2Kind};

const CLOSEST_SAMPLES: u32 = 96;

#[derive(Copy, Clone, Debug, PartialEq)]
pub struct Bezier2 {
    control: [Point2; 4],
}

impl Bezier2 {
    #[must_use]
    pub const fn new(control: [Point2; 4]) -> Self {
        Self { control }
    }

    #[must_use]
    pub const fn control(self) -> [Point2; 4] {
        self.control
    }

    #[must_use]
    pub const fn start(self) -> Point2 {
        self.control[0]
    }

    #[must_use]
    pub const fn end(self) -> Point2 {
        self.control[3]
    }

    #[must_use]
    pub const fn interior(self) -> [Point2; 2] {
        [self.control[1], self.control[2]]
    }

    fn point_at(self, u: f64) -> Point2 {
        let [p0, p1, p2, p3] = self.control;
        let q0 = lerp(p0, p1, u);
        let q1 = lerp(p1, p2, u);
        let q2 = lerp(p2, p3, u);
        let r0 = lerp(q0, q1, u);
        let r1 = lerp(q1, q2, u);
        lerp(r0, r1, u)
    }

    fn velocity(self, u: f64) -> Vec2 {
        let [p0, p1, p2, p3] = self.control;
        let mu = 1.0 - u;
        3.0 * (mu * mu * (p1 - p0) + 2.0 * mu * u * (p2 - p1) + u * u * (p3 - p2))
    }

    fn acceleration(self, u: f64) -> Vec2 {
        let [p0, p1, p2, p3] = self.control;
        let mu = 1.0 - u;
        6.0 * (mu * ((p2 - p1) - (p1 - p0)) + u * ((p3 - p2) - (p2 - p1)))
    }
}

fn lerp(a: Point2, b: Point2, t: f64) -> Point2 {
    let (ax, ay) = a.coords_mm();
    let (bx, by) = b.coords_mm();
    Point2::from_mm(ax + t * (bx - ax), ay + t * (by - ay))
}

impl Curve2 for Bezier2 {
    fn evaluate(&self, t: Parameter) -> Point2 {
        self.point_at(t.value())
    }

    fn derivative(&self, t: Parameter) -> Vec2 {
        self.velocity(t.value())
    }

    fn tangent(&self, t: Parameter) -> UnitVec2 {
        let (dx, dy) = self.velocity(t.value()).coords_mm();
        let len = dx.hypot(dy);
        if len <= f64::EPSILON {
            UnitVec2::new_unchecked(1.0, 0.0)
        } else {
            UnitVec2::new_unchecked(dx / len, dy / len)
        }
    }

    fn curvature(&self, t: Parameter) -> Curvature {
        let (vx, vy) = self.velocity(t.value()).coords_mm();
        let (ax, ay) = self.acceleration(t.value()).coords_mm();
        let speed = vx.hypot(vy);
        let cross = vx * ay - vy * ax;
        if speed <= f64::EPSILON || cross.abs() <= f64::EPSILON {
            Curvature::ZERO
        } else {
            let radius = speed * speed * speed / cross.abs();
            Curvature::from_radius(Length::new::<millimeter>(radius)).with_sign(cross)
        }
    }

    fn bounding_box(&self) -> Aabb2 {
        self.control.into_iter().skip(1).fold(
            Aabb2::from_corners(self.control[0], self.control[0]),
            Aabb2::extend_point,
        )
    }

    fn closest_point(&self, p: Point2, _tolerance: Tolerance) -> ClosestPoint2 {
        let (px, py) = p.coords_mm();
        let (t, point, dist_sq) = (0..=CLOSEST_SAMPLES)
            .map(|i| {
                let u = f64::from(i) / f64::from(CLOSEST_SAMPLES);
                let q = self.point_at(u);
                let (qx, qy) = q.coords_mm();
                (u, q, (qx - px).powi(2) + (qy - py).powi(2))
            })
            .min_by(|a, b| a.2.total_cmp(&b.2))
            .unwrap_or((0.0, self.control[0], 0.0));
        ClosestPoint2::new(
            Parameter::new(t),
            point,
            Length::new::<millimeter>(dist_sq.sqrt()),
        )
    }

    fn as_kind(&self) -> Curve2Kind {
        Curve2Kind::Bezier(*self)
    }
}

impl core::fmt::Display for Bezier2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        let [p0, p1, p2, p3] = self.control;
        write!(f, "bezier2{{ {p0}, {p1}, {p2}, {p3} }}")
    }
}
