use bone_types::{
    Aabb3, Angle, ChordHeightTolerance, Length, Parameter, Plane3, Point2, Point3, Tolerance,
    UnitVec3, Vec3, radian,
};
use uom::si::length::millimeter;

use crate::circle2::Circle2;
use crate::closest::ClosestPoint3;
use crate::curve2::Curve2Kind;
use crate::curve3::{Curve3, Curve3Kind};
use crate::ellipse2::Ellipse2;
use crate::nurbs::{basis_functions, count_to_f64, find_span};

const TRANSPORT_TOLERANCE: Tolerance = Tolerance::new(1.0e-12);
const CHAIN_TOLERANCE_MM: f64 = 1.0e-6;
const SMOOTH_TANGENT_TOLERANCE: f64 = 1.0e-6;

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct SampleCount(u32);

impl SampleCount {
    pub const DEFAULT: Self = Self(256);

    #[must_use]
    pub const fn new(count: u32) -> Self {
        Self(if count < 1 { 1 } else { count })
    }

    #[must_use]
    pub const fn get(self) -> u32 {
        self.0
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct StationCount(u32);

impl StationCount {
    pub const DEFAULT: Self = Self(64);

    #[must_use]
    pub const fn new(count: u32) -> Self {
        Self(if count < 1 { 1 } else { count })
    }

    #[must_use]
    pub const fn get(self) -> u32 {
        self.0
    }
}

#[derive(Copy, Clone, Debug, PartialEq, PartialOrd)]
pub struct ArcFraction(f64);

impl ArcFraction {
    pub const START: Self = Self(0.0);
    pub const END: Self = Self(1.0);

    #[must_use]
    pub fn new(value: f64) -> Self {
        Self(value.clamp(0.0, 1.0))
    }

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

#[derive(Clone, Debug)]
struct ArcLengthTable {
    nodes: Vec<(f64, f64)>,
    total_mm: f64,
}

impl ArcLengthTable {
    fn sample<C: Curve3>(curve: &C, count: SampleCount) -> Self {
        let n = count.get();
        let points: Vec<Point3> = (0..=n)
            .map(|i| curve.evaluate(Parameter::new(f64::from(i) / f64::from(n))))
            .collect();
        let mut nodes = Vec::with_capacity(points.len());
        let total_mm = points.iter().enumerate().fold(0.0, |running, (i, point)| {
            let cumulative = if i == 0 {
                0.0
            } else {
                running + (*point - points[i - 1]).norm_mm()
            };
            nodes.push((
                f64::from(u32::try_from(i).unwrap_or(n)) / f64::from(n),
                cumulative,
            ));
            cumulative
        });
        Self { nodes, total_mm }
    }

    fn fraction_of(&self, native: Parameter) -> ArcFraction {
        if self.total_mm <= f64::EPSILON {
            return ArcFraction::START;
        }
        let target = native.value();
        let last = self.nodes.len() - 1;
        let length = match self.nodes.binary_search_by(|(t, _)| t.total_cmp(&target)) {
            Ok(index) => self.nodes[index].1,
            Err(0) => self.nodes[0].1,
            Err(index) if index > last => self.nodes[last].1,
            Err(index) => {
                let (t0, l0) = self.nodes[index - 1];
                let (t1, l1) = self.nodes[index];
                let span = t1 - t0;
                let weight = if span.abs() <= f64::EPSILON {
                    0.0
                } else {
                    (target - t0) / span
                };
                (l1 - l0).mul_add(weight, l0)
            }
        };
        ArcFraction::new(length / self.total_mm)
    }

    fn native_at(&self, fraction: ArcFraction) -> Parameter {
        let target = fraction.get() * self.total_mm;
        let last = self.nodes.len() - 1;
        match self
            .nodes
            .binary_search_by(|(_, len)| len.total_cmp(&target))
        {
            Ok(index) => Parameter::new(self.nodes[index].0),
            Err(0) => Parameter::new(self.nodes[0].0),
            Err(index) if index > last => Parameter::new(self.nodes[last].0),
            Err(index) => {
                let (t0, l0) = self.nodes[index - 1];
                let (t1, l1) = self.nodes[index];
                let span = l1 - l0;
                let weight = if span.abs() <= f64::EPSILON {
                    0.0
                } else {
                    (target - l0) / span
                };
                Parameter::new(t0 + (t1 - t0) * weight)
            }
        }
    }
}

#[derive(Clone, Debug)]
pub struct SweepPath3<'c, C: Curve3> {
    curve: &'c C,
    table: ArcLengthTable,
}

impl<'c, C: Curve3> SweepPath3<'c, C> {
    #[must_use]
    pub fn new(curve: &'c C, count: SampleCount) -> Self {
        Self {
            curve,
            table: ArcLengthTable::sample(curve, count),
        }
    }

    #[must_use]
    pub fn length(&self) -> Length {
        Length::new::<millimeter>(self.table.total_mm)
    }

    #[must_use]
    pub fn point_at(&self, fraction: ArcFraction) -> Point3 {
        self.curve.evaluate(self.table.native_at(fraction))
    }

    #[must_use]
    pub fn native_at(&self, fraction: ArcFraction) -> Parameter {
        self.table.native_at(fraction)
    }

    #[must_use]
    pub fn fraction_at(&self, native: Parameter) -> ArcFraction {
        self.table.fraction_of(native)
    }

    #[must_use]
    pub fn tangent_at(&self, fraction: ArcFraction) -> UnitVec3 {
        let native = self.table.native_at(fraction);
        self.curve
            .derivative(native)
            .try_normalize(TRANSPORT_TOLERANCE)
            .unwrap_or_else(|_| UnitVec3::z_axis())
    }

    #[must_use]
    pub fn min_curvature_radius_mm(&self, stations: StationCount) -> f64 {
        let n = stations.get();
        if n < 1 || self.table.total_mm <= TRANSPORT_TOLERANCE.value() {
            return f64::INFINITY;
        }
        let step_mm = self.table.total_mm / f64::from(n);
        let tangents: Vec<Vec3> = (0..=n)
            .map(|i| dir(self.tangent_at(ArcFraction::new(f64::from(i) / f64::from(n)))))
            .collect();
        tangents.windows(2).fold(f64::INFINITY, |tightest, pair| {
            let turn = pair[0].dot_mm2(pair[1]).clamp(-1.0, 1.0).acos();
            if turn <= f64::EPSILON {
                tightest
            } else {
                tightest.min(step_mm / turn)
            }
        })
    }
}

fn dir(unit: UnitVec3) -> Vec3 {
    let (x, y, z) = unit.components();
    Vec3::from_mm(x, y, z)
}

fn reflect(vector: Vec3, mirror: Vec3, mirror_sq: f64) -> Vec3 {
    vector - mirror * (2.0 / mirror_sq * vector.dot_mm2(mirror))
}

#[derive(Copy, Clone, Debug, PartialEq)]
pub struct Frame {
    origin: Point3,
    tangent: UnitVec3,
    normal: UnitVec3,
    binormal: UnitVec3,
}

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

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

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

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

    fn complete(origin: Point3, tangent: UnitVec3, reference: Vec3) -> Self {
        let normal = reference
            .try_normalize(TRANSPORT_TOLERANCE)
            .unwrap_or_else(|_| tangent.orthonormal_basis().0);
        let binormal = tangent
            .cross(normal, TRANSPORT_TOLERANCE)
            .unwrap_or_else(|_| tangent.orthonormal_basis().1);
        Self {
            origin,
            tangent,
            normal,
            binormal,
        }
    }

    #[must_use]
    pub fn seed(origin: Point3, tangent: UnitVec3, profile_plane: Plane3) -> Self {
        let along = dir(tangent);
        let project = |axis: UnitVec3| {
            let vector = dir(axis);
            vector - along * vector.dot_mm2(along)
        };
        let reference = project(profile_plane.x_axis());
        let seeded = if reference.norm_mm() > TRANSPORT_TOLERANCE.value() {
            reference
        } else {
            project(profile_plane.y_axis())
        };
        Self::complete(origin, tangent, seeded)
    }

    #[must_use]
    pub fn propagate(self, origin: Point3, tangent: UnitVec3) -> Self {
        let step = origin - self.origin;
        let c1 = step.dot_mm2(step);
        if c1 <= TRANSPORT_TOLERANCE.value() {
            return Self::complete(origin, tangent, dir(self.normal));
        }
        let reference_l = reflect(dir(self.normal), step, c1);
        let tangent_l = reflect(dir(self.tangent), step, c1);
        let tangent_swing = dir(tangent) - tangent_l;
        let c2 = tangent_swing.dot_mm2(tangent_swing);
        let reference = if c2 <= TRANSPORT_TOLERANCE.value() {
            reference_l
        } else {
            reflect(reference_l, tangent_swing, c2)
        };
        Self::complete(origin, tangent, reference)
    }

    #[must_use]
    pub fn reorient(self, tangent: UnitVec3) -> Self {
        let (t0x, t0y, t0z) = self.tangent.components();
        let (t1x, t1y, t1z) = tangent.components();
        let (ax, ay, az) = (
            t0y.mul_add(t1z, -(t0z * t1y)),
            t0z.mul_add(t1x, -(t0x * t1z)),
            t0x.mul_add(t1y, -(t0y * t1x)),
        );
        let sin = (ax * ax + ay * ay + az * az).sqrt();
        if sin <= TRANSPORT_TOLERANCE.value() {
            return Self::complete(self.origin, tangent, dir(self.normal));
        }
        let cos = t0x.mul_add(t1x, t0y.mul_add(t1y, t0z * t1z));
        let (kx, ky, kz) = (ax / sin, ay / sin, az / sin);
        let (nx, ny, nz) = self.normal.components();
        let dot = kx.mul_add(nx, ky.mul_add(ny, kz * nz)) * (1.0 - cos);
        let rotate =
            |value: f64, cross: f64, axis: f64| value.mul_add(cos, cross.mul_add(sin, axis * dot));
        let reference = Vec3::from_mm(
            rotate(nx, ky.mul_add(nz, -(kz * ny)), kx),
            rotate(ny, kz.mul_add(nx, -(kx * nz)), ky),
            rotate(nz, kx.mul_add(ny, -(ky * nx)), kz),
        );
        Self::complete(self.origin, tangent, reference)
    }

    #[must_use]
    pub fn section_plane(self) -> Plane3 {
        Plane3::new_unchecked(self.origin, self.normal, self.binormal)
    }

    #[must_use]
    pub fn place_point(self, local: Point2, anchor: Point2) -> Point3 {
        let (u, v) = local.coords_mm();
        let (au, av) = anchor.coords_mm();
        self.section_plane().point_at_local(u - au, v - av, 0.0)
    }

    #[must_use]
    pub fn twist(self, angle_rad: f64) -> Self {
        let (sin, cos) = angle_rad.sin_cos();
        let normal = dir(self.normal) * cos + dir(self.binormal) * sin;
        Self::complete(self.origin, self.tangent, normal)
    }

    #[must_use]
    pub fn locked(origin: Point3, tangent: UnitVec3, up: UnitVec3) -> Self {
        let along = dir(tangent);
        let raw = dir(up);
        let reference = raw - along * raw.dot_mm2(along);
        Self::complete(origin, tangent, reference)
    }

    #[must_use]
    pub fn twist_from(self, other: Self) -> f64 {
        let cos = dir(self.normal).dot_mm2(dir(other.normal));
        let sin = dir(self.normal)
            .cross(dir(other.normal))
            .dot_mm2(dir(self.tangent));
        sin.atan2(cos)
    }

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

    #[must_use]
    pub fn blend_tangent(self, target: UnitVec3, weight: f64) -> Self {
        if weight <= TRANSPORT_TOLERANCE.value() {
            return self;
        }
        self.reorient(slerp_unit(self.tangent, target, weight))
    }
}

fn slerp_unit(from: UnitVec3, to: UnitVec3, weight: f64) -> UnitVec3 {
    let a = dir(from);
    let b = dir(to);
    let theta = a.dot_mm2(b).clamp(-1.0, 1.0).acos();
    if theta <= TRANSPORT_TOLERANCE.value() {
        return to;
    }
    let sin = theta.sin();
    let scaled = a * (((1.0 - weight) * theta).sin() / sin) + b * ((weight * theta).sin() / sin);
    scaled.try_normalize(TRANSPORT_TOLERANCE).unwrap_or(to)
}

#[derive(Copy, Clone, Debug, PartialEq)]
pub enum FrameGuide {
    MinimumTwist,
    KeepNormal,
    DirectionLocked(UnitVec3),
}

#[derive(Clone, Debug, PartialEq)]
pub struct BSpline3 {
    degree: usize,
    knots: Vec<f64>,
    control: Vec<Point3>,
}

impl BSpline3 {
    #[must_use]
    pub fn degree(&self) -> usize {
        self.degree
    }

