use nalgebra::{Point2 as NPoint2, Point3 as NPoint3, Unit, Vector2 as NVec2, Vector3 as NVec3};
use serde::{Deserialize, Serialize};
use uom::si::angle::radian;
use uom::si::f64::{Angle, Length};
use uom::si::length::millimeter;

use crate::{Result, Tolerance, TypesError};

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

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

#[must_use]
fn without_neg_zero(value: f64) -> f64 {
    if value == 0.0 { 0.0 } else { value }
}

#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
#[serde(from = "Point2Wire", into = "Point2Wire")]
pub struct Point2(NPoint2<f64>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Point2", deny_unknown_fields)]
struct Point2Wire {
    x: f64,
    y: f64,
}

impl From<Point2> for Point2Wire {
    fn from(p: Point2) -> Self {
        Self { x: p.0.x, y: p.0.y }
    }
}

impl From<Point2Wire> for Point2 {
    fn from(w: Point2Wire) -> Self {
        Self(NPoint2::new(w.x, w.y))
    }
}

impl Point2 {
    #[must_use]
    pub fn origin() -> Self {
        Self(NPoint2::origin())
    }

    #[must_use]
    pub fn from_mm(x: f64, y: f64) -> Self {
        Self(NPoint2::new(x, y))
    }

    #[must_use]
    pub fn from_lengths(x: Length, y: Length) -> Self {
        Self(NPoint2::new(x.get::<millimeter>(), y.get::<millimeter>()))
    }

    #[must_use]
    pub fn x(self) -> Length {
        mm(self.0.x)
    }

    #[must_use]
    pub fn y(self) -> Length {
        mm(self.0.y)
    }

    #[must_use]
    pub fn coords_mm(self) -> (f64, f64) {
        (self.0.x, self.0.y)
    }
}

impl core::fmt::Debug for Point2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "Point2({} mm, {} mm)", self.0.x, self.0.y)
    }
}

impl core::fmt::Display for Point2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "({} mm, {} mm)", self.0.x, self.0.y)
    }
}

impl core::ops::Sub for Point2 {
    type Output = Vec2;
    fn sub(self, rhs: Self) -> Vec2 {
        Vec2(self.0 - rhs.0)
    }
}

impl core::ops::Add<Vec2> for Point2 {
    type Output = Self;
    fn add(self, rhs: Vec2) -> Self {
        Self(self.0 + rhs.0)
    }
}

impl core::ops::Sub<Vec2> for Point2 {
    type Output = Self;
    fn sub(self, rhs: Vec2) -> Self {
        Self(self.0 - rhs.0)
    }
}

#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
#[serde(from = "Vec2Wire", into = "Vec2Wire")]
pub struct Vec2(NVec2<f64>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Vec2", deny_unknown_fields)]
struct Vec2Wire {
    x: f64,
    y: f64,
}

impl From<Vec2> for Vec2Wire {
    fn from(v: Vec2) -> Self {
        Self { x: v.0.x, y: v.0.y }
    }
}

impl From<Vec2Wire> for Vec2 {
    fn from(w: Vec2Wire) -> Self {
        Self(NVec2::new(w.x, w.y))
    }
}

impl Vec2 {
    #[must_use]
    pub fn zero() -> Self {
        Self(NVec2::zeros())
    }

    #[must_use]
    pub fn from_mm(x: f64, y: f64) -> Self {
        Self(NVec2::new(x, y))
    }

    #[must_use]
    pub fn from_lengths(x: Length, y: Length) -> Self {
        Self(NVec2::new(x.get::<millimeter>(), y.get::<millimeter>()))
    }

    #[must_use]
    pub fn x(self) -> Length {
        mm(self.0.x)
    }

    #[must_use]
    pub fn y(self) -> Length {
        mm(self.0.y)
    }

    #[must_use]
    pub fn coords_mm(self) -> (f64, f64) {
        (self.0.x, self.0.y)
    }

    #[must_use]
    pub fn dot_mm2(self, other: Self) -> f64 {
        self.0.dot(&other.0)
    }

    #[must_use]
    pub fn cross_z_mm2(self, other: Self) -> f64 {
        self.0.x * other.0.y - self.0.y * other.0.x
    }

    #[must_use]
    pub fn norm_squared_mm2(self) -> f64 {
        self.0.norm_squared()
    }

    #[must_use]
    pub fn norm_mm(self) -> f64 {
        self.0.norm()
    }

    #[must_use]
    pub fn norm(self) -> Length {
        mm(self.0.norm())
    }

    #[must_use]
    pub fn perp_ccw(self) -> Self {
        Self(NVec2::new(-self.0.y, self.0.x))
    }

    pub fn try_normalize(self, tolerance: Tolerance) -> Result<UnitVec2> {
        Unit::try_new(self.0, tolerance.value())
            .map(UnitVec2)
            .ok_or(TypesError::ZeroLengthAxis)
    }
}

impl core::fmt::Debug for Vec2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "Vec2({} mm, {} mm)", self.0.x, self.0.y)
    }
}

