use bone_types::dimensioned_serde;
use bone_types::{
    Angle, Axis3, CoordinateSystem3, DatumTarget, Length, Plane3, Point3, Tolerance, UnitVec3,
    Vec3, millimeter,
};
use serde::{Deserialize, Serialize};

const DATUM_TOL: Tolerance = Tolerance::new(1e-9);
const DEGENERATE_DOT: f64 = 1.0e-6;
const PARALLEL_DOT: f64 = 1.0 - DEGENERATE_DOT;
const COINCIDENCE_TOL_MM: f64 = 1.0e-6;

#[must_use]
fn mm(value: f64) -> Length {
    Length::new::<millimeter>(value)
}

#[must_use]
fn vec_of(point: Point3) -> Vec3 {
    let (x, y, z) = point.coords_mm();
    Vec3::from_mm(x, y, z)
}

#[must_use]
fn point_of(vector: Vec3) -> Point3 {
    let (x, y, z) = vector.coords_mm();
    Point3::from_mm(x, y, z)
}

#[must_use]
fn dot_dir(vector: Vec3, dir: UnitVec3) -> f64 {
    let (vx, vy, vz) = vector.coords_mm();
    let (dx, dy, dz) = dir.components();
    vx * dx + vy * dy + vz * dz
}

#[derive(Copy, Clone, Debug, PartialEq)]
pub enum ReferenceGeometry {
    Plane(Plane3),
    Axis(Axis3),
    Point(Point3),
    CoordinateSystem(CoordinateSystem3),
}

impl ReferenceGeometry {
    fn as_plane(self) -> Result<Plane3, DatumError> {
        match self {
            Self::Plane(plane) => Ok(plane),
            Self::CoordinateSystem(csys) => Ok(csys.xy_plane()),
            Self::Axis(_) | Self::Point(_) => Err(DatumError::WrongReferenceKind),
        }
    }

    fn as_axis(self) -> Result<Axis3, DatumError> {
        match self {
            Self::Axis(axis) => Ok(axis),
            Self::Plane(_) | Self::Point(_) | Self::CoordinateSystem(_) => {
                Err(DatumError::WrongReferenceKind)
            }
        }
    }

    fn as_point(self) -> Result<Point3, DatumError> {
        match self {
            Self::Point(point) => Ok(point),
            Self::CoordinateSystem(csys) => Ok(csys.origin()),
            Self::Plane(_) | Self::Axis(_) => Err(DatumError::WrongReferenceKind),
        }
    }

    fn as_direction(self) -> Result<UnitVec3, DatumError> {
        match self {
            Self::Axis(axis) => Ok(axis.direction()),
            Self::Plane(plane) => Ok(plane.normal()),
            Self::CoordinateSystem(csys) => Ok(csys.z_axis()),
            Self::Point(_) => Err(DatumError::WrongReferenceKind),
        }
    }
}

#[derive(Copy, Clone, Debug, PartialEq)]
pub enum DatumGeometry {
    Plane(Plane3),
    Axis(Axis3),
    Point(Point3),
    CoordinateSystem(CoordinateSystem3),
}

impl DatumGeometry {
    #[must_use]
    pub fn reference(self) -> ReferenceGeometry {
        match self {
            Self::Plane(plane) => ReferenceGeometry::Plane(plane),
            Self::Axis(axis) => ReferenceGeometry::Axis(axis),
            Self::Point(point) => ReferenceGeometry::Point(point),
            Self::CoordinateSystem(csys) => ReferenceGeometry::CoordinateSystem(csys),
        }
    }