    #[must_use]
    pub fn knots(&self) -> &[f64] {
        &self.knots
    }

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

#[must_use]
pub fn chord_params(points: &[Point3]) -> Option<Vec<f64>> {
    if points.len() < 2 {
        return None;
    }
    let chords: Vec<f64> = points
        .windows(2)
        .map(|pair| (pair[1] - pair[0]).norm_mm())
        .collect();
    let total: f64 = chords.iter().sum();
    if chords
        .iter()
        .any(|&chord| chord <= TRANSPORT_TOLERANCE.value())
    {
        return None;
    }
    let params = chords.iter().scan(0.0, |running, chord| {
        *running += chord / total;
        Some(*running)
    });
    Some(core::iter::once(0.0).chain(params).collect())
}

fn averaged_knots(params: &[f64], degree: usize) -> Vec<f64> {
    let count = params.len();
    let total = count + degree + 1;
    (0..total)
        .map(|index| {
            if index <= degree {
                0.0
            } else if index >= count {
                1.0
            } else {
                let start = index - degree;
                params[start..start + degree].iter().sum::<f64>() / count_to_f64(degree)
            }
        })
        .collect()
}

pub struct PathInterpolator {
    degree: usize,
    knots: Vec<f64>,
    lu: nalgebra::LU<f64, nalgebra::Dyn, nalgebra::Dyn>,
}

impl PathInterpolator {
    #[must_use]
    pub fn new(params: &[f64], degree: usize) -> Option<Self> {
        let count = params.len();
        if count < degree + 1 || degree == 0 {
            return None;
        }
        let knots = averaged_knots(params, degree);
        let rows: Vec<(usize, Vec<f64>)> = params
            .iter()
            .map(|&u| {
                let span = find_span(count - 1, degree, u, &knots);
                (span, basis_functions(span, u, degree, &knots))
            })
            .collect();
        let matrix = nalgebra::DMatrix::from_fn(count, count, |row, col| {
            let (span, basis) = &rows[row];
            let first = span - degree;
            if col >= first && col <= *span {
                basis[col - first]
            } else {
                0.0
            }
        });
        let lu = matrix.lu();
        Some(Self { degree, knots, lu })
    }

    #[must_use]
    pub fn degree(&self) -> usize {
        self.degree
    }

    #[must_use]
    pub fn knots(&self) -> &[f64] {
        &self.knots
    }

    #[must_use]
    pub fn solve(&self, points: &[Point3]) -> Option<BSpline3> {
        let count = points.len();
        if count != self.knots.len() - self.degree - 1 {
            return None;
        }
        let rhs = nalgebra::DMatrix::from_fn(count, 3, |row, col| {
            let (x, y, z) = points[row].coords_mm();
            [x, y, z][col]
        });
        let solution = self.lu.solve(&rhs)?;
        let control = (0..count)
            .map(|row| Point3::from_mm(solution[(row, 0)], solution[(row, 1)], solution[(row, 2)]))
            .collect();
        Some(BSpline3 {
            degree: self.degree,
            knots: self.knots.clone(),
            control,
        })
    }
}

pub struct ClampedInterpolator {
    knots: Vec<f64>,
    lu: nalgebra::LU<f64, nalgebra::Dyn, nalgebra::Dyn>,
}

impl ClampedInterpolator {
    #[must_use]
    pub fn new(params: &[f64]) -> Option<Self> {
        let n = params.len().checked_sub(1)?;
        if n < 2 {
            return None;
        }
        let interior = (1..n).map(|j| (params[j - 1] + params[j] + params[j + 1]) / 3.0);
        let knots: Vec<f64> = core::iter::repeat_n(0.0, 4)
            .chain(interior)
            .chain(core::iter::repeat_n(1.0, 4))
            .collect();
        let count = n + 3;
        if knots[4] <= 0.0 || knots[count - 1] >= 1.0 {
            return None;
        }
        let start_scale = 3.0 / knots[4];
        let end_scale = 3.0 / (1.0 - knots[count - 1]);
        let basis_row = |row: usize, col: usize| {
            let u = params[row - 1];
            let span = find_span(count - 1, 3, u, &knots);
            let first = span - 3;
            if col >= first && col <= span {
                basis_functions(span, u, 3, &knots)[col - first]
            } else {
                0.0
            }
        };
        let matrix = nalgebra::DMatrix::from_fn(count, count, |row, col| {
            if row == 0 {
                f64::from(u8::from(col == 0))
            } else if row == 1 {
                match col {
                    0 => -start_scale,
                    1 => start_scale,
                    _ => 0.0,
                }
            } else if row == count - 2 {
                if col == count - 2 {
                    -end_scale
                } else if col == count - 1 {
                    end_scale
                } else {
                    0.0
                }
            } else if row == count - 1 {
                f64::from(u8::from(col == count - 1))
            } else {
                basis_row(row, col)
            }
        });
        Some(Self {
            knots,
            lu: matrix.lu(),
        })
    }

    #[must_use]
    pub fn knots(&self) -> &[f64] {
        &self.knots
    }