impl core::fmt::Display for Vec2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "<{} mm, {} mm>", self.0.x, self.0.y)
    }
}

impl core::ops::Add for Vec2 {
    type Output = Self;
    fn add(self, rhs: Self) -> Self {
        Self(self.0 + rhs.0)
    }
}

impl core::ops::Sub for Vec2 {
    type Output = Self;
    fn sub(self, rhs: Self) -> Self {
        Self(self.0 - rhs.0)
    }
}

impl core::ops::Mul<f64> for Vec2 {
    type Output = Self;
    fn mul(self, rhs: f64) -> Self {
        Self(self.0 * rhs)
    }
}

impl core::ops::Mul<Vec2> for f64 {
    type Output = Vec2;
    fn mul(self, rhs: Vec2) -> Vec2 {
        Vec2(rhs.0 * self)
    }
}

impl core::ops::Neg for Vec2 {
    type Output = Self;
    fn neg(self) -> Self {
        Self(-self.0)
    }
}

const WIRE_UNIT_TOLERANCE: f64 = 1.0e-9;

fn wire_unit_norm(norm: f64) -> Result<()> {
    if !norm.is_finite() || (norm - 1.0).abs() > WIRE_UNIT_TOLERANCE {
        return Err(TypesError::NonUnitAxis(norm));
    }
    Ok(())
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(try_from = "UnitVec2Wire", into = "UnitVec2Wire")]
pub struct UnitVec2(Unit<NVec2<f64>>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "UnitVec2", deny_unknown_fields)]
struct UnitVec2Wire {
    x: f64,
    y: f64,
}

impl From<UnitVec2> for UnitVec2Wire {
    fn from(u: UnitVec2) -> Self {
        Self { x: u.0.x, y: u.0.y }
    }
}

impl TryFrom<UnitVec2Wire> for UnitVec2 {
    type Error = TypesError;
    fn try_from(w: UnitVec2Wire) -> Result<Self> {
        wire_unit_norm((w.x * w.x + w.y * w.y).sqrt())?;
        Ok(Self(Unit::new_unchecked(NVec2::new(w.x, w.y))))
    }
}

impl UnitVec2 {
    #[must_use]
    pub fn x_axis() -> Self {
        Self(Unit::new_unchecked(NVec2::new(1.0, 0.0)))
    }

    #[must_use]
    pub fn y_axis() -> Self {
        Self(Unit::new_unchecked(NVec2::new(0.0, 1.0)))
    }

    #[must_use]
    pub fn new_unchecked(x: f64, y: f64) -> Self {
        Self(Unit::new_unchecked(NVec2::new(x, y)))
    }

    pub fn try_from_components(x: f64, y: f64, tolerance: Tolerance) -> Result<Self> {
        Unit::try_new(NVec2::new(x, y), tolerance.value())
            .map(Self)
            .ok_or(TypesError::ZeroLengthAxis)
    }

    #[must_use]
    pub fn components(self) -> (f64, f64) {
        (self.0.x, self.0.y)
    }

    #[must_use]
    pub fn dot(self, other: Self) -> f64 {
        self.0.dot(&other.0)
    }

    #[must_use]
    pub fn perp_ccw(self) -> Self {
        Self(Unit::new_unchecked(NVec2::new(-self.0.y, self.0.x)))
    }

