use bone_kernel::{CylinderSurface, PlaneSurface, Surface3, TriMesh};
use bone_types::{
    Angle, AngleTolerance, ChordHeightTolerance, Length, Parameter, Plane3, Point3, Tolerance,
    UnitVec3, Vec3,
};
use core::f64::consts::{FRAC_PI_2, PI, TAU};
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 rad(value: f64) -> Angle {
    Angle::new::<radian>(value)
}

fn p(u: f64) -> Parameter {
    Parameter::new(u)
}

fn fmt_point(point: Point3) -> String {
    let (x, y, z) = point.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 dot_un(vector: Vec3, unit: UnitVec3) -> f64 {
    vector.dot_mm2(unit.into_vec(mm(1.0)))
}

fn fmt_surface3<S: Surface3 + Display + Debug>(
    name: &str,
    s: &S,
    chord: ChordHeightTolerance,
    angle: AngleTolerance,
) -> String {
    let samples = [
        (0.0, 0.0),
        (0.5, 0.0),
        (1.0, 0.0),
        (0.0, 1.0),
        (0.5, 0.5),
        (1.0, 1.0),
    ];
    let rows = samples
        .iter()
        .map(|&(u, v)| {
            let (du, dv) = s.partials(p(u), p(v));
            format!(
                "(u={u:.2}, v={v:.2}) -> pt={} n={} du={} dv={}",
                fmt_point(s.evaluate(p(u), p(v))),
                s.normal(p(u), p(v)),
                du,
                dv,
            )
        })
        .collect::<Vec<_>>()
        .join("\n    ");
    let mesh = s.tessellate(chord, angle);
    let verts = mesh
        .vertices()
        .iter()
        .map(|mv| format!("{} n={}", fmt_point(mv.position()), mv.normal()))
        .collect::<Vec<_>>()
        .join("\n    ");
    let tris = mesh
        .triangles()
        .iter()
        .map(|t| format!("{t:?}"))
        .collect::<Vec<_>>()
        .join(" ");
    format!(
        "{name}_display = {s}\n\
         {name}_debug   = {s:?}\n\
         {name}_samples:\n    {rows}\n\
         {name}_bbox    = {}\n\
         {name}_mesh (verts={}, tris={}):\n    {verts}\n\
         {name}_triangles: {tris}",
        s.bounding_box(),
        mesh.vertices().len(),
        mesh.triangles().len(),
    )
}

fn full_cylinder(plane: Plane3, radius: Length, height: Length) -> CylinderSurface {
    let Ok(cyl) = CylinderSurface::new(plane, radius, rad(0.0), rad(TAU), height, TOL) else {
        panic!("nondegenerate cylinder");
    };
    cyl
}

fn neg_sweep_cylinder() -> CylinderSurface {
    let Ok(cyl) = CylinderSurface::new(
        xz_plane(Point3::from_mm(1.0, -2.0, 0.0)),
        mm(3.0),
        rad(PI),
        rad(-PI),
        mm(5.0),
        TOL,
    ) else {
        panic!("nondegenerate cylinder");
    };
    cyl
}

#[test]
fn plane_surface_axis_aligned() {
    let Ok(plane) = PlaneSurface::new(
        xy_plane(Point3::from_mm(1.0, 2.0, 3.0)),
        mm(4.0),
        mm(3.0),
        TOL,
    ) else {
        panic!("positive extents");
    };
    insta::assert_snapshot!(fmt_surface3(
        "plane",
        &plane,
        ChordHeightTolerance::from_mm(0.5),
        AngleTolerance::from_radians(1.0),
    ));
}

#[test]
fn plane_surface_tilted() {
    let Ok(plane) = PlaneSurface::new(
        xz_plane(Point3::from_mm(0.0, 1.0, 0.0)),
        mm(2.0),
        mm(5.0),
        TOL,
    ) else {
        panic!("positive extents");
    };
    insta::assert_snapshot!(fmt_surface3(
        "plane_tilted",
        &plane,
        ChordHeightTolerance::from_mm(0.5),
        AngleTolerance::from_radians(1.0),
    ));
}

#[test]
fn cylinder_surface_full_circle() {
    let cyl = full_cylinder(xy_plane(Point3::origin()), mm(5.0), mm(10.0));
    insta::assert_snapshot!(fmt_surface3(
        "cyl_full",
        &cyl,
        ChordHeightTolerance::from_mm(0.5),
        AngleTolerance::from_radians(1.0),
    ));
}

#[test]
fn cylinder_surface_half_wall() {
    let Ok(cyl) = CylinderSurface::new(
        xz_plane(Point3::from_mm(1.0, 0.0, 0.0)),
        mm(2.0),
        rad(0.0),
        rad(PI),
        mm(4.0),
        TOL,
    ) else {
        panic!("nondegenerate cylinder");
    };
    insta::assert_snapshot!(fmt_surface3(
        "cyl_half",
        &cyl,
        ChordHeightTolerance::from_mm(0.5),
        AngleTolerance::from_radians(1.0),
    ));
}

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

#[test]
fn cylinder_surface_rejects_degenerate() {
    let plane = xy_plane(Point3::origin());
    assert!(CylinderSurface::new(plane, mm(0.0), rad(0.0), rad(TAU), mm(1.0), TOL).is_err());
    assert!(CylinderSurface::new(plane, mm(1.0), rad(0.0), rad(TAU), mm(0.0), TOL).is_err());
    assert!(CylinderSurface::new(plane, mm(1.0), rad(0.0), rad(0.0), mm(1.0), TOL).is_err());
    assert!(CylinderSurface::new(plane, mm(1.0), rad(0.0), rad(3.0 * PI), mm(1.0), TOL).is_err());
}

#[test]
fn plane_surface_rejects_non_finite() {
    let plane = xy_plane(Point3::origin());
    assert!(PlaneSurface::new(plane, mm(f64::NAN), mm(3.0), TOL).is_err());
    assert!(PlaneSurface::new(plane, mm(4.0), mm(f64::NAN), TOL).is_err());
    assert!(PlaneSurface::new(plane, mm(f64::INFINITY), mm(3.0), TOL).is_err());
}

#[test]
fn cylinder_surface_rejects_non_finite() {
    let plane = xy_plane(Point3::origin());
    assert!(CylinderSurface::new(plane, mm(f64::NAN), rad(0.0), rad(PI), mm(1.0), TOL).is_err());
    assert!(CylinderSurface::new(plane, mm(1.0), rad(0.0), rad(PI), mm(f64::NAN), TOL).is_err());
    assert!(CylinderSurface::new(plane, mm(1.0), rad(0.0), rad(f64::NAN), mm(1.0), TOL).is_err());
    assert!(
        CylinderSurface::new(plane, mm(1.0), rad(0.0), rad(f64::INFINITY), mm(1.0), TOL).is_err()
    );
}

#[test]
fn plane_surface_bbox_spans_corners() {
    let Ok(plane) = PlaneSurface::new(
        xy_plane(Point3::from_mm(1.0, 2.0, 5.0)),
        mm(4.0),
        mm(3.0),
        TOL,
    ) else {
        panic!("positive extents");
    };
    let bbox = plane.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 - 2.0).abs() < 1e-9 && (hy - 5.0).abs() < 1e-9);
    assert!((lz - 5.0).abs() < 1e-9 && (hz - 5.0).abs() < 1e-9);
}