    #[must_use]
    pub fn plane(self) -> Option<Plane3> {
        match self {
            Self::Plane(plane) => Some(plane),
            Self::CoordinateSystem(csys) => Some(csys.xy_plane()),
            Self::Axis(_) | Self::Point(_) => None,
        }
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub enum DatumGap {
    TangentSurface,
    CenterOfFace,
    ArcCenter,
    AlongCurve,
    ParallelToScreen,
    CurvedFaceReference,
    CurvedEdgeReference,
    NonParallelMidPlane,
}

impl core::fmt::Display for DatumGap {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        f.write_str(match self {
            Self::TangentSurface => "a plane tangent to a curved surface",
            Self::CenterOfFace => "the center of a face",
            Self::ArcCenter => "the center of an arc edge",
            Self::AlongCurve => "a point spaced along a curve",
            Self::ParallelToScreen => "a plane parallel to the screen",
            Self::CurvedFaceReference => "a reference to a curved face",
            Self::CurvedEdgeReference => "a reference to a curved edge",
            Self::NonParallelMidPlane => "a mid plane between non-parallel references",
        })
    }
}

#[derive(Clone, Debug, PartialEq, thiserror::Error)]
pub enum DatumError {
    #[error("datum recipe references geometry of the wrong kind")]
    WrongReferenceKind,
    #[error("datum reference {0} could not be resolved")]
    Unresolved(Box<DatumTarget>),
    #[error("datum recipe inputs are degenerate")]
    DegenerateInput,
    #[error("datum recipe needs a reference it was not given")]
    MissingReference,
    #[error("datum recipe needs {detail}, which the kernel does not yet solve")]
    Unsupported { detail: DatumGap },
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(deny_unknown_fields)]
pub enum PlaneRecipe {
    Coincident {
        reference: DatumTarget,
    },
    Offset {
        from: DatumTarget,
        #[serde(with = "dimensioned_serde::length_si")]
        distance: Length,
        #[serde(default)]
        flip: bool,
    },
    AtAngle {
        plane: DatumTarget,
        axis: DatumTarget,
        #[serde(with = "dimensioned_serde::angle_si")]
        angle: Angle,
    },
    ThreePoints {
        a: DatumTarget,
        b: DatumTarget,
        c: DatumTarget,
    },
    NormalToCurve {
        curve: DatumTarget,
        point: DatumTarget,
        #[serde(default)]
        set_origin_on_curve: bool,
    },
    Parallel {
        plane: DatumTarget,
        through: DatumTarget,
    },
    MidPlane {
        first: DatumTarget,
        second: DatumTarget,
    },
    TangentToSurface {
        surface: DatumTarget,
        reference: DatumTarget,
        #[serde(with = "dimensioned_serde::angle_si")]
        angle: Angle,
    },
    ParallelToScreen,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(deny_unknown_fields)]
pub enum AxisRecipe {
    OneLine {
        reference: DatumTarget,
    },
    TwoPlanes {
        a: DatumTarget,
        b: DatumTarget,
    },
    TwoPoints {
        a: DatumTarget,
        b: DatumTarget,
    },
    CylindricalFace {
        face: DatumTarget,
    },
    PointAndFace {
        point: DatumTarget,
        face: DatumTarget,
    },
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
pub enum AlongCurvePlacement {
    Distance(#[serde(with = "dimensioned_serde::length_si")] Length),
    Percentage(f64),
    EvenlyDistribute(u32),
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(deny_unknown_fields)]
pub enum PointRecipe {
    OnPoint {
        reference: DatumTarget,
    },
    Intersection {
        a: DatumTarget,
        b: DatumTarget,
    },
    Projection {
        point: DatumTarget,
        onto: DatumTarget,
    },
    ArcCenter {
        edge: DatumTarget,
    },
    CenterOfFace {
        face: DatumTarget,
    },
    AlongCurve {
        curve: DatumTarget,
        placement: AlongCurvePlacement,
    },
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(deny_unknown_fields)]
pub struct CsysAxisRef {
    pub target: DatumTarget,
    #[serde(default)]
    pub reverse: bool,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(deny_unknown_fields)]
pub struct CsysRotation {
    #[serde(with = "dimensioned_serde::angle_si")]
    pub x: Angle,
    #[serde(with = "dimensioned_serde::angle_si")]
    pub y: Angle,
    #[serde(with = "dimensioned_serde::angle_si")]
    pub z: Angle,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(deny_unknown_fields)]
pub struct CsysRecipe {
    pub origin: DatumTarget,
    #[serde(default)]
    pub x_axis: Option<CsysAxisRef>,
    #[serde(default)]
    pub y_axis: Option<CsysAxisRef>,
    #[serde(default)]
    pub z_axis: Option<CsysAxisRef>,
    #[serde(default)]
    pub position: Option<Vec3>,
    #[serde(default)]
    pub rotation: Option<CsysRotation>,
}

#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(deny_unknown_fields)]
pub enum DatumFeature {
    Plane(Box<PlaneRecipe>),
    Axis(Box<AxisRecipe>),
    Point(Box<PointRecipe>),
    CoordinateSystem(Box<CsysRecipe>),
}

impl DatumFeature {
    #[must_use]
    pub fn plane(recipe: PlaneRecipe) -> Self {
        Self::Plane(Box::new(recipe))
    }

    #[must_use]
    pub fn axis(recipe: AxisRecipe) -> Self {
        Self::Axis(Box::new(recipe))
    }

    #[must_use]
    pub fn point(recipe: PointRecipe) -> Self {
        Self::Point(Box::new(recipe))
    }

    #[must_use]
    pub fn coordinate_system(recipe: CsysRecipe) -> Self {
        Self::CoordinateSystem(Box::new(recipe))
    }

    #[must_use]
    pub fn targets(&self) -> Vec<DatumTarget> {
        match self {
            Self::Plane(recipe) => recipe.targets(),
            Self::Axis(recipe) => recipe.targets(),
            Self::Point(recipe) => recipe.targets(),
            Self::CoordinateSystem(recipe) => recipe.targets(),
        }
    }

    pub fn evaluate(
        &self,
        resolve: &mut dyn FnMut(DatumTarget) -> Result<ReferenceGeometry, DatumError>,
    ) -> Result<DatumGeometry, DatumError> {
        match self {
            Self::Plane(recipe) => recipe.solve(resolve).map(DatumGeometry::Plane),
            Self::Axis(recipe) => recipe.solve(resolve).map(DatumGeometry::Axis),
            Self::Point(recipe) => recipe.solve(resolve).map(DatumGeometry::Point),
            Self::CoordinateSystem(recipe) => {
                recipe.solve(resolve).map(DatumGeometry::CoordinateSystem)
            }
        }
    }
}

type Resolver<'a> = &'a mut dyn FnMut(DatumTarget) -> Result<ReferenceGeometry, DatumError>;

impl PlaneRecipe {
    #[must_use]
    fn targets(&self) -> Vec<DatumTarget> {
        match *self {
            Self::Coincident { reference } => vec![reference],
            Self::Offset { from, .. } => vec![from],
            Self::AtAngle { plane, axis, .. } => vec![plane, axis],
            Self::ThreePoints { a, b, c } => vec![a, b, c],
            Self::NormalToCurve { curve, point, .. } => vec![curve, point],
            Self::Parallel { plane, through } => vec![plane, through],
            Self::MidPlane { first, second } => vec![first, second],
            Self::TangentToSurface {
                surface, reference, ..
            } => vec![surface, reference],
            Self::ParallelToScreen => Vec::new(),
        }
    }