    #[must_use]
    pub fn into_vec(self, length: Length) -> Vec2 {
        let mm_value = length.get::<millimeter>();
        Vec2(self.0.into_inner() * mm_value)
    }
}

impl core::fmt::Display for UnitVec2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "[{}, {}]",
            without_neg_zero(self.0.x),
            without_neg_zero(self.0.y)
        )
    }
}

#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
#[serde(from = "Point3Wire", into = "Point3Wire")]
pub struct Point3(NPoint3<f64>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Point3", deny_unknown_fields)]
struct Point3Wire {
    x: f64,
    y: f64,
    z: f64,
}

impl From<Point3> for Point3Wire {
    fn from(p: Point3) -> Self {
        Self {
            x: p.0.x,
            y: p.0.y,
            z: p.0.z,
        }
    }
}

impl From<Point3Wire> for Point3 {
    fn from(w: Point3Wire) -> Self {
        Self(NPoint3::new(w.x, w.y, w.z))
    }
}

impl Point3 {
    #[must_use]
    pub fn origin() -> Self {
        Self(NPoint3::origin())
    }

    #[must_use]
    pub fn from_mm(x: f64, y: f64, z: f64) -> Self {
        Self(NPoint3::new(x, y, z))
    }

    #[must_use]
    pub fn from_lengths(x: Length, y: Length, z: Length) -> Self {
        Self(NPoint3::new(
            x.get::<millimeter>(),
            y.get::<millimeter>(),
            z.get::<millimeter>(),
        ))
    }

    #[must_use]
    pub fn x(self) -> Length {
        Length::new::<millimeter>(self.0.x)
    }

    #[must_use]
    pub fn y(self) -> Length {
        Length::new::<millimeter>(self.0.y)
    }

    #[must_use]
    pub fn z(self) -> Length {
        Length::new::<millimeter>(self.0.z)
    }

    #[must_use]
    pub fn coords_mm(self) -> (f64, f64, f64) {
        (self.0.x, self.0.y, self.0.z)
    }
}

impl core::ops::Sub for Point3 {
    type Output = Vec3;
    fn sub(self, rhs: Self) -> Vec3 {
        Vec3(self.0 - rhs.0)
    }
}

impl core::ops::Add<Vec3> for Point3 {
    type Output = Self;
    fn add(self, rhs: Vec3) -> Self {
        Self(self.0 + rhs.0)
    }
}

impl core::ops::Sub<Vec3> for Point3 {
    type Output = Self;
    fn sub(self, rhs: Vec3) -> Self {
        Self(self.0 - rhs.0)
    }
}

impl core::fmt::Debug for Point3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "Point3({} mm, {} mm, {} mm)",
            self.0.x, self.0.y, self.0.z,
        )
    }
}

impl core::fmt::Display for Point3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "({} mm, {} mm, {} mm)", self.0.x, self.0.y, self.0.z)
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(try_from = "UnitVec3Wire", into = "UnitVec3Wire")]
pub struct UnitVec3(Unit<NVec3<f64>>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "UnitVec3", deny_unknown_fields)]
struct UnitVec3Wire {
    x: f64,
    y: f64,
    z: f64,
}

impl From<UnitVec3> for UnitVec3Wire {
    fn from(u: UnitVec3) -> Self {
        Self {
            x: u.0.x,
            y: u.0.y,
            z: u.0.z,
        }
    }
}

impl TryFrom<UnitVec3Wire> for UnitVec3 {
    type Error = TypesError;
    fn try_from(w: UnitVec3Wire) -> Result<Self> {
        wire_unit_norm((w.x * w.x + w.y * w.y + w.z * w.z).sqrt())?;
        Ok(Self(Unit::new_unchecked(NVec3::new(w.x, w.y, w.z))))
    }
}

impl UnitVec3 {
    #[must_use]
    pub fn x_axis() -> Self {
        Self(Unit::new_unchecked(NVec3::new(1.0, 0.0, 0.0)))
    }

    #[must_use]
    pub fn y_axis() -> Self {
        Self(Unit::new_unchecked(NVec3::new(0.0, 1.0, 0.0)))
    }

