use bone_types::{Angle, Length, Parameter, Point2, Tolerance, UnitVec2, Vec2};
use core::f64::consts::TAU;
use uom::si::angle::radian;
use uom::si::length::millimeter;

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

const CLOSEST_SAMPLES: u32 = 192;

#[derive(Copy, Clone, Debug, PartialEq)]
pub struct Ellipse2 {
    center: Point2,
    semi_major: Length,
    semi_minor: Length,
    rotation: Angle,
}

impl Ellipse2 {
    pub fn new(
        center: Point2,
        semi_major: Length,
        semi_minor: Length,
        rotation: Angle,
        tolerance: Tolerance,
    ) -> Result<Self, KernelError> {
        if semi_major.get::<millimeter>() < tolerance.value()
            || semi_minor.get::<millimeter>() < tolerance.value()
        {
            return Err(KernelError::DegenerateEllipse);
        }
        Ok(Self {
            center,
            semi_major,
            semi_minor,
            rotation,
        })
    }

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

    #[must_use]
    pub fn semi_major_mm(self) -> f64 {
        self.semi_major.get::<millimeter>()
    }

    #[must_use]
    pub fn semi_minor_mm(self) -> f64 {
        self.semi_minor.get::<millimeter>()
    }

    #[must_use]
    pub fn rotation_rad(self) -> f64 {
        self.rotation.get::<radian>()
    }

    fn frame(self) -> (f64, f64) {
        let phi = self.rotation_rad();
        (phi.cos(), phi.sin())
    }

    fn local_to_world(self, lx: f64, ly: f64) -> Point2 {
        let (cx, cy) = self.center.coords_mm();
        let (cp, sp) = self.frame();
        Point2::from_mm(cx + lx * cp - ly * sp, cy + lx * sp + ly * cp)
    }

    fn point_at_angle(self, theta: f64) -> Point2 {
        self.local_to_world(
            self.semi_major_mm() * theta.cos(),
            self.semi_minor_mm() * theta.sin(),
        )
    }

    #[must_use]
    pub fn bezier_arc(self, theta0: f64, theta1: f64) -> (Point2, [Point2; 2], Point2) {
        let k = (4.0 / 3.0) * ((theta1 - theta0) / 4.0).tan();
        let unit = |angle: f64, handle: f64| {
            (
                angle.cos() - handle * angle.sin(),
                angle.sin() + handle * angle.cos(),
            )
        };
        let map = |(ux, uy): (f64, f64)| {
            self.local_to_world(self.semi_major_mm() * ux, self.semi_minor_mm() * uy)
        };
        let start = map((theta0.cos(), theta0.sin()));
        let end = map((theta1.cos(), theta1.sin()));
        let c1 = map(unit(theta0, k));
        let c2 = map(unit(theta1, -k));
        (start, [c1, c2], end)
    }
}

impl Curve2 for Ellipse2 {
    fn evaluate(&self, t: Parameter) -> Point2 {
        self.point_at_angle(TAU * t.value())
    }

    fn derivative(&self, t: Parameter) -> Vec2 {
        let theta = TAU * t.value();
        let (cp, sp) = self.frame();
        let lx = -self.semi_major_mm() * theta.sin() * TAU;
        let ly = self.semi_minor_mm() * theta.cos() * TAU;
        Vec2::from_mm(lx * cp - ly * sp, lx * sp + ly * cp)
    }

    fn tangent(&self, t: Parameter) -> UnitVec2 {
        let (dx, dy) = self.derivative(t).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 theta = TAU * t.value();
        let semi_a = self.semi_major_mm();
        let semi_b = self.semi_minor_mm();
        let sin_t = theta.sin();
        let cos_t = theta.cos();
        let radius = (semi_a * semi_a * sin_t * sin_t + semi_b * semi_b * cos_t * cos_t).powf(1.5)
            / (semi_a * semi_b);
        if !radius.is_finite() || radius <= f64::EPSILON {
            Curvature::ZERO
        } else {
            Curvature::from_radius(Length::new::<millimeter>(radius))
        }
    }

    fn bounding_box(&self) -> Aabb2 {
        let (cx, cy) = self.center.coords_mm();
        let (cp, sp) = self.frame();
        let a = self.semi_major_mm();
        let b = self.semi_minor_mm();
        let hx = (a * a * cp * cp + b * b * sp * sp).sqrt();
        let hy = (a * a * sp * sp + b * b * cp * cp).sqrt();
        Aabb2::from_corners(
            Point2::from_mm(cx - hx, cy - hy),
            Point2::from_mm(cx + hx, cy + hy),
        )
    }

    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 frac = f64::from(i) / f64::from(CLOSEST_SAMPLES);
                let q = self.point_at_angle(TAU * frac);
                let (qx, qy) = q.coords_mm();
                (frac, q, (qx - px).powi(2) + (qy - py).powi(2))
            })
            .min_by(|a, b| a.2.total_cmp(&b.2))
            .unwrap_or((0.0, self.center, 0.0));
        ClosestPoint2::new(
            Parameter::new(t),
            point,
            Length::new::<millimeter>(dist_sq.sqrt()),
        )
    }

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

impl core::fmt::Display for Ellipse2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "ellipse2{{ c={}, a={} mm, b={} mm, phi={} rad }}",
            self.center,
            self.semi_major_mm(),
            self.semi_minor_mm(),
            self.rotation_rad(),
        )
    }
}
