use bone_kernel::{Arc3, Circle3, Curve3, Line3, Polyline3};
use bone_types::{
    Angle, ChordHeightTolerance, Length, Parameter, Plane3, Point3, Tolerance, UnitVec3,
};
use core::fmt::{Debug, Display};
use uom::si::angle::radian;
use uom::si::length::millimeter;

const TOL: Tolerance = Tolerance::new(1e-9);

fn mm(value: f64) -> Length {
    Length::new::<millimeter>(value)
}

fn fmt_point(p: Point3) -> String {
    let (x, y, z) = p.coords_mm();
    format!("({x:.6}, {y:.6}, {z:.6})")
}

fn xy_plane(origin: Point3) -> Plane3 {
    Plane3::new_unchecked(origin, UnitVec3::x_axis(), UnitVec3::y_axis())
}

fn xz_plane(origin: Point3) -> Plane3 {
    Plane3::new_unchecked(origin, UnitVec3::x_axis(), UnitVec3::z_axis())
}

fn fmt_curve3<C: Curve3 + Display + Debug>(
    name: &str,
    c: &C,
    tess: ChordHeightTolerance,
) -> String {
    let samples = [0.0, 0.25, 0.5, 0.75, 1.0].map(Parameter::new);
    let points = samples
        .iter()
        .map(|t| {
            format!(
                "t={:.2} -> p={} v={}",
                t.value(),
                fmt_point(c.evaluate(*t)),
                c.derivative(*t)
            )
        })
        .collect::<Vec<_>>()
        .join("\n    ");
    let bbox = c.bounding_box();
    let closest = c.closest_point(Point3::origin(), TOL);
    let tess_pts = c.tessellate(tess);
    let tess_str = tess_pts
        .iter()
        .map(|p| fmt_point(*p))
        .collect::<Vec<_>>()
        .join("\n    ");
    format!(
        "{name}_display = {c}\n\
         {name}_debug   = {c:?}\n\
         {name}_samples:\n    {points}\n\
         {name}_bbox    = {bbox}\n\
         {name}_closest_origin = {closest}\n\
         {name}_tessellate (n={}):\n    {tess_str}",
        tess_pts.len()
    )
}

#[test]
fn line3_surface() {
    let Ok(line) = Line3::new(
        Point3::from_mm(1.0, 2.0, 3.0),
        Point3::from_mm(7.0, 6.0, 5.0),
        TOL,
    ) else {
        panic!("endpoints distinct");
    };
    insta::assert_snapshot!(fmt_curve3(
        "line3",
        &line,
        ChordHeightTolerance::from_mm(0.5)
    ));
}

#[test]
fn circle3_xy_surface() {
    let Ok(circle) = Circle3::new(xy_plane(Point3::from_mm(2.0, -1.0, 0.0)), mm(3.0), TOL) else {
        panic!("radius positive");
    };
    insta::assert_snapshot!(fmt_curve3(
        "circle3_xy",
        &circle,
        ChordHeightTolerance::from_mm(0.5)
    ));
}

#[test]
fn circle3_tilted_surface() {
    let Ok(circle) = Circle3::new(xz_plane(Point3::from_mm(0.0, 4.0, 0.0)), mm(5.0), TOL) else {
        panic!("radius positive");
    };
    insta::assert_snapshot!(fmt_curve3(
        "circle3_tilted",
        &circle,
        ChordHeightTolerance::from_mm(0.5)
    ));
}

#[test]
fn arc3_quarter_surface() {
    let Ok(arc) = Arc3::new(
        xy_plane(Point3::origin()),
        mm(5.0),
        Angle::new::<radian>(0.0),
        Angle::new::<radian>(core::f64::consts::FRAC_PI_2),
        TOL,
    ) else {
        panic!("sweep nonzero");
    };
    insta::assert_snapshot!(fmt_curve3(
        "arc3_q",
        &arc,
        ChordHeightTolerance::from_mm(0.5)
    ));
}

#[test]
fn arc3_tilted_half_surface() {
    let Ok(arc) = Arc3::new(
        xz_plane(Point3::from_mm(1.0, 0.0, 1.0)),
        mm(2.0),
        Angle::new::<radian>(core::f64::consts::PI),
        Angle::new::<radian>(-core::f64::consts::PI),
        TOL,
    ) else {
        panic!("sweep nonzero");
    };
    insta::assert_snapshot!(fmt_curve3(
        "arc3_h",
        &arc,
        ChordHeightTolerance::from_mm(0.5)
    ));
}