    #[must_use]
    pub fn z_axis() -> Self {
        Self(Unit::new_unchecked(NVec3::new(0.0, 0.0, 1.0)))
    }

    #[must_use]
    pub fn new_unchecked(x: f64, y: f64, z: f64) -> Self {
        Self(Unit::new_unchecked(NVec3::new(x, y, z)))
    }

    pub fn try_from_components(x: f64, y: f64, z: f64, tolerance: Tolerance) -> Result<Self> {
        Unit::try_new(NVec3::new(x, y, z), tolerance.value())
            .map(Self)
            .ok_or(TypesError::ZeroLengthAxis)
    }

    #[must_use]
    pub fn components(self) -> (f64, f64, f64) {
        (self.0.x, self.0.y, self.0.z)
    }

    #[must_use]
    pub fn dot(self, other: Self) -> f64 {
        self.0.dot(&other.0)
    }

    #[must_use]
    pub fn reversed(self) -> Self {
        Self(Unit::new_unchecked(-self.0.into_inner()))
    }

    #[must_use]
    pub fn into_vec(self, length: Length) -> Vec3 {
        Vec3(self.0.into_inner() * length.get::<millimeter>())
    }
}

impl core::fmt::Display for UnitVec3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "[{}, {}, {}]",
            without_neg_zero(self.0.x),
            without_neg_zero(self.0.y),
            without_neg_zero(self.0.z)
        )
    }
}

fn orthonormalize(x: UnitVec3, y: UnitVec3, tolerance: Tolerance) -> Result<(UnitVec3, UnitVec3)> {
    let dot = x.dot(y);
    if dot.abs() > tolerance.value() {
        return Err(TypesError::NonOrthogonalPlaneAxes(dot.abs()));
    }
    let y_projected = y.0.into_inner() - x.0.into_inner() * dot;
    let y_orthonormal =
        UnitVec3(Unit::try_new(y_projected, f64::EPSILON).ok_or(TypesError::ZeroLengthAxis)?);
    Ok((x, y_orthonormal))
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(try_from = "SketchPlaneBasisWire", into = "SketchPlaneBasisWire")]
pub struct SketchPlaneBasis {
    origin: Point3,
    x: UnitVec3,
    y: UnitVec3,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "SketchPlaneBasis", deny_unknown_fields)]
struct SketchPlaneBasisWire {
    origin: Point3,
    x_axis: UnitVec3,
    y_axis: UnitVec3,
}

impl From<SketchPlaneBasis> for SketchPlaneBasisWire {
    fn from(b: SketchPlaneBasis) -> Self {
        Self {
            origin: b.origin,
            x_axis: b.x,
            y_axis: b.y,
        }
    }
}

impl TryFrom<SketchPlaneBasisWire> for SketchPlaneBasis {
    type Error = TypesError;
    fn try_from(w: SketchPlaneBasisWire) -> Result<Self> {
        Self::new(w.origin, w.x_axis, w.y_axis, PLANE_ORTHOGONALITY_TOLERANCE)
    }
}

impl SketchPlaneBasis {
    pub fn new(origin: Point3, x: UnitVec3, y: UnitVec3, tolerance: Tolerance) -> Result<Self> {
        let (x, y) = orthonormalize(x, y, tolerance)?;
        Ok(Self { origin, x, y })
    }

    #[must_use]
    pub fn origin(self) -> Point3 {
        self.origin
    }

    #[must_use]
    pub fn x_axis(self) -> UnitVec3 {
        self.x
    }

    #[must_use]
    pub fn y_axis(self) -> UnitVec3 {
        self.y
    }

    #[must_use]
    pub fn normal(self) -> UnitVec3 {
        UnitVec3(Unit::new_normalize(self.x.0.cross(&self.y.0)))
    }
}

impl From<SketchPlaneBasis> for Plane3 {
    fn from(basis: SketchPlaneBasis) -> Self {
        Self::new_unchecked(basis.origin(), basis.x_axis(), basis.y_axis())
    }
}

impl core::fmt::Display for SketchPlaneBasis {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "basis{{ o={}, x={}, y={} }}",
            self.origin, self.x, self.y,
        )
    }
}

