use bone_document::{Sketch3, Sketch3Entity};
use bone_render::{ViewportExtent, ViewportPoint, world_ray};
use bone_types::{Camera3, Length, Point3, SketchEntityId, SketchPlaneBasis, UnitVec3};
use uom::si::length::millimeter;

const RAY_DENOM_FLOOR: f64 = 1.0e-9;

#[must_use]
pub fn ray_plane_point(
    camera: Camera3,
    extent: ViewportExtent,
    pixel: ViewportPoint,
    plane: SketchPlaneBasis,
) -> Option<Point3> {
    let (origin, direction) = world_ray(camera, extent, pixel).ok()?;
    let (nx, ny, nz) = plane.normal().components();
    let (ox, oy, oz) = origin.coords_mm();
    let (dx, dy, dz) = direction.components();
    let denom = dx * nx + dy * ny + dz * nz;
    if denom.abs() < RAY_DENOM_FLOOR {
        return None;
    }
    let (px, py, pz) = plane.origin().coords_mm();
    let t = ((px - ox) * nx + (py - oy) * ny + (pz - oz) * nz) / denom;
    if t <= 0.0 {
        return None;
    }
    Some(Point3::from_mm(ox + dx * t, oy + dy * t, oz + dz * t))
}

type Ray = ((f64, f64, f64), (f64, f64, f64));

#[derive(Copy, Clone, Debug, PartialEq)]
pub enum Snap3 {
    Origin { at: Point3 },
    Endpoint { point: SketchEntityId, at: Point3 },
    Midpoint { line: SketchEntityId, at: Point3 },
    OnEdge { entity: SketchEntityId, at: Point3 },
}

impl Snap3 {
    #[must_use]
    pub fn at(self) -> Point3 {
        match self {
            Self::Origin { at }
            | Self::Endpoint { at, .. }
            | Self::Midpoint { at, .. }
            | Self::OnEdge { at, .. } => at,
        }
    }
}

fn ray_perp_dist2(ray: Ray, p: Point3) -> f64 {
    let ((ox, oy, oz), (dx, dy, dz)) = ray;
    let (px, py, pz) = p.coords_mm();
    let (vx, vy, vz) = (px - ox, py - oy, pz - oz);
    let t = vx.mul_add(dx, vy.mul_add(dy, vz * dz));
    if t < 0.0 {
        return f64::INFINITY;
    }
    let (ex, ey, ez) = (dx.mul_add(-t, vx), dy.mul_add(-t, vy), dz.mul_add(-t, vz));
    ex.mul_add(ex, ey.mul_add(ey, ez * ez))
}

fn closest_on_segment(ray: Ray, start: Point3, end: Point3) -> Point3 {
    let ((ox, oy, oz), (dx, dy, dz)) = ray;
    let (ax, ay, az) = start.coords_mm();
    let (bx, by, bz) = end.coords_mm();
    let (ux, uy, uz) = (bx - ax, by - ay, bz - az);
    let (rx, ry, rz) = (ox - ax, oy - ay, oz - az);
    let ray_dot_seg = dx.mul_add(ux, dy.mul_add(uy, dz * uz));
    let seg_len2 = ux.mul_add(ux, uy.mul_add(uy, uz * uz));
    let ray_dot_rel = dx.mul_add(rx, dy.mul_add(ry, dz * rz));
    let seg_dot_rel = ux.mul_add(rx, uy.mul_add(ry, uz * rz));
    let denom = ray_dot_seg.mul_add(-ray_dot_seg, seg_len2);
    let t = if denom.abs() < RAY_DENOM_FLOOR {
        0.0
    } else {
        (seg_dot_rel - ray_dot_rel * ray_dot_seg) / denom
    }
    .clamp(0.0, 1.0);
    Point3::from_mm(ux.mul_add(t, ax), uy.mul_add(t, ay), uz.mul_add(t, az))
}

fn nearest<T>(
    candidates: impl Iterator<Item = (T, Point3, f64)>,
    tol2: f64,
) -> Option<(T, Point3)> {
    candidates
        .filter(|(_, _, dist2)| *dist2 <= tol2)
        .min_by(|a, b| a.2.total_cmp(&b.2))
        .map(|(item, at, _)| (item, at))
}

