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

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

#[derive(Copy, Clone, Debug, PartialEq)]
pub struct Arc2 {
    center: Point2,
    radius: Length,
    start_angle: Angle,
    sweep_angle: Angle,
}

impl Arc2 {
    pub fn new(
        center: Point2,
        radius: Length,
        start_angle: Angle,
        sweep_angle: Angle,
        tolerance: Tolerance,
    ) -> Result<Self, KernelError> {
        if radius.get::<millimeter>() < tolerance.value() {
            return Err(KernelError::DegenerateArc);
        }
        let sweep = sweep_angle.get::<radian>();
        let angle_eps = AngleTolerance::from_arc_length(tolerance, radius).radians();
        if sweep.abs() < angle_eps {
            return Err(KernelError::DegenerateArc);
        }
        if sweep.abs() > TAU + angle_eps {
            return Err(KernelError::DegenerateArc);
        }
        Ok(Self {
            center,
            radius,
            start_angle,
            sweep_angle,
        })
    }

    pub fn from_center_start_end(
        center: Point2,
        start: Point2,
        end: Point2,
        tolerance: Tolerance,
    ) -> Result<Self, KernelError> {
        let (cx, cy) = center.coords_mm();
        let (sx, sy) = start.coords_mm();
        let (ex, ey) = end.coords_mm();
        let radius = (sx - cx).hypot(sy - cy);
        let start_angle = (sy - cy).atan2(sx - cx);
        let sweep = ((ey - cy).atan2(ex - cx) - start_angle).rem_euclid(TAU);
        if !(radius.is_finite() && sweep.is_finite()) {
            return Err(KernelError::DegenerateArc);
        }
        Self::new(
            center,
            Length::new::<millimeter>(radius),
            Angle::new::<radian>(start_angle),
            Angle::new::<radian>(sweep),
            tolerance,
        )
    }

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

    #[must_use]
    pub const fn radius(self) -> Length {
        self.radius
    }

    #[must_use]
    pub const fn start_angle(self) -> Angle {
        self.start_angle
    }

    #[must_use]
    pub const fn sweep_angle(self) -> Angle {
        self.sweep_angle
    }

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

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

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

    #[must_use]
    pub fn as_full_circle(self) -> Circle2 {
        Circle2::from_raw(self.center, self.radius)
    }

    #[must_use]
    pub fn contains_point(self, p: Point2, tolerance: Tolerance) -> bool {
        let (cx, cy) = self.center.coords_mm();
        let (px, py) = p.coords_mm();
        let dx = px - cx;
        let dy = py - cy;
        let r = self.radius_mm();
        let radial = (dx * dx + dy * dy).sqrt();
        if (radial - r).abs() > tolerance.value() {
            return false;
        }
        let angle_tol = AngleTolerance::from_arc_length(tolerance, self.radius);
        angles::contains(
            dy.atan2(dx),
            self.start_rad(),
            self.sweep_rad(),
            angle_tol.radians(),
        )
    }

    fn angle_at(self, t: Parameter) -> f64 {
        self.start_rad() + self.sweep_rad() * t.value()
    }
}

impl Curve2 for Arc2 {
    fn evaluate(&self, t: Parameter) -> Point2 {
        let (cx, cy) = self.center.coords_mm();
        let r = self.radius_mm();
        let theta = self.angle_at(t);
        Point2::from_mm(cx + r * theta.cos(), cy + r * theta.sin())
    }

    fn derivative(&self, t: Parameter) -> Vec2 {
        let r = self.radius_mm();
        let sweep = self.sweep_rad();
        let theta = self.angle_at(t);
        Vec2::from_mm(-r * sweep * theta.sin(), r * sweep * theta.cos())
    }

    fn tangent(&self, t: Parameter) -> UnitVec2 {
        let theta = self.angle_at(t);
        let sign = self.sweep_rad().signum();
        UnitVec2::new_unchecked(-sign * theta.sin(), sign * theta.cos())
    }

    fn curvature(&self, _t: Parameter) -> Curvature {
        Curvature::from_radius(self.radius).with_sign(self.sweep_rad())
    }