#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
#[serde(from = "Vec3Wire", into = "Vec3Wire")]
pub struct Vec3(NVec3<f64>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Vec3", deny_unknown_fields)]
struct Vec3Wire {
    x: f64,
    y: f64,
    z: f64,
}

impl From<Vec3> for Vec3Wire {
    fn from(v: Vec3) -> Self {
        Self {
            x: v.0.x,
            y: v.0.y,
            z: v.0.z,
        }
    }
}

impl From<Vec3Wire> for Vec3 {
    fn from(w: Vec3Wire) -> Self {
        Self(NVec3::new(w.x, w.y, w.z))
    }
}

impl Vec3 {
    #[must_use]
    pub fn zero() -> Self {
        Self(NVec3::zeros())
    }

    #[must_use]
    pub fn from_mm(x: f64, y: f64, z: f64) -> Self {
        Self(NVec3::new(x, y, z))
    }

    #[must_use]
    pub fn from_lengths(x: Length, y: Length, z: Length) -> Self {
        Self(NVec3::new(
            x.get::<millimeter>(),
            y.get::<millimeter>(),
            z.get::<millimeter>(),
        ))
    }

    #[must_use]
    pub fn x(self) -> Length {
        mm(self.0.x)
    }

    #[must_use]
    pub fn y(self) -> Length {
        mm(self.0.y)
    }

    #[must_use]
    pub fn z(self) -> Length {
        mm(self.0.z)
    }

    #[must_use]
    pub fn coords_mm(self) -> (f64, f64, f64) {
        (self.0.x, self.0.y, self.0.z)
    }

    #[must_use]
    pub fn dot_mm2(self, other: Self) -> f64 {
        self.0.dot(&other.0)
    }

    #[must_use]
    pub fn cross(self, other: Self) -> Self {
        Self(self.0.cross(&other.0))
    }

    #[must_use]
    pub fn norm_squared_mm2(self) -> f64 {
        self.0.norm_squared()
    }

    #[must_use]
    pub fn norm_mm(self) -> f64 {
        self.0.norm()
    }

    #[must_use]
    pub fn norm(self) -> Length {
        mm(self.0.norm())
    }

    pub fn try_normalize(self, tolerance: Tolerance) -> Result<UnitVec3> {
        Unit::try_new(self.0, tolerance.value())
            .map(UnitVec3)
            .ok_or(TypesError::ZeroLengthAxis)
    }
}

impl core::fmt::Debug for Vec3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "Vec3({} mm, {} mm, {} mm)", self.0.x, self.0.y, self.0.z)
    }
}

impl core::fmt::Display for Vec3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "<{} mm, {} mm, {} mm>", self.0.x, self.0.y, self.0.z)
    }
}

impl core::ops::Add for Vec3 {
    type Output = Self;
    fn add(self, rhs: Self) -> Self {
        Self(self.0 + rhs.0)
    }
}

impl core::ops::Sub for Vec3 {
    type Output = Self;
    fn sub(self, rhs: Self) -> Self {
        Self(self.0 - rhs.0)
    }
}

impl core::ops::Mul<f64> for Vec3 {
    type Output = Self;
    fn mul(self, rhs: f64) -> Self {
        Self(self.0 * rhs)
    }
}

impl core::ops::Mul<Vec3> for f64 {
    type Output = Vec3;
    fn mul(self, rhs: Vec3) -> Vec3 {
        Vec3(rhs.0 * self)
    }
}

impl core::ops::Neg for Vec3 {
    type Output = Self;
    fn neg(self) -> Self {
        Self(-self.0)
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(from = "Aabb3Wire", into = "Aabb3Wire")]
pub struct Aabb3 {
    min: Point3,
    max: Point3,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Aabb3", deny_unknown_fields)]
struct Aabb3Wire {
    min: Point3,
    max: Point3,
}

impl From<Aabb3> for Aabb3Wire {
    fn from(b: Aabb3) -> Self {
        Self {
            min: b.min,
            max: b.max,
        }
    }
}

impl From<Aabb3Wire> for Aabb3 {
    fn from(w: Aabb3Wire) -> Self {
        Self::from_corners(w.min, w.max)
    }
}

impl Aabb3 {
    #[must_use]
    pub fn from_corners(a: Point3, b: Point3) -> Self {
        let (ax, ay, az) = a.coords_mm();
        let (bx, by, bz) = b.coords_mm();
        Self {
            min: Point3::from_mm(ax.min(bx), ay.min(by), az.min(bz)),
            max: Point3::from_mm(ax.max(bx), ay.max(by), az.max(bz)),
        }
    }