#[must_use]
pub fn nearest_snap3(
    sketch: &Sketch3,
    camera: Camera3,
    extent: ViewportExtent,
    pixel: ViewportPoint,
    tolerance: Length,
    exclude: Option<SketchEntityId>,
    reference: Option<Point3>,
) -> Option<Snap3> {
    let Ok((origin, direction)) = world_ray(camera, extent, pixel) else {
        return None;
    };
    let ray: Ray = (origin.coords_mm(), direction.components());
    let tol = tolerance.get::<millimeter>();
    let tol2 = tol * tol;

    if let Some(anchor) = reference
        && ray_perp_dist2(ray, anchor) <= tol2
    {
        return Some(Snap3::Origin { at: anchor });
    }

    let points = sketch
        .entity_order()
        .iter()
        .filter(|id| Some(**id) != exclude)
        .filter_map(|id| {
            sketch
                .point_position(*id)
                .map(|p| (*id, p, ray_perp_dist2(ray, p)))
        });
    if let Some((point, at)) = nearest(points, tol2) {
        return Some(Snap3::Endpoint { point, at });
    }

    let lines: Vec<(SketchEntityId, Point3, Point3)> = sketch
        .entity_order()
        .iter()
        .filter_map(|id| match sketch.entities().get(*id)? {
            Sketch3Entity::Line(line) => Some((
                *id,
                sketch.point_position(line.a())?,
                sketch.point_position(line.b())?,
            )),
            _ => None,
        })
        .collect();

    let midpoints = lines.iter().map(|(id, a, b)| {
        let (ax, ay, az) = a.coords_mm();
        let (bx, by, bz) = b.coords_mm();
        let mid = Point3::from_mm(
            f64::midpoint(ax, bx),
            f64::midpoint(ay, by),
            f64::midpoint(az, bz),
        );
        (*id, mid, ray_perp_dist2(ray, mid))
    });
    if let Some((line, at)) = nearest(midpoints, tol2) {
        return Some(Snap3::Midpoint { line, at });
    }

    let on_edges = sketch.entity_order().iter().filter_map(|id| {
        let poly = entity_polyline(sketch, *id)?;
        let chord = closest_on_polyline(ray, &poly)?;
        let at = refine_on_edge(sketch, *id, chord);
        Some((*id, at, ray_perp_dist2(ray, at)))
    });
    nearest(on_edges, tol2).map(|(entity, at)| Snap3::OnEdge { entity, at })
}

fn project_radial(center: Point3, radius: f64, normal: UnitVec3, at: Point3) -> Point3 {
    let (u, v) = normal.orthonormal_basis();
    let (a, b) = local_angle(center, u, v, at);
    if a.hypot(b) < RAY_DENOM_FLOOR {
        return at;
    }
    around(center, u, v, radius, radius, b.atan2(a))
}

fn refine_on_edge(sketch: &Sketch3, id: SketchEntityId, at: Point3) -> Point3 {
    match sketch.entities().get(id) {
        Some(Sketch3Entity::Circle(circle)) => {
            sketch.point_position(circle.center()).map_or(at, |center| {
                project_radial(
                    center,
                    circle.radius().get::<millimeter>(),
                    circle.normal(),
                    at,
                )
            })
        }
        Some(Sketch3Entity::Arc(arc)) => {
            match (
                sketch.point_position(arc.center()),
                sketch.point_position(arc.start()),
            ) {
                (Some(center), Some(start)) => {
                    let (sx, sy, sz) = (start - center).coords_mm();
                    let radius = sx.mul_add(sx, sy.mul_add(sy, sz * sz)).sqrt();
                    project_radial(center, radius, arc.normal(), at)
                }
                _ => at,
            }
        }
        _ => at,
    }
}