    fn solve(&self, resolve: Resolver<'_>) -> Result<Plane3, DatumError> {
        match *self {
            Self::Coincident { reference } => resolve(reference)?.as_plane(),
            Self::Offset {
                from,
                distance,
                flip,
            } => {
                let plane = resolve(from)?.as_plane()?;
                Ok(plane_offset(plane, distance, flip))
            }
            Self::AtAngle { plane, axis, angle } => {
                let base = resolve(plane)?.as_plane()?;
                let edge = resolve(axis)?.as_axis()?;
                plane_at_angle(base, edge, angle)
            }
            Self::ThreePoints { a, b, c } => {
                let a = resolve(a)?.as_point()?;
                let b = resolve(b)?.as_point()?;
                let c = resolve(c)?.as_point()?;
                plane_through_three_points(a, b, c)
            }
            Self::NormalToCurve {
                curve,
                point,
                set_origin_on_curve,
            } => {
                let axis = resolve(curve)?.as_axis()?;
                let point = resolve(point)?.as_point()?;
                plane_normal_to_axis(axis, point, set_origin_on_curve)
            }
            Self::Parallel { plane, through } => {
                let base = resolve(plane)?.as_plane()?;
                let point = resolve(through)?.as_point()?;
                Ok(Plane3::new_unchecked(point, base.x_axis(), base.y_axis()))
            }
            Self::MidPlane { first, second } => {
                let a = resolve(first)?.as_plane()?;
                let b = resolve(second)?.as_plane()?;
                plane_midplane(a, b)
            }
            Self::TangentToSurface { .. } => Err(DatumError::Unsupported {
                detail: DatumGap::TangentSurface,
            }),
            Self::ParallelToScreen => Err(DatumError::Unsupported {
                detail: DatumGap::ParallelToScreen,
            }),
        }
    }
}

impl AxisRecipe {
    #[must_use]
    fn targets(&self) -> Vec<DatumTarget> {
        match *self {
            Self::OneLine { reference } => vec![reference],
            Self::TwoPlanes { a, b } | Self::TwoPoints { a, b } => vec![a, b],
            Self::CylindricalFace { face } => vec![face],
            Self::PointAndFace { point, face } => vec![point, face],
        }
    }

    fn solve(&self, resolve: Resolver<'_>) -> Result<Axis3, DatumError> {
        match *self {
            Self::OneLine { reference } | Self::CylindricalFace { face: reference } => {
                resolve(reference)?.as_axis()
            }
            Self::TwoPlanes { a, b } => {
                let a = resolve(a)?.as_plane()?;
                let b = resolve(b)?.as_plane()?;
                plane_plane_axis(a, b)
            }
            Self::TwoPoints { a, b } => {
                let a = resolve(a)?.as_point()?;
                let b = resolve(b)?.as_point()?;
                Axis3::through(a, b, DATUM_TOL).map_err(|_| DatumError::DegenerateInput)
            }
            Self::PointAndFace { point, face } => {
                let point = resolve(point)?.as_point()?;
                let normal = resolve(face)?.as_plane()?.normal();
                Ok(Axis3::new(point, normal))
            }
        }
    }
}

impl PointRecipe {
    #[must_use]
    fn targets(&self) -> Vec<DatumTarget> {
        match *self {
            Self::OnPoint { reference } => vec![reference],
            Self::Intersection { a, b } => vec![a, b],
            Self::Projection { point, onto } => vec![point, onto],
            Self::ArcCenter { edge } => vec![edge],
            Self::CenterOfFace { face } => vec![face],
            Self::AlongCurve { curve, .. } => vec![curve],
        }
    }

    fn solve(&self, resolve: Resolver<'_>) -> Result<Point3, DatumError> {
        match *self {
            Self::OnPoint { reference } => resolve(reference)?.as_point(),
            Self::Intersection { a, b } => intersection_point(resolve(a)?, resolve(b)?),
            Self::Projection { point, onto } => {
                let point = resolve(point)?.as_point()?;
                project_point(point, resolve(onto)?)
            }
            Self::ArcCenter { .. } => Err(DatumError::Unsupported {
                detail: DatumGap::ArcCenter,
            }),
            Self::CenterOfFace { .. } => Err(DatumError::Unsupported {
                detail: DatumGap::CenterOfFace,
            }),
            Self::AlongCurve { .. } => Err(DatumError::Unsupported {
                detail: DatumGap::AlongCurve,
            }),
        }
    }
}

impl CsysRecipe {
    #[must_use]
    fn targets(&self) -> Vec<DatumTarget> {
        [
            Some(self.origin),
            self.x_axis.map(|a| a.target),
            self.y_axis.map(|a| a.target),
            self.z_axis.map(|a| a.target),
        ]
        .into_iter()
        .flatten()
        .collect()
    }