    #[must_use]
    pub fn from_points(points: impl IntoIterator<Item = Point3>) -> Option<Self> {
        points
            .into_iter()
            .map(|p| Self { min: p, max: p })
            .reduce(Self::union)
    }

    #[must_use]
    pub fn min(self) -> Point3 {
        self.min
    }

    #[must_use]
    pub fn max(self) -> Point3 {
        self.max
    }

    #[must_use]
    pub fn center(self) -> Point3 {
        let (lx, ly, lz) = self.min.coords_mm();
        let (hx, hy, hz) = self.max.coords_mm();
        Point3::from_mm(
            f64::midpoint(lx, hx),
            f64::midpoint(ly, hy),
            f64::midpoint(lz, hz),
        )
    }

    #[must_use]
    pub fn extent(self) -> Vec3 {
        self.max - self.min
    }

    #[must_use]
    pub fn union(self, other: Self) -> Self {
        let (lx, ly, lz) = self.min.coords_mm();
        let (hx, hy, hz) = self.max.coords_mm();
        let (olx, oly, olz) = other.min.coords_mm();
        let (ohx, ohy, ohz) = other.max.coords_mm();
        Self {
            min: Point3::from_mm(lx.min(olx), ly.min(oly), lz.min(olz)),
            max: Point3::from_mm(hx.max(ohx), hy.max(ohy), hz.max(ohz)),
        }
    }

    #[must_use]
    pub fn contains(self, p: Point3) -> bool {
        let (lx, ly, lz) = self.min.coords_mm();
        let (hx, hy, hz) = self.max.coords_mm();
        let (px, py, pz) = p.coords_mm();
        (lx..=hx).contains(&px) && (ly..=hy).contains(&py) && (lz..=hz).contains(&pz)
    }
}

impl core::fmt::Display for Aabb3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "aabb[{}..{}]", self.min, self.max)
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(try_from = "Plane3Wire", into = "Plane3Wire")]
pub struct Plane3 {
    origin: Point3,
    x: UnitVec3,
    y: UnitVec3,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Plane3", deny_unknown_fields)]
struct Plane3Wire {
    origin: Point3,
    x_axis: UnitVec3,
    y_axis: UnitVec3,
}

impl From<Plane3> for Plane3Wire {
    fn from(p: Plane3) -> Self {
        Self {
            origin: p.origin,
            x_axis: p.x,
            y_axis: p.y,
        }
    }
}

impl TryFrom<Plane3Wire> for Plane3 {
    type Error = TypesError;
    fn try_from(w: Plane3Wire) -> Result<Self> {
        Self::new(w.origin, w.x_axis, w.y_axis, PLANE_ORTHOGONALITY_TOLERANCE)
    }
}

impl Plane3 {
    pub fn new(origin: Point3, x: UnitVec3, y: UnitVec3, tolerance: Tolerance) -> Result<Self> {
        let (x, y) = orthonormalize(x, y, tolerance)?;
        Ok(Self { origin, x, y })
    }

    #[must_use]
    pub fn new_unchecked(origin: Point3, x: UnitVec3, y: UnitVec3) -> Self {
        debug_assert!(
            x.dot(y).abs() <= PLANE_ORTHOGONALITY_TOLERANCE.value(),
            "Plane3::new_unchecked requires orthonormal axes"
        );
        Self { origin, x, y }
    }

    #[must_use]
    pub fn origin(self) -> Point3 {
        self.origin
    }

    #[must_use]
    pub fn x_axis(self) -> UnitVec3 {
        self.x
    }

    #[must_use]
    pub fn y_axis(self) -> UnitVec3 {
        self.y
    }

    #[must_use]
    pub fn normal(self) -> UnitVec3 {
        UnitVec3(Unit::new_normalize(self.x.0.cross(&self.y.0)))
    }