#[test]
fn cylinder_surface_bbox_spans_tube() {
    let cyl = full_cylinder(xy_plane(Point3::origin()), mm(5.0), mm(10.0));
    let bbox = cyl.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 + 5.0).abs() < 1e-9 && (hy - 5.0).abs() < 1e-9);
    assert!(lz.abs() < 1e-9 && (hz - 10.0).abs() < 1e-9);
}

#[test]
fn cylinder_neg_sweep_bbox_spans_arc() {
    let bbox = neg_sweep_cylinder().bounding_box();
    let (lx, ly, lz) = bbox.min().coords_mm();
    let (hx, hy, hz) = bbox.max().coords_mm();
    assert!((lx + 2.0).abs() < 1e-9 && (hx - 4.0).abs() < 1e-9);
    assert!((ly + 7.0).abs() < 1e-9 && (hy + 2.0).abs() < 1e-9);
    assert!(lz.abs() < 1e-9 && (hz - 3.0).abs() < 1e-9);
}

fn assert_partials_orthogonal_to_normal<S: Surface3>(name: &str, s: &S) {
    let grid = [(0.0, 0.0), (0.3, 0.7), (0.5, 0.5), (1.0, 1.0), (0.8, 0.2)];
    grid.iter().for_each(|&(u, v)| {
        let (du, dv) = s.partials(p(u), p(v));
        let n = s.normal(p(u), p(v));
        assert!(
            dot_un(du, n).abs() < 1e-9,
            "{name}: du not orthogonal to normal at ({u},{v})"
        );
        assert!(
            dot_un(dv, n).abs() < 1e-9,
            "{name}: dv not orthogonal to normal at ({u},{v})"
        );
    });
}