    fn solve(&self, resolve: Resolver<'_>) -> Result<CoordinateSystem3, DatumError> {
        let origin = resolve(self.origin)?.as_point()?;
        let dirs = AxisDirections {
            x: resolve_direction(resolve, self.x_axis)?,
            y: resolve_direction(resolve, self.y_axis)?,
            z: resolve_direction(resolve, self.z_axis)?,
        };
        let base = csys_frame(origin, dirs)?;
        Ok(apply_local_transform(base, self.position, self.rotation))
    }
}

fn resolve_direction(
    resolve: Resolver<'_>,
    slot: Option<CsysAxisRef>,
) -> Result<Option<UnitVec3>, DatumError> {
    match slot {
        None => Ok(None),
        Some(axis) => {
            let dir = resolve(axis.target)?.as_direction()?;
            Ok(Some(if axis.reverse { dir.reversed() } else { dir }))
        }
    }
}

#[derive(Copy, Clone)]
struct AxisDirections {
    x: Option<UnitVec3>,
    y: Option<UnitVec3>,
    z: Option<UnitVec3>,
}

fn auto_hint(normal: UnitVec3) -> UnitVec3 {
    if normal.dot(UnitVec3::x_axis()).abs() < 0.9 {
        UnitVec3::x_axis()
    } else {
        UnitVec3::y_axis()
    }
}

fn plane_from_normal(origin: Point3, normal: UnitVec3) -> Result<Plane3, DatumError> {
    plane_from_normal_hint(origin, normal, auto_hint(normal))
}

fn plane_from_normal_hint(
    origin: Point3,
    normal: UnitVec3,
    hint: UnitVec3,
) -> Result<Plane3, DatumError> {
    let x = orthonormal_against(hint, normal)?;
    let y = normal
        .cross(x, DATUM_TOL)
        .map_err(|_| DatumError::DegenerateInput)?;
    Ok(Plane3::new_unchecked(origin, x, y))
}

fn plane_offset(plane: Plane3, distance: Length, flip: bool) -> Plane3 {
    let signed = if flip { -distance } else { distance };
    let origin = plane.origin() + plane.normal().into_vec(signed);
    Plane3::new_unchecked(origin, plane.x_axis(), plane.y_axis())
}

fn plane_at_angle(base: Plane3, edge: Axis3, angle: Angle) -> Result<Plane3, DatumError> {
    if edge.direction().dot(base.normal()).abs() > DEGENERATE_DOT {
        return Err(DatumError::DegenerateInput);
    }
    let rotation = bone_types::AxisAngle::new(edge.direction(), angle);
    let normal = base.normal().rotated(rotation);
    plane_from_normal_hint(edge.origin(), normal, edge.direction())
}

fn plane_through_three_points(
    first: Point3,
    second: Point3,
    third: Point3,
) -> Result<Plane3, DatumError> {
    let to_second = second - first;
    let to_third = third - first;
    let normal = to_second
        .cross(to_third)
        .try_normalize(DATUM_TOL)
        .map_err(|_| DatumError::DegenerateInput)?;
    let hint = to_second
        .try_normalize(DATUM_TOL)
        .unwrap_or(auto_hint(normal));
    plane_from_normal_hint(first, normal, hint)
}

fn plane_normal_to_axis(
    axis: Axis3,
    point: Point3,
    set_origin_on_curve: bool,
) -> Result<Plane3, DatumError> {
    let origin = if set_origin_on_curve {
        foot_on_axis(point, axis)
    } else {
        point
    };
    plane_from_normal(origin, axis.direction())
}

fn plane_midplane(a: Plane3, b: Plane3) -> Result<Plane3, DatumError> {
    if a.normal().dot(b.normal()).abs() < PARALLEL_DOT {
        return Err(DatumError::Unsupported {
            detail: DatumGap::NonParallelMidPlane,
        });
    }
    let gap = dot_dir(b.origin() - a.origin(), a.normal());
    let origin = a.origin() + a.normal().into_vec(mm(gap / 2.0));
    Ok(Plane3::new_unchecked(origin, a.x_axis(), a.y_axis()))
}

fn plane_plane_axis(a: Plane3, b: Plane3) -> Result<Axis3, DatumError> {
    let n1 = a.normal();
    let n2 = b.normal();
    let dir = n1
        .cross(n2, DATUM_TOL)
        .map_err(|_| DatumError::DegenerateInput)?;
    let cos = n1.dot(n2);
    let denom = 1.0 - cos * cos;
    let c1 = dot_dir(vec_of(a.origin()), n1);
    let c2 = dot_dir(vec_of(b.origin()), n2);
    let alpha = (c1 - cos * c2) / denom;
    let beta = (c2 - cos * c1) / denom;
    let origin = point_of(n1.into_vec(mm(alpha)) + n2.into_vec(mm(beta)));
    Ok(Axis3::new(origin, dir))
}

fn foot_on_axis(point: Point3, axis: Axis3) -> Point3 {
    let t = dot_dir(point - axis.origin(), axis.direction());
    axis.point_at(t)
}

fn project_point(point: Point3, onto: ReferenceGeometry) -> Result<Point3, DatumError> {
    match onto {
        ReferenceGeometry::Plane(plane) => {
            let distance = dot_dir(point - plane.origin(), plane.normal());
            Ok(point - plane.normal().into_vec(mm(distance)))
        }
        ReferenceGeometry::CoordinateSystem(csys) => {
            let plane = csys.xy_plane();
            let distance = dot_dir(point - plane.origin(), plane.normal());
            Ok(point - plane.normal().into_vec(mm(distance)))
        }
        ReferenceGeometry::Axis(axis) => Ok(foot_on_axis(point, axis)),
        ReferenceGeometry::Point(_) => Err(DatumError::WrongReferenceKind),
    }
}

fn intersection_point(a: ReferenceGeometry, b: ReferenceGeometry) -> Result<Point3, DatumError> {
    match (a, b) {
        (ReferenceGeometry::Axis(axis), other) | (other, ReferenceGeometry::Axis(axis)) => {
            match other {
                ReferenceGeometry::Plane(plane) => axis_plane_point(axis, plane),
                ReferenceGeometry::CoordinateSystem(csys) => {
                    axis_plane_point(axis, csys.xy_plane())
                }
                ReferenceGeometry::Axis(other) => axis_axis_point(axis, other),
                ReferenceGeometry::Point(_) => Err(DatumError::WrongReferenceKind),
            }
        }
        _ => Err(DatumError::WrongReferenceKind),
    }
}

fn axis_plane_point(axis: Axis3, plane: Plane3) -> Result<Point3, DatumError> {
    let denom = axis.direction().dot(plane.normal());
    if denom.abs() < DEGENERATE_DOT {
        return Err(DatumError::DegenerateInput);
    }
    let t = dot_dir(plane.origin() - axis.origin(), plane.normal()) / denom;
    Ok(axis.point_at(t))
}

fn axis_axis_point(a: Axis3, b: Axis3) -> Result<Point3, DatumError> {
    let da = a.direction();
    let db = b.direction();
    let r = b.origin() - a.origin();
    let dab = da.dot(db);
    let denom = 1.0 - dab * dab;
    if denom.abs() < DEGENERATE_DOT {
        return Err(DatumError::DegenerateInput);
    }
    let rda = dot_dir(r, da);
    let rdb = dot_dir(r, db);
    let ta = (rda - dab * rdb) / denom;
    let tb = (dab * rda - rdb) / denom;
    let pa = a.point_at(ta);
    let pb = b.point_at(tb);
    if (pb - pa).norm_mm() > COINCIDENCE_TOL_MM {
        return Err(DatumError::DegenerateInput);
    }
    Ok(point_of((vec_of(pa) + vec_of(pb)) * 0.5))
}

fn csys_frame(origin: Point3, dirs: AxisDirections) -> Result<CoordinateSystem3, DatumError> {
    let (x, y) = match (dirs.x, dirs.y, dirs.z) {
        (Some(px), Some(sy), _) => {
            let y = orthonormal_against(sy, px)?;
            (px, y)
        }
        (Some(px), None, Some(sz)) => {
            let z = orthonormal_against(sz, px)?;
            let y = z
                .cross(px, DATUM_TOL)
                .map_err(|_| DatumError::DegenerateInput)?;
            (px, y)
        }
        (None, Some(py), Some(sz)) => {
            let z = orthonormal_against(sz, py)?;
            let x = py
                .cross(z, DATUM_TOL)
                .map_err(|_| DatumError::DegenerateInput)?;
            (x, py)
        }
        (Some(px), None, None) => single_axis_frame(Slot::X, px)?,
        (None, Some(py), None) => single_axis_frame(Slot::Y, py)?,
        (None, None, Some(pz)) => single_axis_frame(Slot::Z, pz)?,
        (None, None, None) => (UnitVec3::x_axis(), UnitVec3::y_axis()),
    };
    CoordinateSystem3::new(origin, x, y, DATUM_TOL).map_err(|_| DatumError::DegenerateInput)
}

#[derive(Copy, Clone)]
enum Slot {
    X,
    Y,
    Z,
}

fn single_axis_frame(slot: Slot, dir: UnitVec3) -> Result<(UnitVec3, UnitVec3), DatumError> {
    let other = orthonormal_against(auto_hint(dir), dir)?;
    match slot {
        Slot::X => Ok((dir, other)),
        Slot::Y => {
            let z = dir
                .cross(other, DATUM_TOL)
                .map_err(|_| DatumError::DegenerateInput)?;
            Ok((z, dir))
        }
        Slot::Z => {
            let y = dir
                .cross(other, DATUM_TOL)
                .map_err(|_| DatumError::DegenerateInput)?;
            Ok((other, y))
        }
    }
}

fn orthonormal_against(vector: UnitVec3, axis: UnitVec3) -> Result<UnitVec3, DatumError> {
    let projection = axis.into_vec(mm(vector.dot(axis)));
    (vector.into_vec(mm(1.0)) - projection)
        .try_normalize(DATUM_TOL)
        .map_err(|_| DatumError::DegenerateInput)
}

fn apply_local_transform(
    base: CoordinateSystem3,
    position: Option<Vec3>,
    rotation: Option<CsysRotation>,
) -> CoordinateSystem3 {
    let rotated = match rotation {
        None => base,
        Some(turn) => {
            let about = |axis: UnitVec3, angle: Angle| bone_types::AxisAngle::new(axis, angle);
            let x1 = base.x_axis();
            let y1 = base.y_axis();
            let qx = about(x1, turn.x);
            let (x2, y2) = (x1.rotated(qx), y1.rotated(qx));
            let qy = about(y2, turn.y);
            let (x3, y3) = (x2.rotated(qy), y2.rotated(qy));
            let z3 = CoordinateSystem3::new_unchecked(base.origin(), x3, y3).z_axis();
            let qz = about(z3, turn.z);
            CoordinateSystem3::new_unchecked(base.origin(), x3.rotated(qz), y3.rotated(qz))
        }
    };
    match position {
        None => rotated,
        Some(offset) => {
            let (ox, oy, oz) = offset.coords_mm();
            let moved = rotated.point_at_local(ox, oy, oz);
            CoordinateSystem3::new_unchecked(moved, rotated.x_axis(), rotated.y_axis())
        }
    }
}

#[cfg(test)]
mod tests {
    use super::{
        AxisRecipe, CsysAxisRef, CsysRecipe, CsysRotation, DatumError, DatumFeature, DatumGap,
        DatumGeometry, PlaneRecipe, PointRecipe, ReferenceGeometry, dot_dir,
    };
    use bone_types::{
        Angle, Axis3, DatumTarget, EntityRef, FaceFingerprint, FaceLabel, FaceRef, FaceRole,
        FeatureId, Length, Plane3, Point3, Tolerance, UnitVec3, millimeter, radian,
    };
    use slotmap::{Key, KeyData};