fn entity_polyline(sketch: &Sketch3, id: SketchEntityId) -> Option<Vec<Point3>> {
    match sketch.entities().get(id)? {
        Sketch3Entity::Line(line) => Some(vec![
            sketch.point_position(line.a())?,
            sketch.point_position(line.b())?,
        ]),
        Sketch3Entity::Circle(circle) => Some(circle3_polyline(
            sketch.point_position(circle.center())?,
            circle.radius().get::<millimeter>(),
            circle.normal(),
        )),
        Sketch3Entity::Arc(arc) => Some(arc3_polyline(
            sketch.point_position(arc.center())?,
            sketch.point_position(arc.start())?,
            sketch.point_position(arc.end())?,
            arc.normal(),
        )),
        Sketch3Entity::Ellipse(ellipse) => Some(ellipse3_polyline(
            sketch.point_position(ellipse.center())?,
            sketch.point_position(ellipse.major())?,
            sketch.point_position(ellipse.minor())?,
            ellipse.normal(),
        )),
        Sketch3Entity::Point(_) | Sketch3Entity::Spline(_) => None,
    }
}

fn closest_on_polyline(ray: Ray, polyline: &[Point3]) -> Option<Point3> {
    polyline
        .windows(2)
        .map(|w| {
            let q = closest_on_segment(ray, w[0], w[1]);
            (q, ray_perp_dist2(ray, q))
        })
        .min_by(|a, b| a.1.total_cmp(&b.1))
        .map(|(q, _)| q)
}

pub(crate) const SKETCH3_CURVE_SEGMENTS: u32 = 48;

pub(crate) fn around(
    center: Point3,
    u: UnitVec3,
    v: UnitVec3,
    ru: f64,
    rv: f64,
    angle: f64,
) -> Point3 {
    let (cx, cy, cz) = center.coords_mm();
    let (ux, uy, uz) = u.components();
    let (vx, vy, vz) = v.components();
    let (ca, sa) = (angle.cos(), angle.sin());
    Point3::from_mm(
        cx + ru * ca * ux + rv * sa * vx,
        cy + ru * ca * uy + rv * sa * vy,
        cz + ru * ca * uz + rv * sa * vz,
    )
}

pub(crate) fn circle3_polyline(center: Point3, radius: f64, normal: UnitVec3) -> Vec<Point3> {
    let (u, v) = normal.orthonormal_basis();
    (0..=SKETCH3_CURVE_SEGMENTS)
        .map(|i| {
            let angle = core::f64::consts::TAU * f64::from(i) / f64::from(SKETCH3_CURVE_SEGMENTS);
            around(center, u, v, radius, radius, angle)
        })
        .collect()
}

fn local_angle(center: Point3, u: UnitVec3, v: UnitVec3, p: Point3) -> (f64, f64) {
    let (px, py, pz) = (p - center).coords_mm();
    let (ux, uy, uz) = u.components();
    let (vx, vy, vz) = v.components();
    (px * ux + py * uy + pz * uz, px * vx + py * vy + pz * vz)
}

pub(crate) fn arc3_polyline(
    center: Point3,
    start: Point3,
    end: Point3,
    normal: UnitVec3,
) -> Vec<Point3> {
    let (u, v) = normal.orthonormal_basis();
    let (sx, sy) = local_angle(center, u, v, start);
    let radius = sx.hypot(sy);
    let start_angle = sy.atan2(sx);
    let (ex, ey) = local_angle(center, u, v, end);
    let mut sweep = ey.atan2(ex) - start_angle;
    while sweep <= 0.0 {
        sweep += core::f64::consts::TAU;
    }
    (0..=SKETCH3_CURVE_SEGMENTS)
        .map(|i| {
            let t = f64::from(i) / f64::from(SKETCH3_CURVE_SEGMENTS);
            around(center, u, v, radius, radius, start_angle + sweep * t)
        })
        .collect()
}

pub(crate) fn ellipse3_polyline(
    center: Point3,
    major: Point3,
    minor: Point3,
    normal: UnitVec3,
) -> Vec<Point3> {
    let (u, v) = normal.orthonormal_basis();
    let (mu, _) = local_angle(center, u, v, major);
    let (_, mv) = local_angle(center, u, v, minor);
    (0..=SKETCH3_CURVE_SEGMENTS)
        .map(|i| {
            let angle = core::f64::consts::TAU * f64::from(i) / f64::from(SKETCH3_CURVE_SEGMENTS);
            around(center, u, v, mu, mv, angle)
        })
        .collect()
}