    #[must_use]
    pub fn solve(&self, points: &[Point3], start: Vec3, end: Vec3) -> Option<BSpline3> {
        let count = self.knots.len() - 4;
        if points.len() + 2 != count {
            return None;
        }
        let coords = |point: Point3| {
            let (x, y, z) = point.coords_mm();
            [x, y, z]
        };
        let derivative = |vector: Vec3| {
            let (x, y, z) = vector.coords_mm();
            [x, y, z]
        };
        let rhs = nalgebra::DMatrix::from_fn(count, 3, |row, col| {
            let values = if row == 0 {
                coords(points[0])
            } else if row == 1 {
                derivative(start)
            } else if row == count - 2 {
                derivative(end)
            } else if row == count - 1 {
                coords(points[points.len() - 1])
            } else {
                coords(points[row - 1])
            };
            values[col]
        });
        let solution = self.lu.solve(&rhs)?;
        let control = (0..count)
            .map(|row| Point3::from_mm(solution[(row, 0)], solution[(row, 1)], solution[(row, 2)]))
            .collect();
        Some(BSpline3 {
            degree: 3,
            knots: self.knots.clone(),
            control,
        })
    }
}

#[must_use]
pub fn transport<C: Curve3>(
    path: &SweepPath3<C>,
    profile_plane: Plane3,
    stations: StationCount,
) -> Vec<Frame> {
    transport_with(path, profile_plane, stations, FrameGuide::MinimumTwist)
}

#[must_use]
pub fn transport_with<C: Curve3>(
    path: &SweepPath3<C>,
    profile_plane: Plane3,
    stations: StationCount,
    guide: FrameGuide,
) -> Vec<Frame> {
    let (fractions, seed_index) = station_fractions(ArcFraction::START, stations);
    transport_stations(path, profile_plane, &fractions, seed_index, guide)
}

#[must_use]
pub fn station_fractions(origin: ArcFraction, stations: StationCount) -> (Vec<f64>, usize) {
    let s0 = origin.get();
    let interior = s0 > f64::EPSILON && 1.0 - s0 > f64::EPSILON;
    let n = if interior {
        stations.get().max(2)
    } else {
        stations.get()
    };
    let backward = if s0 <= f64::EPSILON {
        0
    } else if 1.0 - s0 <= f64::EPSILON {
        n
    } else {
        (1..n)
            .min_by(|a, b| {
                let miss = |i: u32| (f64::from(i) / f64::from(n) - s0).abs();
                miss(*a).total_cmp(&miss(*b))
            })
            .unwrap_or(0)
    };
    let forward = n - backward;
    let back = (0..backward).map(|i| s0 * f64::from(i) / f64::from(backward.max(1)));
    let fore =
        (0..=forward).map(|i| (1.0 - s0).mul_add(f64::from(i) / f64::from(forward.max(1)), s0));
    let fractions: Vec<f64> = if forward == 0 {
        back.chain(core::iter::once(s0)).collect()
    } else {
        back.chain(fore).collect()
    };
    (fractions, usize::try_from(backward).unwrap_or(0))
}

#[must_use]
pub fn transport_stations<C: Curve3>(
    path: &SweepPath3<C>,
    profile_plane: Plane3,
    fractions: &[f64],
    seed_index: usize,
    guide: FrameGuide,
) -> Vec<Frame> {
    if fractions.is_empty() || seed_index >= fractions.len() {
        return Vec::new();
    }
    let frame_at = |fraction: f64| {
        let at = ArcFraction::new(fraction);
        (path.point_at(at), path.tangent_at(at))
    };
    let (seed_origin, seed_tangent) = frame_at(fractions[seed_index]);
    let seed = Frame::seed(seed_origin, seed_tangent, profile_plane);
    let lock_offset = match guide {
        FrameGuide::DirectionLocked(axis) => {
            Frame::locked(seed_origin, seed_tangent, axis).twist_from(seed)
        }
        FrameGuide::MinimumTwist | FrameGuide::KeepNormal => 0.0,
    };
    let step = |previous: Frame, fraction: f64| {
        let (origin, tangent) = frame_at(fraction);
        match guide {
            FrameGuide::MinimumTwist => previous.propagate(origin, tangent),
            FrameGuide::KeepNormal => seed.translated(origin),
            FrameGuide::DirectionLocked(axis) => {
                Frame::locked(origin, tangent, axis).twist(lock_offset)
            }
        }
    };
    let forward = fractions[seed_index + 1..]
        .iter()
        .scan(seed, |carried, &fraction| {
            *carried = step(*carried, fraction);
            Some(*carried)
        });
    let backward = fractions[..seed_index]
        .iter()
        .rev()
        .scan(seed, |carried, &fraction| {
            *carried = step(*carried, fraction);
            Some(*carried)
        });
    let mut leading: Vec<Frame> = backward.collect();
    leading.reverse();
    leading
        .into_iter()
        .chain(core::iter::once(seed))
        .chain(forward)
        .collect()
}

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub enum SweepDefect {
    EmptyPath,
    DisconnectedPath,
    BranchingPath,
    PathTooTight,
    CornerTooTight,
    PathMissesProfile,
    EmptySpan,
    GuideEmpty,
    GuideMissesProfile,
    GuideCollapse,
    GuideSpanDegenerate,
    GuideBidirectional,
    GuideTwist,
    GuideKeepNormal,
    OrientationNeedsGuides,
    TangencyBidirectional,
    AlignmentOrientation,
    PathNotPlanar,
    AlignmentAlongPath,
    SolidToolBoss,
    SolidToolCorner,
    SolidToolOffBody,
}

impl core::fmt::Display for SweepDefect {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        f.write_str(match self {
            Self::EmptyPath => "an empty sweep path",
            Self::GuideEmpty => "a sweep guide with no curves",
            Self::DisconnectedPath => "a sweep path whose segments do not meet end to end",
            Self::BranchingPath => "a sweep path where more than two segments meet at a point",
            Self::PathTooTight => "a sweep path that curves tighter than the profile can follow",
            Self::CornerTooTight => "a path corner too tight for the profile to miter",
            Self::PathMissesProfile => "a sweep path that never crosses the profile plane",
            Self::EmptySpan => "a sweep direction with no path beyond the profile",
            Self::GuideMissesProfile => "a guide curve that does not touch the profile",
            Self::GuideCollapse => "a guide that pinches the section to a point",
            Self::GuideSpanDegenerate => "guide contact points that cannot span the section",
            Self::GuideBidirectional => "guide curves on a bidirectional sweep",
            Self::GuideTwist => "a twisted sweep with guide curves",
            Self::GuideKeepNormal => "a keep-normal sweep with guide curves",
            Self::OrientationNeedsGuides => "an orientation with too few guide curves",
            Self::TangencyBidirectional => "start or end tangency on a bidirectional sweep",
            Self::AlignmentOrientation => "a path alignment without follow-path orientation",
            Self::PathNotPlanar => "a planar-alignment sweep along a non-planar path",
            Self::AlignmentAlongPath => "a direction vector parallel to the path tangent",
            Self::SolidToolBoss => "a solid tool profile on a boss sweep",
            Self::SolidToolCorner => "a solid tool path with a corner",
            Self::SolidToolOffBody => "a solid tool path that does not start on the tool body",
        })
    }
}

#[derive(Copy, Clone, Debug, PartialEq)]
struct OrientedSegment {
    curve: Curve3Kind,
    reversed: bool,
}

impl OrientedSegment {
    fn param(self, t: Parameter) -> Parameter {
        if self.reversed {
            Parameter::new(1.0 - t.value())
        } else {
            t
        }
    }

    fn head(self) -> Point3 {
        self.evaluate(Parameter::new(0.0))
    }

    fn tail(self) -> Point3 {
        self.evaluate(Parameter::new(1.0))
    }
}

impl Curve3 for OrientedSegment {
    fn evaluate(&self, t: Parameter) -> Point3 {
        self.curve.evaluate(self.param(t))
    }

    fn derivative(&self, t: Parameter) -> Vec3 {
        let raw = self.curve.derivative(self.param(t));
        if self.reversed { -raw } else { raw }
    }

    fn bounding_box(&self) -> Aabb3 {
        self.curve.bounding_box()
    }

    fn closest_point(&self, p: Point3, tolerance: Tolerance) -> ClosestPoint3 {
        let inner = self.curve.closest_point(p, tolerance);
        ClosestPoint3::new(
            self.param(inner.parameter()),
            inner.point(),
            inner.distance(),
        )
    }

    fn tessellate(&self, tolerance: ChordHeightTolerance) -> Vec<Point3> {
        let mut points = self.curve.tessellate(tolerance);
        if self.reversed {
            points.reverse();
        }
        points
    }
}

#[derive(Clone, Debug, PartialEq)]
pub struct CompositePath3 {
    segments: Vec<OrientedSegment>,
    closed: bool,
}

impl CompositePath3 {
    pub fn chain(segments: &[Curve3Kind]) -> Result<Self, SweepDefect> {
        match segments {
            [] => Err(SweepDefect::EmptyPath),
            [only] => Ok(Self {
                segments: vec![OrientedSegment {
                    curve: *only,
                    reversed: false,
                }],
                closed: curve_is_closed(*only),
            }),
            many => Self::order(many),
        }
    }