    fn target(idx: u64) -> DatumTarget {
        DatumTarget::Feature(FeatureId::from(KeyData::from_ffi((1u64 << 32) | idx)))
    }

    fn entity_target() -> DatumTarget {
        DatumTarget::Entity(EntityRef::Face(FaceRef::new(
            FaceLabel {
                feature: FeatureId::null(),
                role: FaceRole::EndCap,
            },
            FaceFingerprint {
                plane: Plane3::new_unchecked(
                    Point3::origin(),
                    UnitVec3::x_axis(),
                    UnitVec3::y_axis(),
                ),
                centroid: Point3::origin(),
            },
        )))
    }

    fn xy_plane(z: f64) -> Plane3 {
        Plane3::new_unchecked(
            Point3::from_mm(0.0, 0.0, z),
            UnitVec3::x_axis(),
            UnitVec3::y_axis(),
        )
    }

    fn resolve_map(
        entries: Vec<(DatumTarget, ReferenceGeometry)>,
    ) -> impl FnMut(DatumTarget) -> Result<ReferenceGeometry, DatumError> {
        move |target| {
            entries
                .iter()
                .find(|(key, _)| *key == target)
                .map(|(_, geom)| *geom)
                .ok_or_else(|| DatumError::Unresolved(Box::new(target)))
        }
    }

    #[test]
    fn offset_plane_shifts_along_the_normal() {
        let from = target(10);
        let feature = DatumFeature::plane(PlaneRecipe::Offset {
            from,
            distance: Length::new::<millimeter>(25.0),
            flip: false,
        });
        let mut resolve = resolve_map(vec![(from, ReferenceGeometry::Plane(xy_plane(0.0)))]);
        let Ok(DatumGeometry::Plane(plane)) = feature.evaluate(&mut resolve) else {
            panic!("offset plane builds");
        };
        assert!((plane.origin().z().get::<millimeter>() - 25.0).abs() < 1e-9);
        assert!((plane.normal().components().2 - 1.0).abs() < 1e-9);
    }