#[test]
fn normal_is_orthogonal_to_partials() {
    let Ok(plane) = PlaneSurface::new(
        xz_plane(Point3::from_mm(1.0, 2.0, 3.0)),
        mm(4.0),
        mm(3.0),
        TOL,
    ) else {
        panic!("positive extents");
    };
    assert_partials_orthogonal_to_normal("plane", &plane);
    let cyl = full_cylinder(xz_plane(Point3::from_mm(2.0, 0.0, -1.0)), mm(3.0), mm(7.0));
    assert_partials_orthogonal_to_normal("cylinder", &cyl);
    assert_partials_orthogonal_to_normal("neg_cylinder", &neg_sweep_cylinder());
}

fn assert_normal_matches_cross<S: Surface3>(name: &str, s: &S) {
    let grid = [(0.0, 0.5), (0.4, 0.0), (1.0, 1.0)];
    grid.iter().for_each(|&(u, v)| {
        let (du, dv) = s.partials(p(u), p(v));
        let Ok(cross) = du.cross(dv).try_normalize(TOL) else {
            panic!("{name}: degenerate partials at ({u},{v})");
        };
        let n = s.normal(p(u), p(v));
        let (cx, cy, cz) = cross.components();
        let (nx, ny, nz) = n.components();
        assert!(
            (cx - nx).abs() < 1e-9 && (cy - ny).abs() < 1e-9 && (cz - nz).abs() < 1e-9,
            "{name}: normal disagrees with normalize(du x dv) at ({u},{v})"
        );
    });
}

#[test]
fn normal_agrees_with_partial_cross_product() {
    let Ok(plane) = PlaneSurface::new(xy_plane(Point3::origin()), mm(4.0), mm(3.0), TOL) else {
        panic!("positive extents");
    };
    assert_normal_matches_cross("plane", &plane);
    let cyl = full_cylinder(xy_plane(Point3::origin()), mm(5.0), mm(10.0));
    assert_normal_matches_cross("cylinder", &cyl);
    assert_normal_matches_cross("neg_cylinder", &neg_sweep_cylinder());
}

fn assert_winding_matches_normals<S: Surface3>(name: &str, s: &S) {
    let mesh: TriMesh = s.tessellate(
        ChordHeightTolerance::from_mm(0.25),
        AngleTolerance::from_radians(0.4),
    );
    let verts = mesh.vertices();
    mesh.triangles().iter().for_each(|&[a, b, c]| {
        let va = verts[a as usize];
        let vb = verts[b as usize];
        let vc = verts[c as usize];
        let face = (vb.position() - va.position()).cross(vc.position() - va.position());
        [va, vb, vc].iter().for_each(|vert| {
            assert!(
                dot_un(face, vert.normal()) > 0.0,
                "{name}: triangle [{a},{b},{c}] winds against vertex normal"
            );
        });
    });
}