pub(crate) fn spline3_polyline(control: &[Point3], closed: bool) -> Vec<Point3> {
    let count = control.len();
    if count < 3 {
        return control.to_vec();
    }
    let lerp = |p: Point3, q: Point3, t: f64| -> Point3 {
        let (px, py, pz) = p.coords_mm();
        let (qx, qy, qz) = q.coords_mm();
        Point3::from_mm(px + (qx - px) * t, py + (qy - py) * t, pz + (qz - pz) * t)
    };
    let blend = |a: Point3, wa: f64, b: Point3, wb: f64, c: Point3, wc: f64| -> Point3 {
        let (ax, ay, az) = a.coords_mm();
        let (bx, by, bz) = b.coords_mm();
        let (cx, cy, cz) = c.coords_mm();
        let total = wa + wb + wc;
        Point3::from_mm(
            ax.mul_add(wa, bx.mul_add(wb, cx * wc)) / total,
            ay.mul_add(wa, by.mul_add(wb, cy * wc)) / total,
            az.mul_add(wa, bz.mul_add(wb, cz * wc)) / total,
        )
    };
    let cubic = |b0: Point3, b1: Point3, b2: Point3, b3: Point3, t: f64| -> Point3 {
        lerp(
            lerp(lerp(b0, b1, t), lerp(b1, b2, t), t),
            lerp(lerp(b1, b2, t), lerp(b2, b3, t), t),
            t,
        )
    };
    let spans = if closed { count } else { count - 1 };
    let prev = |i: usize| {
        if closed {
            control[(i + count - 1) % count]
        } else {
            control[i.saturating_sub(1)]
        }
    };
    let next1 = |i: usize| {
        if closed {
            control[(i + 1) % count]
        } else {
            control[i + 1]
        }
    };
    let next2 = |i: usize| {
        if closed {
            control[(i + 2) % count]
        } else {
            control[(i + 2).min(count - 1)]
        }
    };
    let segment = |i: usize| {
        let b0 = control[i];
        let b1 = blend(prev(i), -1.0 / 6.0, control[i], 1.0, next1(i), 1.0 / 6.0);
        let b2 = blend(control[i], 1.0 / 6.0, next1(i), 1.0, next2(i), -1.0 / 6.0);
        let b3 = next1(i);
        (0..SKETCH3_CURVE_SEGMENTS).map(move |j| {
            cubic(
                b0,
                b1,
                b2,
                b3,
                f64::from(j) / f64::from(SKETCH3_CURVE_SEGMENTS),
            )
        })
    };
    (0..spans)
        .flat_map(segment)
        .chain(std::iter::once(next1(spans - 1)))
        .collect()
}