    fn order(segments: &[Curve3Kind]) -> Result<Self, SweepDefect> {
        if segments.iter().any(|curve| curve_is_closed(*curve)) {
            return Err(SweepDefect::BranchingPath);
        }
        let endpoints: Vec<(Point3, Point3)> = segments
            .iter()
            .map(|curve| {
                (
                    curve.evaluate(Parameter::new(0.0)),
                    curve.evaluate(Parameter::new(1.0)),
                )
            })
            .collect();
        if branches(&endpoints) {
            return Err(SweepDefect::BranchingPath);
        }
        let start = start_index(&endpoints);
        let (start_head, start_tail) = endpoints[start];
        let reversed =
            incidence(&endpoints, start_tail) == 1 && incidence(&endpoints, start_head) != 1;
        let first = OrientedSegment {
            curve: segments[start],
            reversed,
        };
        let ordered = (1..segments.len()).try_fold(
            (
                vec![first],
                first.tail(),
                single_mask(segments.len(), start),
            ),
            |(mut chain, tail, used), _| {
                let (index, oriented) = next_segment(segments, &endpoints, &used, tail)
                    .ok_or(SweepDefect::DisconnectedPath)?;
                let next_tail = oriented.tail();
                chain.push(oriented);
                Ok((chain, next_tail, set_mask(used, index)))
            },
        );
        let (chain, tail, _) = ordered?;
        let closed = near(tail, chain[0].head());
        Ok(Self {
            segments: chain,
            closed,
        })
    }

    #[must_use]
    pub fn is_closed(&self) -> bool {
        self.closed
    }

    #[must_use]
    pub fn has_corner(&self) -> bool {
        let interior = self.segments.windows(2).any(|pair| {
            let outgoing = pair[0].derivative(Parameter::new(1.0));
            let incoming = pair[1].derivative(Parameter::new(0.0));
            !tangent_continuous(outgoing, incoming)
        });
        let seam = self.closed
            && self.segments.len() >= 2
            && !tangent_continuous(
                self.segments[self.segments.len() - 1].derivative(Parameter::new(1.0)),
                self.segments[0].derivative(Parameter::new(0.0)),
            );
        interior || seam
    }

    #[must_use]
    pub fn segment_count(&self) -> usize {
        self.segments.len()
    }

    #[must_use]
    pub fn is_polyline(&self) -> bool {
        self.segments
            .iter()
            .all(|segment| matches!(segment.curve, Curve3Kind::Line(_)))
    }

    #[must_use]
    pub fn runs(&self) -> Vec<Self> {
        let breaks = (1..self.segments.len()).filter(|&index| {
            !tangent_continuous(
                self.segments[index - 1].derivative(Parameter::new(1.0)),
                self.segments[index].derivative(Parameter::new(0.0)),
            )
        });
        let bounds: Vec<usize> = core::iter::once(0)
            .chain(breaks)
            .chain(core::iter::once(self.segments.len()))
            .collect();
        bounds
            .windows(2)
            .map(|window| Self {
                segments: self.segments[window[0]..window[1]].to_vec(),
                closed: false,
            })
            .collect()
    }

    #[must_use]
    pub fn vertices(&self) -> Vec<Point3> {
        let heads = self.segments.first().map(|segment| segment.head());
        heads
            .into_iter()
            .chain(self.segments.iter().map(|segment| segment.tail()))
            .collect()
    }

    #[must_use]
    pub fn trimmed(&self, from: Parameter, to: Parameter) -> Option<Self> {
        if from.value() >= to.value() {
            return None;
        }
        let (first, local_from) = self.locate(from.value());
        let (last, local_to) = self.locate(to.value());
        let sub = |index: usize, a: f64, b: f64| {
            let segment = self.segments[index];
            segment.curve.sub_curve(
                segment.param(Parameter::new(a)),
                segment.param(Parameter::new(b)),
            )
        };
        let segments: Vec<OrientedSegment> = if first == last {
            sub(first, local_from, local_to)
                .map(|curve| OrientedSegment {
                    curve,
                    reversed: false,
                })
                .into_iter()
                .collect()
        } else {
            let head = sub(first, local_from, 1.0);
            let tail = sub(last, 0.0, local_to);
            let middle = ((first + 1)..last).map(|index| sub(index, 0.0, 1.0));
            head.into_iter()
                .chain(middle.flatten())
                .chain(tail)
                .map(|curve| OrientedSegment {
                    curve,
                    reversed: false,
                })
                .collect()
        };
        if segments.is_empty() {
            return None;
        }
        Some(Self {
            segments,
            closed: false,
        })
    }

    #[must_use]
    pub fn segment_at(&self, t: Parameter) -> usize {
        self.locate(t.value()).0
    }

    #[must_use]
    pub fn reversed(&self) -> Self {
        Self {
            segments: self
                .segments
                .iter()
                .rev()
                .map(|segment| OrientedSegment {
                    curve: segment.curve,
                    reversed: !segment.reversed,
                })
                .collect(),
            closed: self.closed,
        }
    }
}

impl Curve3 for CompositePath3 {
    fn evaluate(&self, t: Parameter) -> Point3 {
        let (index, local) = self.locate(t.value());
        self.segments[index].evaluate(Parameter::new(local))
    }

    fn derivative(&self, t: Parameter) -> Vec3 {
        let (index, local) = self.locate(t.value());
        self.segments[index].derivative(Parameter::new(local))
    }

    fn bounding_box(&self) -> Aabb3 {
        self.segments
            .iter()
            .map(Curve3::bounding_box)
            .reduce(Aabb3::union)
            .unwrap_or_else(|| Aabb3::from_corners(Point3::origin(), Point3::origin()))
    }

    fn closest_point(&self, p: Point3, tolerance: Tolerance) -> ClosestPoint3 {
        let n = self.segments.len();
        self.segments
            .iter()
            .enumerate()
            .map(|(index, segment)| {
                let local = segment.closest_point(p, tolerance);
                let global = (count_to_f64(index) + local.parameter().value()) / count_to_f64(n);
                ClosestPoint3::new(Parameter::new(global), local.point(), local.distance())
            })
            .reduce(|best, candidate| {
                if candidate.distance() < best.distance() {
                    candidate
                } else {
                    best
                }
            })
            .unwrap_or_else(|| {
                ClosestPoint3::new(Parameter::new(0.0), p, Length::new::<millimeter>(0.0))
            })
    }