    fn bounding_box(&self) -> Aabb2 {
        arc_bounding_box(self.center, self.radius, self.start_angle, self.sweep_angle)
    }

    fn closest_point(&self, p: Point2, tolerance: Tolerance) -> ClosestPoint2 {
        let (cx, cy) = self.center.coords_mm();
        let (px, py) = p.coords_mm();
        let dx = px - cx;
        let dy = py - cy;
        let dist = (dx * dx + dy * dy).sqrt();
        let r = self.radius_mm();
        let angle_tol = AngleTolerance::from_arc_length(tolerance, self.radius);

        let theta_unclamped = if dist < tolerance.value() {
            self.start_rad()
        } else {
            dy.atan2(dx)
        };

        let theta = angles::clamp(
            theta_unclamped,
            self.start_rad(),
            self.sweep_rad(),
            angle_tol.radians(),
        );
        let sweep = self.sweep_rad();
        let delta = if sweep >= 0.0 {
            (theta - self.start_rad()).rem_euclid(TAU)
        } else {
            (self.start_rad() - theta).rem_euclid(TAU)
        };
        let t = (delta / sweep.abs()).clamp(0.0, 1.0);
        let proj_x = cx + r * theta.cos();
        let proj_y = cy + r * theta.sin();
        let projected = Point2::from_mm(proj_x, proj_y);
        let distance =
            Length::new::<millimeter>(((px - proj_x).powi(2) + (py - proj_y).powi(2)).sqrt());
        ClosestPoint2::new(Parameter::new(t), projected, distance)
    }

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

impl core::fmt::Display for Arc2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "arc2{{ c={}, r={} mm, start={} rad, sweep={} rad }}",
            self.center,
            self.radius.get::<millimeter>(),
            self.start_rad(),
            self.sweep_rad(),
        )
    }
}

#[must_use]
pub fn arc_bounding_box(
    center: Point2,
    radius: Length,
    start_angle: Angle,
    sweep_angle: Angle,
) -> Aabb2 {
    let (cx, cy) = center.coords_mm();
    let r = radius.get::<millimeter>();
    let start = start_angle.get::<radian>();
    let sweep = sweep_angle.get::<radian>();
    let end = start + sweep;
    let start_pt = Point2::from_mm(cx + r * start.cos(), cy + r * start.sin());
    let end_pt = Point2::from_mm(cx + r * end.cos(), cy + r * end.sin());
    let base = Aabb2::from_corners(start_pt, end_pt);
    let cardinals: [(f64, Point2); 4] = [
        (0.0, Point2::from_mm(cx + r, cy)),
        (PI * 0.5, Point2::from_mm(cx, cy + r)),
        (PI, Point2::from_mm(cx - r, cy)),
        (-PI * 0.5, Point2::from_mm(cx, cy - r)),
    ];
    cardinals.into_iter().fold(base, |bbox, (theta, extreme)| {
        if angles::contains(theta, start, sweep, 0.0) {
            bbox.extend_point(extreme)
        } else {
            bbox
        }
    })
}

#[cfg(test)]
mod tests {
    use super::{Arc2, arc_bounding_box};
    use bone_types::{Angle, Length, Point2, Tolerance};
    use core::f64::consts::{FRAC_PI_2, PI, TAU};
    use uom::si::angle::radian;
    use uom::si::length::millimeter;

    fn approx(a: f64, b: f64) -> bool {
        (a - b).abs() < 1e-9
    }

    #[test]
    fn center_start_end_sweep_is_ccw_and_order_picks_minor_or_major() {
        let tol = Tolerance::new(1e-9);
        let center = Point2::from_mm(0.0, 0.0);
        let east = Point2::from_mm(5.0, 0.0);
        let north = Point2::from_mm(0.0, 5.0);

        let Ok(minor) = Arc2::from_center_start_end(center, east, north, tol) else {
            panic!("east->north is a buildable quarter arc");
        };
        assert!(approx(minor.radius_mm(), 5.0));
        assert!(approx(minor.start_rad(), 0.0));
        assert!(approx(minor.sweep_rad(), FRAC_PI_2));

        let Ok(major) = Arc2::from_center_start_end(center, north, east, tol) else {
            panic!("north->east is the complementary major arc");
        };
        assert!(approx(major.start_rad(), FRAC_PI_2));
        assert!(approx(major.sweep_rad(), 3.0 * FRAC_PI_2));
    }