    #[test]
    fn offset_flip_reverses_the_shift() {
        let from = target(10);
        let feature = DatumFeature::plane(PlaneRecipe::Offset {
            from,
            distance: Length::new::<millimeter>(25.0),
            flip: true,
        });
        let mut resolve = resolve_map(vec![(from, ReferenceGeometry::Plane(xy_plane(0.0)))]);
        let Ok(DatumGeometry::Plane(plane)) = feature.evaluate(&mut resolve) else {
            panic!("offset plane builds");
        };
        assert!((plane.origin().z().get::<millimeter>() + 25.0).abs() < 1e-9);
    }

    #[test]
    fn plane_through_three_points_spans_them() {
        let (a, b, c) = (target(1), target(2), target(3));
        let feature = DatumFeature::plane(PlaneRecipe::ThreePoints { a, b, c });
        let mut resolve = resolve_map(vec![
            (a, ReferenceGeometry::Point(Point3::from_mm(0.0, 0.0, 5.0))),
            (b, ReferenceGeometry::Point(Point3::from_mm(10.0, 0.0, 5.0))),
            (c, ReferenceGeometry::Point(Point3::from_mm(0.0, 10.0, 5.0))),
        ]);
        let Ok(DatumGeometry::Plane(plane)) = feature.evaluate(&mut resolve) else {
            panic!("three-point plane builds");
        };
        assert!((plane.normal().components().2.abs() - 1.0).abs() < 1e-9);
        assert!((plane.origin().z().get::<millimeter>() - 5.0).abs() < 1e-9);
    }

    #[test]
    fn collinear_points_are_degenerate() {
        let (a, b, c) = (target(1), target(2), target(3));
        let feature = DatumFeature::plane(PlaneRecipe::ThreePoints { a, b, c });
        let mut resolve = resolve_map(vec![
            (a, ReferenceGeometry::Point(Point3::from_mm(0.0, 0.0, 0.0))),
            (b, ReferenceGeometry::Point(Point3::from_mm(1.0, 0.0, 0.0))),
            (c, ReferenceGeometry::Point(Point3::from_mm(2.0, 0.0, 0.0))),
        ]);
        assert_eq!(
            feature.evaluate(&mut resolve),
            Err(DatumError::DegenerateInput)
        );
    }

    #[test]
    fn midplane_between_two_parallel_planes() {
        let (a, b) = (target(1), target(2));
        let feature = DatumFeature::plane(PlaneRecipe::MidPlane {
            first: a,
            second: b,
        });
        let mut resolve = resolve_map(vec![
            (a, ReferenceGeometry::Plane(xy_plane(0.0))),
            (b, ReferenceGeometry::Plane(xy_plane(10.0))),
        ]);
        let Ok(DatumGeometry::Plane(plane)) = feature.evaluate(&mut resolve) else {
            panic!("midplane builds");
        };
        assert!((plane.origin().z().get::<millimeter>() - 5.0).abs() < 1e-9);
    }

    #[test]
    fn non_parallel_midplane_reports_a_typed_gap() {
        let (first, second) = (target(1), target(2));
        let feature = DatumFeature::plane(PlaneRecipe::MidPlane { first, second });
        let tilted =
            Plane3::new_unchecked(Point3::origin(), UnitVec3::y_axis(), UnitVec3::z_axis());
        let mut resolve = resolve_map(vec![
            (first, ReferenceGeometry::Plane(xy_plane(0.0))),
            (second, ReferenceGeometry::Plane(tilted)),
        ]);
        assert_eq!(
            feature.evaluate(&mut resolve),
            Err(DatumError::Unsupported {
                detail: DatumGap::NonParallelMidPlane,
            }),
        );
    }

    #[test]
    fn near_parallel_midplane_succeeds_within_tolerance() {
        let (first, second) = (target(1), target(2));
        let feature = DatumFeature::plane(PlaneRecipe::MidPlane { first, second });
        let angle: f64 = 1.0e-4;
        let tilted_x = UnitVec3::new_unchecked(angle.cos(), 0.0, -angle.sin());
        let tilted = Plane3::new_unchecked(
            Point3::from_mm(0.0, 0.0, 10.0),
            tilted_x,
            UnitVec3::y_axis(),
        );
        let mut resolve = resolve_map(vec![
            (first, ReferenceGeometry::Plane(xy_plane(0.0))),
            (second, ReferenceGeometry::Plane(tilted)),
        ]);
        let Ok(DatumGeometry::Plane(plane)) = feature.evaluate(&mut resolve) else {
            panic!("a near-parallel midplane builds within the loosened tolerance");
        };
        assert!((plane.origin().z().get::<millimeter>() - 5.0).abs() < 1.0e-6);
    }

    #[test]
    fn plane_at_angle_accepts_an_in_plane_edge_with_float_noise() {
        let (plane, axis) = (target(1), target(2));
        let feature = DatumFeature::plane(PlaneRecipe::AtAngle {
            plane,
            axis,
            angle: Angle::new::<radian>(core::f64::consts::FRAC_PI_2),
        });
        let edge = Axis3::new(Point3::origin(), UnitVec3::new_unchecked(1.0, 0.0, 1.0e-8));
        let mut resolve = resolve_map(vec![
            (plane, ReferenceGeometry::Plane(xy_plane(0.0))),
            (axis, ReferenceGeometry::Axis(edge)),
        ]);
        assert!(matches!(
            feature.evaluate(&mut resolve),
            Ok(DatumGeometry::Plane(_)),
        ));
    }