    fn tessellate(&self, tolerance: ChordHeightTolerance) -> Vec<Point3> {
        self.segments
            .iter()
            .enumerate()
            .flat_map(|(index, segment)| {
                let points = segment.tessellate(tolerance);
                let skip = usize::from(index > 0);
                points.into_iter().skip(skip).collect::<Vec<_>>()
            })
            .collect()
    }
}

impl CompositePath3 {
    fn locate(&self, t: f64) -> (usize, f64) {
        let n = self.segments.len();
        let scaled = t.clamp(0.0, 1.0) * count_to_f64(n);
        let index = (0..n)
            .find(|&i| scaled < count_to_f64(i + 1))
            .unwrap_or(n - 1);
        (index, scaled - count_to_f64(index))
    }
}

fn curve_is_closed(curve: Curve3Kind) -> bool {
    near(
        curve.evaluate(Parameter::new(0.0)),
        curve.evaluate(Parameter::new(1.0)),
    )
}

fn near(a: Point3, b: Point3) -> bool {
    (a - b).norm_mm() <= CHAIN_TOLERANCE_MM
}

fn tangent_continuous(outgoing: Vec3, incoming: Vec3) -> bool {
    match (
        outgoing.try_normalize(TRANSPORT_TOLERANCE),
        incoming.try_normalize(TRANSPORT_TOLERANCE),
    ) {
        (Ok(a), Ok(b)) => 1.0 - a.dot(b) <= SMOOTH_TANGENT_TOLERANCE,
        _ => false,
    }
}

fn incidence(endpoints: &[(Point3, Point3)], point: Point3) -> usize {
    endpoints
        .iter()
        .flat_map(|(head, tail)| [*head, *tail])
        .filter(|end| near(*end, point))
        .count()
}

fn branches(endpoints: &[(Point3, Point3)]) -> bool {
    endpoints
        .iter()
        .flat_map(|(head, tail)| [*head, *tail])
        .any(|point| incidence(endpoints, point) > 2)
}

fn start_index(endpoints: &[(Point3, Point3)]) -> usize {
    endpoints
        .iter()
        .position(|(head, _)| incidence(endpoints, *head) == 1)
        .or_else(|| {
            endpoints
                .iter()
                .position(|(_, tail)| incidence(endpoints, *tail) == 1)
        })
        .unwrap_or(0)
}

fn single_mask(count: usize, set: usize) -> Vec<bool> {
    (0..count).map(|index| index == set).collect()
}

fn set_mask(mut mask: Vec<bool>, index: usize) -> Vec<bool> {
    mask[index] = true;
    mask
}

fn next_segment(
    segments: &[Curve3Kind],
    endpoints: &[(Point3, Point3)],
    used: &[bool],
    tail: Point3,
) -> Option<(usize, OrientedSegment)> {
    (0..segments.len())
        .filter(|&index| !used[index])
        .find_map(|index| {
            let (head, back) = endpoints[index];
            if near(head, tail) {
                Some((
                    index,
                    OrientedSegment {
                        curve: segments[index],
                        reversed: false,
                    },
                ))
            } else if near(back, tail) {
                Some((
                    index,
                    OrientedSegment {
                        curve: segments[index],
                        reversed: true,
                    },
                ))
            } else {
                None
            }
        })
}

#[derive(Clone, Debug, PartialEq)]
pub struct SectionSpline {
    pub degree: usize,
    pub knots: Vec<f64>,
    pub control: Vec<Point2>,
    pub weights: Vec<f64>,
}

fn segment_count(sweep_abs: f64) -> usize {
    let quarter = core::f64::consts::FRAC_PI_2;
    let eps = 1.0e-9;
    if sweep_abs <= quarter + eps {
        1
    } else if sweep_abs <= core::f64::consts::PI + eps {
        2
    } else if sweep_abs <= 3.0 * quarter + eps {
        3
    } else {
        4
    }
}

fn arc_spline(center: Point2, radius: f64, start: f64, sweep: f64) -> SectionSpline {
    let segments = segment_count(sweep.abs());
    let step = sweep / count_to_f64(segments);
    let mid_weight = (step / 2.0).cos().abs();
    let (cx, cy) = center.coords_mm();
    let ring = |angle: f64| {
        Point2::from_mm(
            radius.mul_add(angle.cos(), cx),
            radius.mul_add(angle.sin(), cy),
        )
    };
    let apex = |angle: f64| {
        let reach = radius / (step / 2.0).cos().abs();
        Point2::from_mm(
            reach.mul_add(angle.cos(), cx),
            reach.mul_add(angle.sin(), cy),
        )
    };
    let arcs = (0..segments).flat_map(|index| {
        let a1 = start + count_to_f64(index) * step;
        [(apex(a1 + step / 2.0), mid_weight), (ring(a1 + step), 1.0)]
    });
    let weighted: Vec<(Point2, f64)> = core::iter::once((ring(start), 1.0)).chain(arcs).collect();
    let internal = (1..segments).flat_map(|index| {
        let knot = count_to_f64(index) / count_to_f64(segments);
        [knot, knot]
    });
    let knots: Vec<f64> = [0.0, 0.0, 0.0]
        .into_iter()
        .chain(internal)
        .chain([1.0, 1.0, 1.0])
        .collect();
    SectionSpline {
        degree: 2,
        knots,
        control: weighted.iter().map(|(point, _)| *point).collect(),
        weights: weighted.iter().map(|(_, weight)| *weight).collect(),
    }
}

fn line_spline(start: Point2, end: Point2) -> SectionSpline {
    SectionSpline {
        degree: 1,
        knots: vec![0.0, 0.0, 1.0, 1.0],
        control: vec![start, end],
        weights: vec![1.0, 1.0],
    }
}

fn bezier_spline(control: [Point2; 4]) -> SectionSpline {
    SectionSpline {
        degree: 3,
        knots: vec![0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0],
        control: control.to_vec(),
        weights: vec![1.0; 4],
    }
}

#[must_use]
pub fn circle_arc_spline(circle: Circle2, start: Angle, sweep: Angle) -> SectionSpline {
    arc_spline(
        circle.center(),
        circle.radius_mm(),
        start.get::<radian>(),
        sweep.get::<radian>(),
    )
}

#[must_use]
pub fn ellipse_arc_spline(ellipse: Ellipse2, start: Angle, sweep: Angle) -> SectionSpline {
    let unit = arc_spline(
        Point2::origin(),
        1.0,
        start.get::<radian>(),
        sweep.get::<radian>(),
    );
    let (cx, cy) = ellipse.center().coords_mm();
    let rotation = ellipse.rotation_rad();
    let (semi_major, semi_minor) = (ellipse.semi_major_mm(), ellipse.semi_minor_mm());
    let (major_x, major_y) = (rotation.cos(), rotation.sin());
    let (minor_x, minor_y) = (-rotation.sin(), rotation.cos());
    let control = unit
        .control
        .iter()
        .map(|point| {
            let (px, py) = point.coords_mm();
            let x = (semi_major * px).mul_add(major_x, (semi_minor * py) * minor_x) + cx;
            let y = (semi_major * px).mul_add(major_y, (semi_minor * py) * minor_y) + cy;
            Point2::from_mm(x, y)
        })
        .collect();
    SectionSpline {
        degree: unit.degree,
        knots: unit.knots,
        control,
        weights: unit.weights,
    }
}

#[must_use]
pub fn section_spline(curve: Curve2Kind) -> SectionSpline {
    let full_turn = Angle::new::<radian>(core::f64::consts::TAU);
    match curve {
        Curve2Kind::Line(line) => line_spline(line.start(), line.end()),
        Curve2Kind::Arc(arc) => arc_spline(
            arc.center(),
            arc.radius_mm(),
            arc.start_rad(),
            arc.sweep_rad(),
        ),
        Curve2Kind::Circle(circle) => {
            circle_arc_spline(circle, Angle::new::<radian>(0.0), full_turn)
        }
        Curve2Kind::Ellipse(ellipse) => {
            ellipse_arc_spline(ellipse, Angle::new::<radian>(0.0), full_turn)
        }
        Curve2Kind::Bezier(bezier) => bezier_spline(bezier.control()),
    }
}

#[must_use]
pub fn reverse_section(spline: &SectionSpline) -> SectionSpline {
    let first = spline.knots.first().copied().unwrap_or(0.0);
    let last = spline.knots.last().copied().unwrap_or(1.0);
    let span = first + last;
    SectionSpline {
        degree: spline.degree,
        knots: spline.knots.iter().rev().map(|knot| span - knot).collect(),
        control: spline.control.iter().rev().copied().collect(),
        weights: spline.weights.iter().rev().copied().collect(),
    }
}

#[cfg(test)]
mod tests {
    use super::{
        ArcFraction, BSpline3, CompositePath3, Frame, FrameGuide, PathInterpolator, SampleCount,
        SectionSpline, StationCount, SweepDefect, SweepPath3, chord_params, section_spline,
        transport, transport_with,
    };
    use crate::arc3::Arc3;
    use crate::circle3::Circle3;
    use crate::curve2::Curve2Kind;
    use crate::curve3::{Curve3, Curve3Kind};
    use crate::line3::Line3;
    use crate::nurbs::{Degree, KnotVector, RationalControlPoint};
    use crate::nurbs_curve::NurbsCurve3;
    use bone_types::{
        Aabb3, Angle, ChordHeightTolerance, Length, Parameter, Plane3, Point2, Point3, Tolerance,
        UnitVec3, Vec3, radian,
    };
    use core::f64::consts::{FRAC_PI_2, TAU};
    use uom::si::length::millimeter;

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

    fn interpolate(points: &[Point3], degree: usize) -> Option<BSpline3> {
        let effective = degree.min(points.len().saturating_sub(1));
        let params = chord_params(points)?;
        PathInterpolator::new(&params, effective)?.solve(points)
    }

    fn plane(origin: Point3, x: UnitVec3, y: UnitVec3) -> Plane3 {
        let Ok(plane) = Plane3::new(origin, x, y, TOL) else {
            panic!("axes orthonormal")
        };
        plane
    }

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

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

    fn circle(radius_mm: f64) -> Circle3 {
        let Ok(circle) = Circle3::new(xy_plane(), Length::new::<millimeter>(radius_mm), TOL) else {
            panic!("positive radius")
        };
        circle
    }

    fn z_line(len_mm: f64) -> Line3 {
        let Ok(line) = Line3::new(Point3::origin(), Point3::from_mm(0.0, 0.0, len_mm), TOL) else {
            panic!("distinct endpoints")
        };
        line
    }

    fn unit(x: f64, y: f64, z: f64) -> UnitVec3 {
        let Ok(direction) = Vec3::from_mm(x, y, z).try_normalize(TOL) else {
            panic!("nonzero direction")
        };
        direction
    }

    fn off(a: UnitVec3, target: Vec3) -> f64 {
        (super::dir(a) - target).norm_mm()
    }

    fn nurbs(degree: usize, knots: Vec<f64>, control: Vec<RationalControlPoint>) -> NurbsCurve3 {
        let Ok(degree) = Degree::new(degree) else {
            panic!("positive degree")
        };
        let Ok(knots) = KnotVector::new(knots) else {
            panic!("valid knot vector")
        };
        let Ok(spline) = NurbsCurve3::new(degree, knots, control) else {
            panic!("valid spline")
        };
        spline
    }