    #[test]
    fn center_start_end_rejects_non_finite_and_degenerate() {
        let tol = Tolerance::new(1e-9);
        let center = Point2::from_mm(0.0, 0.0);
        let coincident = Point2::from_mm(0.0, 0.0);
        let any = Point2::from_mm(5.0, 0.0);
        assert!(Arc2::from_center_start_end(center, coincident, any, tol).is_err());
        let nan = Point2::from_mm(f64::NAN, 0.0);
        assert!(Arc2::from_center_start_end(center, nan, any, tol).is_err());
    }

    fn arc(cx: f64, cy: f64, r_mm: f64, start_rad: f64, sweep_rad: f64) -> (Point2, Point2) {
        let bbox = arc_bounding_box(
            Point2::from_mm(cx, cy),
            Length::new::<millimeter>(r_mm),
            Angle::new::<radian>(start_rad),
            Angle::new::<radian>(sweep_rad),
        );
        (bbox.min(), bbox.max())
    }

    #[test]
    fn full_circle_sweep_spans_all_cardinals() {
        let (mn, mx) = arc(0.0, 0.0, 2.0, 0.0, TAU);
        let (mnx, mny) = mn.coords_mm();
        let (mxx, mxy) = mx.coords_mm();
        assert!(approx(mnx, -2.0) && approx(mny, -2.0));
        assert!(approx(mxx, 2.0) && approx(mxy, 2.0));
    }

    #[test]
    fn quarter_arc_hugs_its_quadrant() {
        let (mn, mx) = arc(0.0, 0.0, 1.0, 0.0, FRAC_PI_2);
        let (mnx, mny) = mn.coords_mm();
        let (mxx, mxy) = mx.coords_mm();
        assert!(approx(mnx, 0.0) && approx(mny, 0.0));
        assert!(approx(mxx, 1.0) && approx(mxy, 1.0));
    }

    #[test]
    fn negative_sweep_mirrors_positive() {
        let (fwd_min, fwd_max) = arc(0.0, 0.0, 1.0, 0.0, FRAC_PI_2);
        let (rev_min, rev_max) = arc(0.0, 0.0, 1.0, FRAC_PI_2, -FRAC_PI_2);
        assert_eq!(fwd_min, rev_min);
        assert_eq!(fwd_max, rev_max);
    }

    #[test]
    fn small_sweep_hugs_endpoints_only() {
        let start = FRAC_PI_2 * 0.5;
        let sweep = 0.1;
        let (mn, mx) = arc(0.0, 0.0, 1.0, start, sweep);
        let (mnx, mny) = mn.coords_mm();
        let (mxx, mxy) = mx.coords_mm();
        let end = start + sweep;
        assert!(approx(mnx, end.cos()) && approx(mny, start.sin()));
        assert!(approx(mxx, start.cos()) && approx(mxy, end.sin()));
    }

    #[test]
    fn wide_sweep_crosses_three_axes() {
        let start = FRAC_PI_2 * 0.5;
        let sweep = FRAC_PI_2 * 2.5;
        let (mn, mx) = arc(0.0, 0.0, 1.0, start, sweep);
        let (mnx, mny) = mn.coords_mm();
        let (mxx, mxy) = mx.coords_mm();
        assert!(approx(mnx, -1.0) && approx(mny, -1.0));
        assert!(approx(mxx, start.cos()) && approx(mxy, 1.0));
    }

    #[test]
    fn translated_center_offsets_the_box() {
        let (mn, mx) = arc(10.0, -5.0, 3.0, 0.0, PI);
        let (mnx, mny) = mn.coords_mm();
        let (mxx, mxy) = mx.coords_mm();
        assert!(approx(mnx, 7.0) && approx(mny, -5.0));
        assert!(approx(mxx, 13.0) && approx(mxy, -2.0));
    }
}