    #[test]
    fn axis_from_two_planes_runs_along_their_intersection() {
        let (a, b) = (target(1), target(2));
        let feature = DatumFeature::axis(AxisRecipe::TwoPlanes { a, b });
        let zx = Plane3::new_unchecked(Point3::origin(), UnitVec3::z_axis(), UnitVec3::x_axis());
        let yz = Plane3::new_unchecked(Point3::origin(), UnitVec3::y_axis(), UnitVec3::z_axis());
        let mut resolve = resolve_map(vec![
            (a, ReferenceGeometry::Plane(zx)),
            (b, ReferenceGeometry::Plane(yz)),
        ]);
        let Ok(DatumGeometry::Axis(axis)) = feature.evaluate(&mut resolve) else {
            panic!("two-plane axis builds");
        };
        assert!((axis.direction().components().2.abs() - 1.0).abs() < 1e-9);
    }

    #[test]
    fn axis_from_two_planes_lies_on_both_when_offset() {
        let (a, b) = (target(1), target(2));
        let feature = DatumFeature::axis(AxisRecipe::TwoPlanes { a, b });
        let plane_a = Plane3::new_unchecked(
            Point3::from_mm(2.0, 0.0, 0.0),
            UnitVec3::y_axis(),
            UnitVec3::z_axis(),
        );
        let diagonal = UnitVec3::new_unchecked(0.5_f64.sqrt(), -(0.5_f64.sqrt()), 0.0);
        let plane_b = Plane3::new_unchecked(Point3::origin(), UnitVec3::z_axis(), diagonal);
        let mut resolve = resolve_map(vec![
            (a, ReferenceGeometry::Plane(plane_a)),
            (b, ReferenceGeometry::Plane(plane_b)),
        ]);
        let Ok(DatumGeometry::Axis(axis)) = feature.evaluate(&mut resolve) else {
            panic!("two-plane axis builds");
        };
        assert!((axis.direction().components().2.abs() - 1.0).abs() < 1e-9);
        let on_a = dot_dir(axis.origin() - plane_a.origin(), plane_a.normal());
        let on_b = dot_dir(axis.origin() - plane_b.origin(), plane_b.normal());
        assert!(on_a.abs() < 1e-9, "axis origin must lie on plane a: {on_a}");
        assert!(on_b.abs() < 1e-9, "axis origin must lie on plane b: {on_b}");
    }

    #[test]
    fn point_projects_onto_a_plane() {
        let (p, onto) = (target(1), target(2));
        let feature = DatumFeature::point(PointRecipe::Projection { point: p, onto });
        let mut resolve = resolve_map(vec![
            (p, ReferenceGeometry::Point(Point3::from_mm(3.0, 4.0, 9.0))),
            (onto, ReferenceGeometry::Plane(xy_plane(0.0))),
        ]);
        let Ok(DatumGeometry::Point(point)) = feature.evaluate(&mut resolve) else {
            panic!("projection builds");
        };
        let (x, y, z) = point.coords_mm();
        assert!((x - 3.0).abs() < 1e-9 && (y - 4.0).abs() < 1e-9 && z.abs() < 1e-9);
    }

    #[test]
    fn coordinate_system_builds_a_right_handed_frame() {
        let (origin, x, y) = (target(1), target(2), target(3));
        let feature = DatumFeature::coordinate_system(CsysRecipe {
            origin,
            x_axis: Some(CsysAxisRef {
                target: x,
                reverse: false,
            }),
            y_axis: Some(CsysAxisRef {
                target: y,
                reverse: false,
            }),
            z_axis: None,
            position: None,
            rotation: None,
        });
        let mut resolve = resolve_map(vec![
            (
                origin,
                ReferenceGeometry::Point(Point3::from_mm(1.0, 2.0, 3.0)),
            ),
            (
                x,
                ReferenceGeometry::Axis(Axis3::new(Point3::origin(), UnitVec3::x_axis())),
            ),
            (
                y,
                ReferenceGeometry::Axis(Axis3::new(Point3::origin(), UnitVec3::y_axis())),
            ),
        ]);
        let Ok(DatumGeometry::CoordinateSystem(csys)) = feature.evaluate(&mut resolve) else {
            panic!("coordinate system builds");
        };
        assert!((csys.z_axis().components().2 - 1.0).abs() < 1e-9);
        let (ox, oy, oz) = csys.origin().coords_mm();
        assert!((ox - 1.0).abs() < 1e-9 && (oy - 2.0).abs() < 1e-9 && (oz - 3.0).abs() < 1e-9);
    }

    #[test]
    fn plane_at_angle_tilts_about_the_edge() {
        let (plane, axis) = (target(1), target(2));
        let feature = DatumFeature::plane(PlaneRecipe::AtAngle {
            plane,
            axis,
            angle: Angle::new::<radian>(core::f64::consts::FRAC_PI_2),
        });
        let edge = Axis3::new(Point3::origin(), UnitVec3::x_axis());
        let mut resolve = resolve_map(vec![
            (plane, ReferenceGeometry::Plane(xy_plane(0.0))),
            (axis, ReferenceGeometry::Axis(edge)),
        ]);
        let Ok(DatumGeometry::Plane(result)) = feature.evaluate(&mut resolve) else {
            panic!("at-angle plane builds");
        };
        assert!(result.normal().dot(edge.direction()).abs() < 1e-9);
        assert!((result.normal().components().1.abs() - 1.0).abs() < 1e-9);
    }

    #[test]
    fn plane_at_angle_rejects_edge_off_the_plane() {
        let (plane, axis) = (target(1), target(2));
        let feature = DatumFeature::plane(PlaneRecipe::AtAngle {
            plane,
            axis,
            angle: Angle::new::<radian>(0.5),
        });
        let tilted = Axis3::new(Point3::origin(), UnitVec3::z_axis());
        let mut resolve = resolve_map(vec![
            (plane, ReferenceGeometry::Plane(xy_plane(0.0))),
            (axis, ReferenceGeometry::Axis(tilted)),
        ]);
        assert_eq!(
            feature.evaluate(&mut resolve),
            Err(DatumError::DegenerateInput)
        );
    }