    fn eval_bspline(spline: &BSpline3, u: f64) -> Point3 {
        let control = spline
            .control()
            .iter()
            .map(|&point| {
                let Ok(control_point) = RationalControlPoint::new(point, 1.0, TOL) else {
                    panic!("unit weight")
                };
                control_point
            })
            .collect();
        nurbs(spline.degree(), spline.knots().to_vec(), control).evaluate(Parameter::new(u))
    }

    fn eval_bspline_derivative(spline: &BSpline3, u: f64) -> Vec3 {
        let step = 1.0e-6;
        let (lo, hi) = if u < 0.5 {
            (u, u + step)
        } else {
            (u - step, u)
        };
        (eval_bspline(spline, hi) - eval_bspline(spline, lo)) * (1.0 / step)
    }

    fn eval_section(spline: &SectionSpline, u: f64) -> Point2 {
        let control = spline
            .control
            .iter()
            .zip(&spline.weights)
            .map(|(point, &weight)| {
                let (x, y) = point.coords_mm();
                let Ok(control_point) =
                    RationalControlPoint::new(Point3::from_mm(x, y, 0.0), weight, TOL)
                else {
                    panic!("positive weight")
                };
                control_point
            })
            .collect();
        let (x, y, _) = nurbs(spline.degree, spline.knots.clone(), control)
            .evaluate(Parameter::new(u))
            .coords_mm();
        Point2::from_mm(x, y)
    }

    fn line_kind(a: Point3, b: Point3) -> Curve3Kind {
        let Ok(line) = Line3::new(a, b, TOL) else {
            panic!("distinct endpoints")
        };
        line.as_kind()
    }

    fn tangent_bend() -> Curve3Kind {
        let center = Point3::from_mm(10.0, 10.0, 0.0);
        let arc_plane = plane(center, UnitVec3::x_axis(), UnitVec3::y_axis());
        let Ok(arc) = Arc3::new(
            arc_plane,
            Length::new::<millimeter>(10.0),
            Angle::new::<radian>(-FRAC_PI_2),
            Angle::new::<radian>(FRAC_PI_2),
            TOL,
        ) else {
            panic!("valid quarter arc")
        };
        arc.as_kind()
    }

    struct Helix {
        radius: f64,
        climb: f64,
        turns: f64,
    }

    impl Curve3 for Helix {
        fn evaluate(&self, t: Parameter) -> Point3 {
            let angle = TAU * self.turns * t.value();
            Point3::from_mm(
                self.radius * angle.cos(),
                self.radius * angle.sin(),
                self.climb * angle,
            )
        }

        fn derivative(&self, t: Parameter) -> Vec3 {
            let angle = TAU * self.turns * t.value();
            let scale = TAU * self.turns;
            Vec3::from_mm(
                -self.radius * angle.sin() * scale,
                self.radius * angle.cos() * scale,
                self.climb * scale,
            )
        }

        fn bounding_box(&self) -> Aabb3 {
            Aabb3::from_corners(Point3::origin(), Point3::origin())
        }

        fn closest_point(
            &self,
            _p: Point3,
            _tolerance: Tolerance,
        ) -> crate::closest::ClosestPoint3 {
            unreachable!("unused in tests")
        }

        fn tessellate(&self, _tolerance: ChordHeightTolerance) -> Vec<Point3> {
            unreachable!("unused in tests")
        }
    }

    #[test]
    fn interpolation_hits_points_lines_and_clamped_end_tangents()
    -> Result<(), Box<dyn std::error::Error>> {
        let wobble: Vec<Point3> = (0..=10)
            .map(|i| {
                let t = f64::from(i) / 10.0;
                Point3::from_mm(t * 5.0, (t * TAU).sin() * 2.0, t * t * 3.0)
            })
            .collect();
        let spline = interpolate(&wobble, 3).ok_or("interpolates")?;
        let params = chord_params(&wobble).ok_or("chord params")?;
        wobble.iter().zip(params).for_each(|(expected, u)| {
            let got = eval_bspline(&spline, u);
            assert!(
                (got - *expected).norm_mm() < 1.0e-7,
                "interpolant misses {expected:?}, got {got:?}"
            );
        });

        let collinear: Vec<Point3> = (0..=6)
            .map(|i| Point3::from_mm(f64::from(i), 2.0 * f64::from(i), -f64::from(i)))
            .collect();
        let straight = interpolate(&collinear, 3).ok_or("interpolates")?;
        let mid = eval_bspline(&straight, 0.5);
        let (x, y, z) = mid.coords_mm();
        assert!(
            (y - 2.0 * x).abs() < 1.0e-7 && (z + x).abs() < 1.0e-7,
            "interpolant stays on the line: {mid:?}"
        );

        let arc: Vec<Point3> = (0..=8)
            .map(|i| {
                let theta = FRAC_PI_2 * f64::from(i) / 8.0;
                Point3::from_mm(5.0 * theta.cos(), 5.0 * theta.sin(), 0.0)
            })
            .collect();
        let arc_params = chord_params(&arc).ok_or("chord params")?;
        let clamped = super::ClampedInterpolator::new(&arc_params).ok_or("clamped interpolator")?;
        let start = Vec3::from_mm(0.0, 7.85, 0.0);
        let end = Vec3::from_mm(-7.85, 0.0, 0.0);
        let clamped_spline = clamped.solve(&arc, start, end).ok_or("clamped solve")?;
        arc.iter().zip(&arc_params).for_each(|(expected, &u)| {
            let got = eval_bspline(&clamped_spline, u);
            assert!(
                (got - *expected).norm_mm() < 1.0e-6,
                "clamped interpolant misses {expected:?}: got {got:?}"
            );
        });
        let aligned = |vector: Vec3, wanted: Vec3| {
            vector.dot_mm2(wanted) / (vector.norm_mm() * wanted.norm_mm())
        };
        let start_d = eval_bspline_derivative(&clamped_spline, 0.0);
        let end_d = eval_bspline_derivative(&clamped_spline, 1.0);
        assert!(
            aligned(start_d, start) > 0.9999,
            "the start derivative honors the clamp: {start_d:?}"
        );
        assert!(
            aligned(end_d, end) > 0.9999,
            "the end derivative honors the clamp: {end_d:?}"
        );
        assert!(
            (start_d.norm_mm() - start.norm_mm()).abs() < 0.05 * start.norm_mm(),
            "the clamped derivative magnitude matches the request: {}",
            start_d.norm_mm()
        );
        Ok(())
    }

    #[test]
    fn section_spline_lies_on_its_curve_hits_endpoints_and_reverses()
    -> Result<(), Box<dyn std::error::Error>> {
        let circle2 = crate::circle2::Circle2::new(
            Point2::from_mm(1.0, 2.0),
            Length::new::<millimeter>(3.0),
            TOL,
        )?;
        let on_circle = section_spline(Curve2Kind::Circle(circle2));
        (0..=40).for_each(|i| {
            let (x, y) = eval_section(&on_circle, f64::from(i) / 40.0).coords_mm();
            let radius = (x - 1.0).hypot(y - 2.0);
            assert!(
                (radius - 3.0).abs() < 1.0e-6,
                "sample sits on the r=3 circle: drift {}",
                (radius - 3.0).abs()
            );
        });

        let quarter = crate::arc2::Arc2::new(
            Point2::origin(),
            Length::new::<millimeter>(2.0),
            Angle::new::<radian>(0.0),
            Angle::new::<radian>(FRAC_PI_2),
            TOL,
        )?;
        let ends = section_spline(Curve2Kind::Arc(quarter));
        let start = eval_section(&ends, 0.0);
        let stop = eval_section(&ends, 1.0);
        assert!((start.coords_mm().0 - 2.0).abs() < 1.0e-9 && start.coords_mm().1.abs() < 1.0e-9);
        assert!(stop.coords_mm().0.abs() < 1.0e-9 && (stop.coords_mm().1 - 2.0).abs() < 1.0e-9);

        let tilted = crate::arc2::Arc2::new(
            Point2::from_mm(1.0, -1.0),
            Length::new::<millimeter>(2.0),
            Angle::new::<radian>(0.3),
            Angle::new::<radian>(1.1),
            TOL,
        )?;
        let forward = section_spline(Curve2Kind::Arc(tilted));
        let reversed = super::reverse_section(&forward);
        (0..=40).for_each(|i| {
            let u = f64::from(i) / 40.0;
            let (fx, fy) = eval_section(&forward, u).coords_mm();
            let (bx, by) = eval_section(&reversed, 1.0 - u).coords_mm();
            assert!(
                (fx - bx).hypot(fy - by) < 1.0e-9,
                "reverse(u') matches forward(1-u'): drift {}",
                (fx - bx).hypot(fy - by)
            );
        });
        Ok(())
    }

