use bone_types::{Aabb3, Angle, ChordHeightTolerance, Length, Plane3, Point3, Vec3};
use core::f64::consts::PI;
use uom::si::angle::radian;
use uom::si::length::millimeter;

use crate::angles;

const MAX_SEGMENTS: u32 = 4096;

#[must_use]
pub(crate) fn point_at(plane: Plane3, radius: Length, theta: Angle) -> Point3 {
    let radius_mm = radius.get::<millimeter>();
    let theta = theta.get::<radian>();
    let (cx, cy, cz) = plane.origin().coords_mm();
    let (xx, xy, xz) = plane.x_axis().components();
    let (yx, yy, yz) = plane.y_axis().components();
    let (c, s) = (theta.cos(), theta.sin());
    Point3::from_mm(
        cx + radius_mm * (c * xx + s * yx),
        cy + radius_mm * (c * xy + s * yy),
        cz + radius_mm * (c * xz + s * yz),
    )
}

#[must_use]
pub(crate) fn tangent_vec(plane: Plane3, radius: Length, theta: Angle) -> Vec3 {
    let radius_mm = radius.get::<millimeter>();
    let theta = theta.get::<radian>();
    let (xx, xy, xz) = plane.x_axis().components();
    let (yx, yy, yz) = plane.y_axis().components();
    let (c, s) = (theta.cos(), theta.sin());
    Vec3::from_mm(
        radius_mm * (-s * xx + c * yx),
        radius_mm * (-s * xy + c * yy),
        radius_mm * (-s * xz + c * yz),
    )
}

#[must_use]
pub(crate) fn local_coords(plane: Plane3, p: Point3) -> (f64, f64, f64) {
    let (dx, dy, dz) = (p - plane.origin()).coords_mm();
    let (xx, xy, xz) = plane.x_axis().components();
    let (yx, yy, yz) = plane.y_axis().components();
    let (nx, ny, nz) = plane.normal().components();
    (
        dx * xx + dy * xy + dz * xz,
        dx * yx + dy * yy + dz * yz,
        dx * nx + dy * ny + dz * nz,
    )
}

#[must_use]
pub(crate) fn bounding_box(plane: Plane3, radius: Length, start: Angle, sweep: Angle) -> Aabb3 {
    let start = start.get::<radian>();
    let sweep = sweep.get::<radian>();
    let start_pt = point_at(plane, radius, Angle::new::<radian>(start));
    let end_pt = point_at(plane, radius, Angle::new::<radian>(start + sweep));
    let base = Aabb3::from_corners(start_pt, end_pt);
    let (xx, xy, xz) = plane.x_axis().components();
    let (yx, yy, yz) = plane.y_axis().components();
    [(xx, yx), (xy, yy), (xz, yz)]
        .into_iter()
        .flat_map(|(xk, yk)| {
            let phi = yk.atan2(xk);
            [phi, phi + PI]
        })
        .filter(|&theta| angles::contains(theta, start, sweep, 0.0))
        .map(|theta| point_at(plane, radius, Angle::new::<radian>(theta)))
        .fold(base, |bb, p| bb.union(Aabb3::from_corners(p, p)))
}

#[must_use]
pub(crate) fn sample_count(
    radius: Length,
    sweep: Angle,
    tolerance: ChordHeightTolerance,
    min_segments: u32,
) -> u32 {
    let radius_mm = radius.get::<millimeter>();
    let sweep_abs = sweep.get::<radian>().abs();
    let chord_tol_mm = tolerance.millimeters();
    (min_segments..=MAX_SEGMENTS)
        .find(|&n| {
            let half_step = sweep_abs / (2.0 * f64::from(n));
            radius_mm * (1.0 - half_step.cos()) <= chord_tol_mm
        })
        .unwrap_or(MAX_SEGMENTS)
}