    #[must_use]
    pub fn point_at_local(self, u_mm: f64, v_mm: f64, normal_offset_mm: f64) -> Point3 {
        let (ox, oy, oz) = self.origin.coords_mm();
        let (xx, xy, xz) = self.x.components();
        let (yx, yy, yz) = self.y.components();
        let (nx, ny, nz) = self.normal().components();
        Point3::from_mm(
            ox + u_mm * xx + v_mm * yx + normal_offset_mm * nx,
            oy + u_mm * xy + v_mm * yy + normal_offset_mm * ny,
            oz + u_mm * xz + v_mm * yz + normal_offset_mm * nz,
        )
    }
}

impl core::fmt::Display for Plane3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "plane{{ o={}, x={}, y={} }}",
            self.origin, self.x, self.y,
        )
    }
}

#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
#[serde(from = "AxisAngleWire", into = "AxisAngleWire")]
pub struct AxisAngle {
    axis: UnitVec3,
    angle: Angle,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "AxisAngle", deny_unknown_fields)]
struct AxisAngleWire {
    axis: UnitVec3,
    angle_rad: f64,
}

impl From<AxisAngle> for AxisAngleWire {
    fn from(a: AxisAngle) -> Self {
        Self {
            axis: a.axis,
            angle_rad: a.angle.get::<radian>(),
        }
    }
}

impl From<AxisAngleWire> for AxisAngle {
    fn from(w: AxisAngleWire) -> Self {
        Self {
            axis: w.axis,
            angle: Angle::new::<radian>(w.angle_rad),
        }
    }
}

impl AxisAngle {
    #[must_use]
    pub fn new(axis: UnitVec3, angle: Angle) -> Self {
        Self { axis, angle }
    }

    #[must_use]
    pub fn axis(self) -> UnitVec3 {
        self.axis
    }

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

impl core::fmt::Debug for AxisAngle {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "AxisAngle(axis={:?}, angle={} rad)",
            self.axis,
            self.angle.get::<radian>()
        )
    }
}

impl core::fmt::Display for AxisAngle {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "axisangle{{ axis={}, angle={} rad }}",
            self.axis,
            self.angle.get::<radian>()
        )
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(try_from = "OrientedBox3Wire", into = "OrientedBox3Wire")]
pub struct OrientedBox3 {
    frame: Plane3,
    half_extents: Vec3,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "OrientedBox3", deny_unknown_fields)]
struct OrientedBox3Wire {
    frame: Plane3,
    half_extents: Vec3,
}

impl From<OrientedBox3> for OrientedBox3Wire {
    fn from(b: OrientedBox3) -> Self {
        Self {
            frame: b.frame,
            half_extents: b.half_extents,
        }
    }
}

impl TryFrom<OrientedBox3Wire> for OrientedBox3 {
    type Error = TypesError;
    fn try_from(w: OrientedBox3Wire) -> Result<Self> {
        Self::new(w.frame, w.half_extents)
    }
}

impl OrientedBox3 {
    pub fn new(frame: Plane3, half_extents: Vec3) -> Result<Self> {
        let (hx, hy, hz) = half_extents.coords_mm();
        match [hx, hy, hz]
            .iter()
            .copied()
            .find(|h| !h.is_finite() || *h < 0.0)
        {
            Some(bad) => Err(TypesError::InvalidHalfExtent(bad)),
            None => Ok(Self {
                frame,
                half_extents,
            }),
        }
    }

    #[must_use]
    pub fn from_aabb(aabb: Aabb3) -> Self {
        let frame = Plane3::new_unchecked(aabb.center(), UnitVec3::x_axis(), UnitVec3::y_axis());
        Self {
            frame,
            half_extents: aabb.extent() * 0.5,
        }
    }

    #[must_use]
    pub fn center(self) -> Point3 {
        self.frame.origin()
    }

    #[must_use]
    pub fn frame(self) -> Plane3 {
        self.frame
    }

    #[must_use]
    pub fn half_extents(self) -> Vec3 {
        self.half_extents
    }
}

impl core::fmt::Display for OrientedBox3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "obox{{ c={}, half={} }}",
            self.center(),
            self.half_extents
        )
    }
}