    #[test]
    fn frame_reorient_and_twist_swing_the_section_axes() -> Result<(), Box<dyn std::error::Error>> {
        let turned = Frame::seed(Point3::origin(), UnitVec3::x_axis(), xy_plane())
            .reorient(UnitVec3::y_axis());
        assert!(
            off(turned.tangent(), super::dir(UnitVec3::y_axis())) < 1.0e-9,
            "tangent tracks the target"
        );
        assert!(
            turned.normal().dot(turned.tangent()).abs() < 1.0e-9,
            "normal stays perpendicular to the reoriented tangent"
        );
        assert!(
            off(turned.normal(), super::dir(unit(-1.0, 0.0, 0.0))) < 1.0e-9,
            "a +X to +Y turn rotates the +Y normal onto -X"
        );
        assert!(
            off(turned.binormal(), super::dir(unit(0.0, 0.0, 1.0))) < 1.0e-9,
            "binormal is preserved"
        );

        let arc = circle(4.0);
        let path = SweepPath3::new(&arc, SampleCount::new(64));
        let frame = transport(&path, xz_plane(), StationCount::new(4))
            .first()
            .copied()
            .ok_or("at least one frame")?;
        let twisted = frame.twist(FRAC_PI_2);
        let onto_binormal = off(twisted.normal(), super::dir(frame.binormal()));
        let onto_neg_normal =
            (super::dir(twisted.binormal()) + super::dir(frame.normal())).norm_mm();
        assert!(
            onto_binormal < 1.0e-9 && onto_neg_normal < 1.0e-9,
            "a quarter twist rotates the normal onto the binormal"
        );
        assert!(
            super::dir(twisted.tangent()).dot_mm2(super::dir(frame.tangent())) > 1.0 - 1.0e-12,
            "twist leaves the tangent unchanged"
        );
        Ok(())
    }

    #[test]
    fn sweep_path_measures_length_and_curvature_on_lines_and_circles() {
        let straight = z_line(10.0);
        let line_path = SweepPath3::new(&straight, SampleCount::new(32));
        assert!((line_path.length().get::<millimeter>() - 10.0).abs() < 1.0e-9);
        assert!(
            (line_path.point_at(ArcFraction::new(0.5)).coords_mm().2 - 5.0).abs() < 1.0e-9,
            "half arc is the midpoint"
        );
        assert!(
            SweepPath3::new(&z_line(10.0), SampleCount::new(8))
                .min_curvature_radius_mm(StationCount::new(8))
                .is_infinite(),
            "a straight path never curves"
        );

        let ring = circle(4.0);
        let circle_path = SweepPath3::new(&ring, SampleCount::new(512));
        assert!(
            (circle_path.length().get::<millimeter>() - TAU * 4.0).abs() < 1.0e-2,
            "circle arc length is the circumference"
        );
        let quarter = circle_path.point_at(ArcFraction::new(0.25));
        let (x, y, _) = quarter.coords_mm();
        assert!(
            x.abs() < 1.0e-2 && (y - 4.0).abs() < 1.0e-2,
            "a quarter of the arc length lands a quarter turn around: {quarter:?}"
        );
        let radius = SweepPath3::new(&circle(4.0), SampleCount::new(2048))
            .min_curvature_radius_mm(StationCount::new(720));
        assert!(
            (radius - 4.0).abs() < 0.02,
            "a circle's tightest curvature radius is its radius: got {radius}"
        );
    }

    #[test]
    fn rmf_on_a_circle_tracks_radial_and_can_hold_the_seed_normal()
    -> Result<(), Box<dyn std::error::Error>> {
        let ring = circle(5.0);
        let path = SweepPath3::new(&ring, SampleCount::new(2048));
        let profile = xz_plane();

        transport(&path, profile, StationCount::new(720))
            .iter()
            .enumerate()
            .for_each(|(i, frame)| {
                let angle = TAU * f64::from(u32::try_from(i).unwrap_or(0)) / 720.0;
                let radial = Vec3::from_mm(angle.cos(), angle.sin(), 0.0);
                assert!(
                    off(frame.normal(), radial) < 5.0e-3,
                    "station {i}: reference {:?} should track radial {radial:?}",
                    frame.normal()
                );
            });

        let stations = StationCount::new(360);
        let held = transport_with(&path, profile, stations, FrameGuide::KeepNormal);
        let seed = held.first().copied().ok_or("at least one frame")?;
        held.iter().for_each(|frame| {
            assert!(
                off(frame.normal(), super::dir(seed.normal())) < 1.0e-9
                    && off(frame.binormal(), super::dir(seed.binormal())) < 1.0e-9,
                "keep-normal sections stay parallel to the seed: normal {:?}",
                frame.normal()
            );
        });
        let follow_drift = transport_with(&path, profile, stations, FrameGuide::MinimumTwist)
            .iter()
            .map(|frame| off(frame.normal(), super::dir(seed.normal())))
            .fold(0.0_f64, f64::max);
        assert!(
            follow_drift > 0.5,
            "the same curved path swings the follow-path normal well away from the seed, so \
             holding it constant is a real choice, not what transport does anyway: drift {follow_drift}"
        );
        Ok(())
    }

    #[test]
    fn rmf_on_a_helix_has_zero_twist_and_matches_the_closed_form() {
        let helix = Helix {
            radius: 3.0,
            climb: 1.5,
            turns: 1.0,
        };

        let path = SweepPath3::new(&helix, SampleCount::new(4096));
        let profile = plane(Point3::origin(), unit(0.0, 1.0, 0.0), unit(0.0, 0.0, 1.0));
        let frames = transport(&path, profile, StationCount::new(1024));
        let max_twist = frames.windows(2).fold(0.0_f64, |worst, pair| {
            let delta = super::dir(pair[1].normal()) - super::dir(pair[0].normal());
            let mean_binormal = super::dir(pair[0].binormal()) + super::dir(pair[1].binormal());
            worst.max(delta.dot_mm2(mean_binormal).abs())
        });
        assert!(max_twist < 1.0e-4, "helix frame twists by {max_twist}");
        frames.iter().for_each(|frame| {
            assert!(
                frame.normal().dot(frame.tangent()).abs() < 1.0e-6,
                "frame stays orthonormal"
            );
            assert!(frame.binormal().dot(frame.tangent()).abs() < 1.0e-6);
        });

        let fine = SweepPath3::new(&helix, SampleCount::new(8192));
        let closed_profile = plane(Point3::origin(), unit(-1.0, 0.0, 0.0), unit(0.0, 1.0, 0.0));
        let closed = transport(&fine, closed_profile, StationCount::new(2048));
        let speed = (helix.radius * helix.radius + helix.climb * helix.climb).sqrt();
        let stations = closed.len() - 1;
        closed.iter().enumerate().for_each(|(i, frame)| {
            let t = f64::from(u32::try_from(i).unwrap_or(0))
                / f64::from(u32::try_from(stations).unwrap_or(1));
            let angle = TAU * t;
            let frenet_n = Vec3::from_mm(-angle.cos(), -angle.sin(), 0.0);
            let frenet_b = Vec3::from_mm(
                helix.climb * angle.sin() / speed,
                -helix.climb * angle.cos() / speed,
                helix.radius / speed,
            );
            let lag = helix.climb / speed * angle;
            let expected = frenet_n * lag.cos() - frenet_b * lag.sin();
            let drift = off(frame.normal(), expected);
            assert!(drift < 5.0e-3, "station {i}: closed-form drift {drift}");
        });
    }

    #[test]
    fn section_placement_reproduces_the_seed_and_rides_the_path() {
        let axis = z_line(8.0);
        let path = SweepPath3::new(&axis, SampleCount::new(16));
        let frames = transport(&path, xy_plane(), StationCount::new(4));
        let begin = frames[0]
            .place_point(Point2::from_mm(1.0, 2.0), Point2::origin())
            .coords_mm();
        assert!(
            (begin.0 - 1.0).abs() < 1.0e-9
                && (begin.1 - 2.0).abs() < 1.0e-9
                && begin.2.abs() < 1.0e-9,
            "the seed section sits on the profile plane: {begin:?}"
        );
        let end = frames[frames.len() - 1]
            .place_point(Point2::from_mm(1.0, 0.0), Point2::origin())
            .coords_mm();
        assert!(
            (end.2 - 8.0).abs() < 1.0e-9,
            "the end section has travelled the full path length: {end:?}"
        );
    }