    #[test]
    fn normal_to_curve_sets_origin_on_the_axis() {
        let (curve, point) = (target(1), target(2));
        let feature = DatumFeature::plane(PlaneRecipe::NormalToCurve {
            curve,
            point,
            set_origin_on_curve: true,
        });
        let axis = Axis3::new(Point3::origin(), UnitVec3::z_axis());
        let mut resolve = resolve_map(vec![
            (curve, ReferenceGeometry::Axis(axis)),
            (
                point,
                ReferenceGeometry::Point(Point3::from_mm(3.0, 4.0, 7.0)),
            ),
        ]);
        let Ok(DatumGeometry::Plane(plane)) = feature.evaluate(&mut resolve) else {
            panic!("normal-to-curve plane builds");
        };
        let (x, y, z) = plane.origin().coords_mm();
        assert!(x.abs() < 1e-9 && y.abs() < 1e-9 && (z - 7.0).abs() < 1e-9);
        assert!((plane.normal().components().2.abs() - 1.0).abs() < 1e-9);
    }

    #[test]
    fn axis_point_and_face_runs_along_the_normal() {
        let (point, face) = (target(1), target(2));
        let feature = DatumFeature::axis(AxisRecipe::PointAndFace { point, face });
        let mut resolve = resolve_map(vec![
            (
                point,
                ReferenceGeometry::Point(Point3::from_mm(1.0, 2.0, 3.0)),
            ),
            (face, ReferenceGeometry::Plane(xy_plane(0.0))),
        ]);
        let Ok(DatumGeometry::Axis(axis)) = feature.evaluate(&mut resolve) else {
            panic!("point-and-face axis builds");
        };
        assert!((axis.direction().components().2.abs() - 1.0).abs() < 1e-9);
        let (x, y, z) = axis.origin().coords_mm();
        assert!((x - 1.0).abs() < 1e-9 && (y - 2.0).abs() < 1e-9 && (z - 3.0).abs() < 1e-9);
    }

    #[test]
    fn axis_through_two_points_spans_them() {
        let (a, b) = (target(1), target(2));
        let feature = DatumFeature::axis(AxisRecipe::TwoPoints { a, b });
        let mut resolve = resolve_map(vec![
            (a, ReferenceGeometry::Point(Point3::origin())),
            (b, ReferenceGeometry::Point(Point3::from_mm(0.0, 0.0, 5.0))),
        ]);
        let Ok(DatumGeometry::Axis(axis)) = feature.evaluate(&mut resolve) else {
            panic!("two-point axis builds");
        };
        assert!((axis.direction().components().2 - 1.0).abs() < 1e-9);
    }

    #[test]
    fn point_at_axis_plane_intersection() {
        let (axis_ref, plane_ref) = (target(1), target(2));
        let feature = DatumFeature::point(PointRecipe::Intersection {
            a: axis_ref,
            b: plane_ref,
        });
        let axis = Axis3::new(Point3::from_mm(1.0, 2.0, -3.0), UnitVec3::z_axis());
        let mut resolve = resolve_map(vec![
            (axis_ref, ReferenceGeometry::Axis(axis)),
            (plane_ref, ReferenceGeometry::Plane(xy_plane(0.0))),
        ]);
        let Ok(DatumGeometry::Point(point)) = feature.evaluate(&mut resolve) else {
            panic!("intersection point builds");
        };
        let (x, y, z) = point.coords_mm();
        assert!((x - 1.0).abs() < 1e-9 && (y - 2.0).abs() < 1e-9 && z.abs() < 1e-9);
    }

    #[test]
    fn coordinate_system_applies_numeric_rotation() {
        let origin = target(1);
        let feature = DatumFeature::coordinate_system(CsysRecipe {
            origin,
            x_axis: None,
            y_axis: None,
            z_axis: None,
            position: None,
            rotation: Some(CsysRotation {
                x: Angle::new::<radian>(0.0),
                y: Angle::new::<radian>(0.0),
                z: Angle::new::<radian>(core::f64::consts::FRAC_PI_2),
            }),
        });
        let mut resolve = resolve_map(vec![(origin, ReferenceGeometry::Point(Point3::origin()))]);
        let Ok(DatumGeometry::CoordinateSystem(csys)) = feature.evaluate(&mut resolve) else {
            panic!("coordinate system builds");
        };
        let (x, y, _z) = csys.x_axis().components();
        assert!(x.abs() < 1e-9 && (y - 1.0).abs() < 1e-9);
    }

    #[test]
    fn tangent_plane_reports_a_typed_gap() {
        let (surface, reference) = (target(1), target(2));
        let feature = DatumFeature::plane(PlaneRecipe::TangentToSurface {
            surface,
            reference,
            angle: Angle::new::<radian>(0.0),
        });
        let mut resolve = resolve_map(vec![
            (surface, ReferenceGeometry::Plane(xy_plane(0.0))),
            (reference, ReferenceGeometry::Plane(xy_plane(0.0))),
        ]);
        assert_eq!(
            feature.evaluate(&mut resolve),
            Err(DatumError::Unsupported {
                detail: DatumGap::TangentSurface,
            }),
        );
    }

    #[test]
    fn targets_lists_every_reference() {
        let feature = DatumFeature::plane(PlaneRecipe::AtAngle {
            plane: target(1),
            axis: entity_target(),
            angle: Angle::new::<radian>(0.5),
        });
        assert_eq!(feature.targets().len(), 2);
    }

    #[test]
    fn datum_feature_ron_round_trip() {
        let feature = DatumFeature::plane(PlaneRecipe::Offset {
            from: target(1),
            distance: Length::new::<millimeter>(12.0),
            flip: true,
        });
        let Ok(text) = ron::to_string(&feature) else {
            panic!("serialize datum feature");
        };
        let Ok(back) = ron::from_str::<DatumFeature>(&text) else {
            panic!("deserialize datum feature");
        };
        assert_eq!(feature, back);
        let _ = Tolerance::new(1e-9);
    }
}