#[must_use]
pub fn solved(sketch: Sketch3) -> Sketch3 {
    sketch.solve().unwrap_or(sketch)
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::sketch3_project::{embed_line, embed_point};
    use bone_types::{Point2, Tolerance};

    fn xy_plane() -> SketchPlaneBasis {
        let Ok(basis) = SketchPlaneBasis::new(
            Point3::origin(),
            UnitVec3::x_axis(),
            UnitVec3::y_axis(),
            Tolerance::new(1.0e-9),
        ) else {
            panic!("xy plane is orthonormal");
        };
        basis
    }

    #[test]
    fn a_solved_line_yields_an_axis_along_its_direction() {
        let plane = xy_plane();
        let (sketch, start) = embed_point(&Sketch3::new(), plane, Point2::from_mm(0.0, 0.0));
        let (sketch, end) = embed_point(&sketch, plane, Point2::from_mm(5.0, 0.0));
        let (sketch, line) = embed_line(&sketch, start, end, false);
        let solved = solved(sketch);
        let Some(axis) = solved.line_axis(line) else {
            panic!("the placed line yields an axis");
        };
        let (dx, dy, dz) = axis.direction().components();
        assert!(dx.abs() > 1.0 - 1e-6 && dy.abs() < 1e-6 && dz.abs() < 1e-6);
        assert_ne!(start, end);
    }

    #[test]
    fn ray_perp_dist2_rejects_points_behind_the_origin() {
        let ray: Ray = ((0.0, 0.0, 0.0), (0.0, 0.0, 1.0));
        let ahead = ray_perp_dist2(ray, Point3::from_mm(1.0, 0.0, 5.0));
        let behind = ray_perp_dist2(ray, Point3::from_mm(1.0, 0.0, -5.0));
        assert!(
            (ahead - 1.0).abs() < 1e-9,
            "a point in front keeps its true perpendicular distance: {ahead}"
        );
        assert!(
            behind.is_infinite(),
            "a point behind the camera never snaps: {behind}"
        );
    }

    #[test]
    fn circle3_polyline_lies_on_the_radius_in_its_plane() {
        let center = Point3::from_mm(1.0, 2.0, 3.0);
        let points = circle3_polyline(center, 5.0, UnitVec3::z_axis());
        assert!(points.len() >= 8, "the ring is tessellated");
        points.iter().for_each(|p| {
            let (px, py, pz) = (*p - center).coords_mm();
            let radius = px.hypot(py);
            assert!(
                (radius - 5.0).abs() < 1e-6,
                "point sits on the radius: {radius}"
            );
            assert!(pz.abs() < 1e-9, "point stays in the z plane: {pz}");
        });
    }

    #[test]
    fn arc3_polyline_runs_from_start_to_end_on_the_radius() {
        let center = Point3::from_mm(0.0, 0.0, 0.0);
        let start = Point3::from_mm(4.0, 0.0, 0.0);
        let end = Point3::from_mm(0.0, 4.0, 0.0);
        let points = arc3_polyline(center, start, end, UnitVec3::z_axis());
        let (Some(first), Some(last)) = (points.first(), points.last()) else {
            panic!("an arc has points");
        };
        assert!(
            (*first - start).coords_mm().0.abs() < 1e-6
                && (*first - start).coords_mm().1.abs() < 1e-6
        );
        let (lx, ly, _) = (*last - center).coords_mm();
        assert!(
            (lx).abs() < 1e-6 && (ly - 4.0).abs() < 1e-6,
            "arc ends at the end point"
        );
        points.iter().for_each(|p| {
            let (px, py, _) = (*p - center).coords_mm();
            assert!(
                (px.hypot(py) - 4.0).abs() < 1e-6,
                "every sample is on the radius"
            );
        });
    }

    #[test]
    fn an_open_spline_does_not_return_to_its_first_control_point() {
        let control = [
            Point3::from_mm(0.0, 0.0, 0.0),
            Point3::from_mm(10.0, 5.0, 0.0),
            Point3::from_mm(20.0, -5.0, 0.0),
            Point3::from_mm(30.0, 0.0, 0.0),
        ];
        let open = spline3_polyline(&control, false);
        let closed = spline3_polyline(&control, true);
        let (Some(open_first), Some(open_last)) = (open.first(), open.last()) else {
            panic!("an open spline tessellates");
        };
        let span = (*open_last - *open_first).coords_mm();
        let separation = span.0.hypot(span.1).hypot(span.2);
        assert!(
            separation > 1.0,
            "the open spline keeps its ends apart: {separation}"
        );
        assert!(
            closed.len() > open.len(),
            "the closed spline adds the wrap-around span"
        );
        let near = |a: Point3, b: Point3| {
            let (dx, dy, dz) = (a - b).coords_mm();
            dx.hypot(dy).hypot(dz) < 1e-9
        };
        assert!(
            near(*open_first, control[0]),
            "an open spline interpolates its first control point"
        );
        assert!(
            near(*open_last, control[3]),
            "an open spline interpolates its last control point"
        );
        let (Some(closed_first), Some(closed_last)) =
            (closed.first().copied(), closed.last().copied())
        else {
            panic!("a closed spline tessellates");
        };
        assert!(
            near(closed_last, closed_first),
            "a closed spline returns to its start"
        );
    }
}