    #[test]
    fn composite_path_orders_open_chains_detects_shape_and_rejects_broken()
    -> Result<(), Box<dyn std::error::Error>> {
        type Chain = (Vec<Curve3Kind>, usize, f64, [Point3; 2], bool);
        let origin = Point3::from_mm(0.0, 0.0, 0.0);
        let east = Point3::from_mm(10.0, 0.0, 0.0);
        let northeast = Point3::from_mm(10.0, 10.0, 0.0);
        let north = Point3::from_mm(0.0, 10.0, 0.0);
        let far_east = Point3::from_mm(20.0, 0.0, 0.0);
        #[rustfmt::skip]
        let chains: [Chain; 2] = [
            (vec![line_kind(east, northeast), line_kind(north, northeast), line_kind(origin, east)], 3, 30.0, [origin, north], true),
            (vec![line_kind(east, origin), line_kind(east, far_east)], 2, 20.0, [origin, far_east], false),
        ];
        chains.into_iter().try_for_each(
            |(segments, count, length_mm, termini, corner)| -> Result<(), Box<dyn std::error::Error>> {
                let path = CompositePath3::chain(&segments).map_err(|_| "one connected open chain")?;
                assert!(!path.is_closed(), "an open chain does not return");
                assert_eq!(path.segment_count(), count);
                let spine = SweepPath3::new(&path, SampleCount::new(300));
                assert!(
                    (spine.length().get::<millimeter>() - length_mm).abs() < 1.0e-6,
                    "arc length is the sum of the edges: {}",
                    spine.length().get::<millimeter>()
                );
                let start = path.evaluate(Parameter::new(0.0));
                let end = path.evaluate(Parameter::new(1.0));
                assert!(
                    termini.iter().any(|t| (start - *t).norm_mm() < 1.0e-9),
                    "the path opens at a terminus: {start:?}"
                );
                assert!(
                    termini.iter().any(|t| (end - *t).norm_mm() < 1.0e-9),
                    "the path closes at the other terminus: {end:?}"
                );
                assert_eq!(
                    path.has_corner(),
                    corner,
                    "corner detection matches the polyline shape"
                );
                Ok(())
            },
        )?;

        #[rustfmt::skip]
        let l1 = line_kind(Point3::from_mm(0.0, 0.0, 0.0), Point3::from_mm(10.0, 0.0, 0.0));
        #[rustfmt::skip]
        let l2 = line_kind(Point3::from_mm(20.0, 10.0, 0.0), Point3::from_mm(20.0, 20.0, 0.0));
        let smooth =
            CompositePath3::chain(&[l1, tangent_bend(), l2]).map_err(|_| "tangent chain")?;
        assert_eq!(smooth.segment_count(), 3);
        assert!(!smooth.is_closed());
        assert!(
            !smooth.has_corner(),
            "a line into a tangent arc into a line has no tangent break"
        );

        #[rustfmt::skip]
        let square = [line_kind(origin, east), line_kind(east, northeast), line_kind(northeast, north), line_kind(north, origin)];
        let closed = CompositePath3::chain(&square).map_err(|_| "the square closes")?;
        assert!(
            closed.is_closed(),
            "a chain that returns to its start is closed"
        );

        #[rustfmt::skip]
        let gap = [
            line_kind(Point3::from_mm(0.0, 0.0, 0.0), Point3::from_mm(10.0, 0.0, 0.0)),
            line_kind(Point3::from_mm(11.0, 0.0, 0.0), Point3::from_mm(20.0, 0.0, 0.0)),
        ];
        let hub = Point3::from_mm(5.0, 0.0, 0.0);
        let branch = [
            line_kind(Point3::from_mm(0.0, 0.0, 0.0), hub),
            line_kind(hub, Point3::from_mm(10.0, 0.0, 0.0)),
            line_kind(hub, Point3::from_mm(5.0, 5.0, 0.0)),
        ];
        [
            (&gap[..], SweepDefect::DisconnectedPath),
            (&branch[..], SweepDefect::BranchingPath),
        ]
        .into_iter()
        .for_each(|(segments, defect)| {
            assert_eq!(CompositePath3::chain(segments), Err(defect));
        });
        Ok(())
    }

    #[test]
    fn composite_path_trims_reverses_and_finds_closest_points()
    -> Result<(), Box<dyn std::error::Error>> {
        let a = Point3::from_mm(0.0, 0.0, 0.0);
        let b = Point3::from_mm(10.0, 0.0, 0.0);
        let bent_c = Point3::from_mm(10.0, 10.0, 0.0);
        let bent = CompositePath3::chain(&[line_kind(a, b), line_kind(b, bent_c)])
            .map_err(|_| "two joined edges chain")?;

        let trimmed = bent
            .trimmed(Parameter::new(0.25), Parameter::new(1.0))
            .ok_or("a quarter-in trim keeps three quarters of the chain")?;
        let trim_start = trimmed.evaluate(Parameter::new(0.0));
        assert!(
            (trim_start - Point3::from_mm(5.0, 0.0, 0.0)).norm_mm() < 1.0e-9,
            "the trim opens a quarter of the way along the first edge: {trim_start:?}"
        );
        let trim_end = trimmed.evaluate(Parameter::new(1.0));
        assert!(
            (trim_end - bent_c).norm_mm() < 1.0e-9,
            "the trim keeps the original terminus: {trim_end:?}"
        );
        let spine = SweepPath3::new(&trimmed, SampleCount::new(200));
        assert!(
            (spine.length().get::<millimeter>() - 15.0).abs() < 1.0e-6,
            "the trimmed chain keeps 15 of the 20 mm"
        );

        let reversed = bent.reversed();
        assert!(
            (reversed.evaluate(Parameter::new(0.0)) - bent.evaluate(Parameter::new(1.0))).norm_mm()
                < 1.0e-9
                && (reversed.evaluate(Parameter::new(1.0)) - bent.evaluate(Parameter::new(0.0)))
                    .norm_mm()
                    < 1.0e-9,
            "reversal walks the same chain backward"
        );

        let one_edge = CompositePath3::chain(&[line_kind(a, b)]).map_err(|_| "one edge chains")?;
        assert!(
            one_edge
                .trimmed(Parameter::new(0.5), Parameter::new(0.5))
                .is_none(),
            "a zero-width window has no curve to keep"
        );

        let straight_c = Point3::from_mm(20.0, 0.0, 0.0);
        let straight = CompositePath3::chain(&[line_kind(b, a), line_kind(b, straight_c)])
            .map_err(|_| "a-b-c is one connected open path")?;
        [
            (Point3::from_mm(3.0, 4.0, 0.0), Point3::from_mm(3.0, 0.0, 0.0)),
            (Point3::from_mm(17.0, 4.0, 0.0), Point3::from_mm(17.0, 0.0, 0.0)),
        ]
        .into_iter()
        .for_each(|(query, foot)| {
            let hit = straight.closest_point(query, TOL);
            let echoed = straight.evaluate(hit.parameter());
            assert!(
                (echoed - hit.point()).norm_mm() < 1.0e-9,
                "feeding the returned parameter back into evaluate reproduces the returned point: {echoed:?} vs {:?}",
                hit.point()
            );
            assert!(
                (hit.point() - foot).norm_mm() < 1.0e-9,
                "the closest point is the perpendicular foot: {:?} vs {foot:?}",
                hit.point()
            );
            assert!(
                (hit.distance().get::<millimeter>() - 4.0).abs() < 1.0e-9,
                "the distance is the perpendicular offset: {}",
                hit.distance().get::<millimeter>()
            );
        });
        Ok(())
    }

    #[test]
    fn station_fractions_pin_the_seed_and_seeded_transport_matches_on_a_line() {
        let (mid, mid_seed) =
            super::station_fractions(ArcFraction::new(0.4), StationCount::new(10));
        assert_eq!(mid.len(), 11, "n stations produce n + 1 fractions");
        assert!(
            (mid[mid_seed] - 0.4).abs() < 1.0e-12,
            "the seed station sits exactly on the origin fraction: {}",
            mid[mid_seed]
        );
        assert!(
            mid.windows(2).all(|pair| pair[1] > pair[0]),
            "fractions ascend"
        );
        assert!(
            mid[0].abs() < 1.0e-12 && (mid[10] - 1.0).abs() < 1.0e-12,
            "the stations still span the whole path"
        );

        let (uniform, start_seed) =
            super::station_fractions(ArcFraction::START, StationCount::new(8));
        assert_eq!(start_seed, 0);
        uniform.iter().enumerate().for_each(|(i, fraction)| {
            let expected = f64::from(u32::try_from(i).unwrap_or(0)) / 8.0;
            assert!(
                (fraction - expected).abs() < 1.0e-12,
                "station {i} sits at {expected}: got {fraction}"
            );
        });

        let axis = z_line(8.0);
        let path = SweepPath3::new(&axis, SampleCount::new(64));
        let (fractions, seed) =
            super::station_fractions(ArcFraction::new(0.5), StationCount::new(8));
        let seeded = super::transport_stations(
            &path,
            xy_plane(),
            &fractions,
            seed,
            FrameGuide::MinimumTwist,
        );
        let start_seeded = transport(&path, xy_plane(), StationCount::new(8));
        seeded
            .iter()
            .zip(&start_seeded)
            .for_each(|(mid_frame, start_frame)| {
                let drift = off(mid_frame.normal(), super::dir(start_frame.normal()));
                assert!(
                    drift < 1.0e-9,
                    "on a straight path the seed position cannot twist the frame: drift {drift}"
                );
            });
    }
}