#[test]
fn polyline3_surface() {
    let Ok(poly) = Polyline3::new(
        vec![
            Point3::from_mm(0.0, 0.0, 0.0),
            Point3::from_mm(2.0, 0.0, 1.0),
            Point3::from_mm(2.0, 3.0, 1.0),
            Point3::from_mm(-1.0, 3.0, 4.0),
        ],
        TOL,
    ) else {
        panic!("at least two distinct vertices");
    };
    insta::assert_snapshot!(fmt_curve3(
        "polyline3",
        &poly,
        ChordHeightTolerance::from_mm(0.5)
    ));
}

#[test]
fn line3_rejects_degenerate_endpoints() {
    assert!(
        Line3::new(
            Point3::from_mm(1.0, 1.0, 1.0),
            Point3::from_mm(1.0, 1.0, 1.0 + 1e-12),
            TOL,
        )
        .is_err()
    );
}

#[test]
fn circle3_rejects_zero_radius() {
    assert!(Circle3::new(xy_plane(Point3::origin()), mm(0.0), TOL).is_err());
}

#[test]
fn arc3_rejects_zero_sweep() {
    assert!(
        Arc3::new(
            xy_plane(Point3::origin()),
            mm(1.0),
            Angle::new::<radian>(0.0),
            Angle::new::<radian>(0.0),
            TOL,
        )
        .is_err()
    );
}

#[test]
fn polyline3_rejects_too_few_and_zero_segments() {
    assert!(Polyline3::new(vec![Point3::origin()], TOL).is_err());
    assert!(
        Polyline3::new(
            vec![
                Point3::from_mm(1.0, 1.0, 1.0),
                Point3::from_mm(1.0, 1.0, 1.0)
            ],
            TOL,
        )
        .is_err()
    );
}

#[test]
fn circle3_xy_bbox_is_flat_in_normal() {
    let Ok(circle) = Circle3::new(xy_plane(Point3::from_mm(2.0, -1.0, 5.0)), mm(3.0), TOL) else {
        panic!("radius positive");
    };
    let bbox = circle.bounding_box();
    let (lx, ly, lz) = bbox.min().coords_mm();
    let (hx, hy, hz) = bbox.max().coords_mm();
    assert!((lx + 1.0).abs() < 1e-9 && (hx - 5.0).abs() < 1e-9);
    assert!((ly + 4.0).abs() < 1e-9 && (hy - 2.0).abs() < 1e-9);
    assert!((lz - 5.0).abs() < 1e-9 && (hz - 5.0).abs() < 1e-9);
}

#[test]
fn circle3_tilted_bbox_matches_projection() {
    let Ok(circle) = Circle3::new(xz_plane(Point3::from_mm(0.0, 4.0, 0.0)), mm(5.0), TOL) else {
        panic!("radius positive");
    };
    let bbox = circle.bounding_box();
    let (lx, ly, lz) = bbox.min().coords_mm();
    let (hx, hy, hz) = bbox.max().coords_mm();
    assert!((lx + 5.0).abs() < 1e-9 && (hx - 5.0).abs() < 1e-9);
    assert!((ly - 4.0).abs() < 1e-9 && (hy - 4.0).abs() < 1e-9);
    assert!((lz + 5.0).abs() < 1e-9 && (hz - 5.0).abs() < 1e-9);
}

#[test]
fn polyline3_bbox_spans_vertices() {
    let Ok(poly) = Polyline3::new(
        vec![
            Point3::from_mm(0.0, 0.0, 0.0),
            Point3::from_mm(2.0, -1.0, 5.0),
            Point3::from_mm(-3.0, 4.0, 1.0),
        ],
        TOL,
    ) else {
        panic!("distinct vertices");
    };
    let bbox = poly.bounding_box();
    let (lx, ly, lz) = bbox.min().coords_mm();
    let (hx, hy, hz) = bbox.max().coords_mm();
    assert!((lx + 3.0).abs() < 1e-9 && (ly + 1.0).abs() < 1e-9 && lz.abs() < 1e-9);
    assert!((hx - 2.0).abs() < 1e-9 && (hy - 4.0).abs() < 1e-9 && (hz - 5.0).abs() < 1e-9);
}