#[test]
fn tessellation_winding_follows_normals() {
    let Ok(plane) = PlaneSurface::new(xy_plane(Point3::origin()), mm(4.0), mm(3.0), TOL) else {
        panic!("positive extents");
    };
    assert_winding_matches_normals("plane", &plane);
    let cyl = full_cylinder(xy_plane(Point3::origin()), mm(5.0), mm(10.0));
    assert_winding_matches_normals("cylinder", &cyl);
    let Ok(arc_wall) = CylinderSurface::new(
        xz_plane(Point3::origin()),
        mm(2.0),
        rad(FRAC_PI_2),
        rad(PI),
        mm(3.0),
        TOL,
    ) else {
        panic!("nondegenerate cylinder");
    };
    assert_winding_matches_normals("arc_wall", &arc_wall);
    assert_winding_matches_normals("neg_wall", &neg_sweep_cylinder());
}

#[test]
fn tessellation_is_deterministic() {
    let cyl = full_cylinder(xy_plane(Point3::from_mm(1.0, -2.0, 0.5)), mm(4.0), mm(6.0));
    let chord = ChordHeightTolerance::from_mm(0.3);
    let angle = AngleTolerance::from_radians(0.5);
    assert_eq!(cyl.tessellate(chord, angle), cyl.tessellate(chord, angle));
}

#[test]
fn cylinder_tessellation_count_responds_to_both_tolerances() {
    let cyl = full_cylinder(xy_plane(Point3::origin()), mm(5.0), mm(10.0));
    let coarse = cyl.tessellate(
        ChordHeightTolerance::from_mm(1.0),
        AngleTolerance::from_radians(1.5),
    );
    let chord_driven = cyl.tessellate(
        ChordHeightTolerance::from_mm(0.05),
        AngleTolerance::from_radians(1.5),
    );
    let angle_driven = cyl.tessellate(
        ChordHeightTolerance::from_mm(1.0),
        AngleTolerance::from_radians(0.1),
    );
    assert!(chord_driven.vertices().len() > coarse.vertices().len());
    assert!(angle_driven.vertices().len() > coarse.vertices().len());
}

#[test]
fn plane_surface_membership() {
    let Ok(plane) = PlaneSurface::new(
        xy_plane(Point3::from_mm(1.0, 1.0, 2.0)),
        mm(4.0),
        mm(3.0),
        TOL,
    ) else {
        panic!("positive extents");
    };
    assert!(plane.contains_point(plane.evaluate(p(0.5), p(0.5)), Tolerance::new(1e-6)));
    assert!(!plane.contains_point(Point3::from_mm(2.0, 2.0, 3.0), Tolerance::new(1e-6)));
    assert!(!plane.contains_point(Point3::from_mm(20.0, 2.0, 2.0), Tolerance::new(1e-6)));
}

#[test]
fn cylinder_surface_membership() {
    let Ok(cyl) = CylinderSurface::new(
        xy_plane(Point3::origin()),
        mm(5.0),
        rad(0.0),
        rad(PI),
        mm(10.0),
        TOL,
    ) else {
        panic!("nondegenerate cylinder");
    };
    let on = Tolerance::new(1e-6);
    assert!(cyl.contains_point(cyl.evaluate(p(0.5), p(0.5)), on));
    assert!(!cyl.contains_point(Point3::from_mm(6.0, 0.0, 5.0), on));
    assert!(!cyl.contains_point(Point3::from_mm(5.0, 0.0, 12.0), on));
    assert!(!cyl.contains_point(Point3::from_mm(0.0, -5.0, 5.0), on));
}

#[test]
fn cylinder_full_circle_membership_wraps() {
    let cyl = full_cylinder(xy_plane(Point3::origin()), mm(5.0), mm(10.0));
    let on = Tolerance::new(1e-6);
    [0.0, 0.25, 0.5, 0.75].into_iter().for_each(|u| {
        assert!(
            cyl.contains_point(cyl.evaluate(p(u), p(0.5)), on),
            "full sweep should contain its surface point at u={u}"
        );
    });
    assert!(!cyl.contains_point(Point3::from_mm(0.0, 0.0, 5.0), on));
    assert!(!cyl.contains_point(Point3::from_mm(5.0, 0.0, 12.0), on));
}