fn dist_mm(a: Point3, b: Point3) -> f64 {
    (a - b).norm_mm()
}

fn brute_min_distance<C: Curve3>(curve: &C, query: Point3) -> f64 {
    (0..=20_000)
        .map(|i| {
            dist_mm(
                curve.evaluate(Parameter::new(f64::from(i) / 20_000.0)),
                query,
            )
        })
        .fold(f64::INFINITY, f64::min)
}

fn assert_closest_contract<C: Curve3>(name: &str, curve: &C, query: Point3) {
    let cp = curve.closest_point(query, TOL);
    let reported = cp.distance().get::<millimeter>();
    let to_returned = dist_mm(query, cp.point());
    assert!(
        (reported - to_returned).abs() < 1e-9,
        "{name}: reported distance {reported} disagrees with distance to returned point {to_returned}"
    );
    assert!(
        reported <= brute_min_distance(curve, query) + 1e-6,
        "{name}: reported distance {reported} undershoots the nearest sampled point"
    );
}

#[test]
fn closest_point_distance_matches_returned_point() {
    let Ok(line) = Line3::new(
        Point3::from_mm(0.0, 0.0, 0.0),
        Point3::from_mm(4.0, 0.0, 0.0),
        TOL,
    ) else {
        panic!("distinct endpoints");
    };
    assert_closest_contract("line/side", &line, Point3::from_mm(2.0, 3.0, 0.0));
    assert_closest_contract("line/past_end", &line, Point3::from_mm(9.0, 1.0, -2.0));
    assert_closest_contract("line/before_start", &line, Point3::from_mm(-5.0, 2.0, 0.0));

    let Ok(quarter) = Arc3::new(
        xy_plane(Point3::origin()),
        mm(5.0),
        Angle::new::<radian>(0.0),
        Angle::new::<radian>(core::f64::consts::FRAC_PI_2),
        TOL,
    ) else {
        panic!("sweep nonzero");
    };
    assert_closest_contract("arc/inside", &quarter, Point3::from_mm(3.0, 3.0, 0.0));
    assert_closest_contract(
        "arc/inside_offplane",
        &quarter,
        Point3::from_mm(3.0, 3.0, 4.0),
    );
    assert_closest_contract(
        "arc/below_start",
        &quarter,
        Point3::from_mm(10.0, -10.0, 0.0),
    );
    assert_closest_contract("arc/past_end", &quarter, Point3::from_mm(-10.0, 10.0, 0.0));

    let Ok(tilted) = Arc3::new(
        xz_plane(Point3::from_mm(1.0, 0.0, 1.0)),
        mm(2.0),
        Angle::new::<radian>(core::f64::consts::PI),
        Angle::new::<radian>(-core::f64::consts::PI),
        TOL,
    ) else {
        panic!("sweep nonzero");
    };
    assert_closest_contract("tilted/origin", &tilted, Point3::origin());
    assert_closest_contract("tilted/far", &tilted, Point3::from_mm(-5.0, 3.0, 5.0));

    let Ok(circle) = Circle3::new(xy_plane(Point3::from_mm(2.0, -1.0, 0.0)), mm(3.0), TOL) else {
        panic!("radius positive");
    };
    assert_closest_contract("circle/in_plane", &circle, Point3::from_mm(8.0, -1.0, 0.0));
    assert_closest_contract("circle/offplane", &circle, Point3::from_mm(2.0, -1.0, 6.0));
    assert_closest_contract("circle/center", &circle, Point3::from_mm(2.0, -1.0, 0.0));

    let Ok(poly) = Polyline3::new(
        vec![
            Point3::from_mm(0.0, 0.0, 0.0),
            Point3::from_mm(2.0, 0.0, 1.0),
            Point3::from_mm(2.0, 3.0, 1.0),
        ],
        TOL,
    ) else {
        panic!("distinct vertices");
    };
    assert_closest_contract("poly/mid", &poly, Point3::from_mm(1.0, 2.0, 3.0));
    assert_closest_contract("poly/past_end", &poly, Point3::from_mm(5.0, 5.0, 0.0));
}
