use bone_types::{Angle, Parameter, Plane3, Point2, Point3, Tolerance, UnitVec3, Vec3};
use uom::si::angle::radian;

use crate::arc2::Arc2;
use crate::arc3::Arc3;
use crate::circle2::Circle2;
use crate::circle3::Circle3;
use crate::circular3;
use crate::curve2::Curve2;
use crate::curve3::{Curve3, Curve3Kind};
use crate::intersect::{IntersectionSet, IntersectionSet2, intersect_curves};
use crate::line2::Line2;
use crate::line3::Line3;

pub type IntersectionSet3 = IntersectionSet<Point3>;

#[must_use]
pub fn intersect_curves_3(
    a: &Curve3Kind,
    b: &Curve3Kind,
    tolerance: Tolerance,
) -> IntersectionSet3 {
    use Curve3Kind::{Arc, Circle, Line};
    match (a, b) {
        (Line(l), Line(m)) => line_line(l, m, tolerance),
        (Line(l), Circle(c)) | (Circle(c), Line(l)) => line_circle(l, c, tolerance),
        (Line(l), Arc(a)) | (Arc(a), Line(l)) => line_arc(l, a, tolerance),
        (Circle(c), Circle(d)) => circle_circle(c, d, tolerance),
        (Circle(c), Arc(a)) | (Arc(a), Circle(c)) => circle_arc(c, a, tolerance),
        (Arc(a), Arc(b)) => arc_arc(a, b, tolerance),
    }
}

#[must_use]
fn unit_to_vec(u: UnitVec3) -> Vec3 {
    let (x, y, z) = u.components();
    Vec3::from_mm(x, y, z)
}

#[must_use]
fn project(plane: Plane3, p: Point3) -> Point2 {
    let (px, py, _) = circular3::local_coords(plane, p);
    Point2::from_mm(px, py)
}

#[must_use]
fn lift(plane: Plane3, p: Point2) -> Point3 {
    let (px, py) = p.coords_mm();
    let (ox, oy, oz) = plane.origin().coords_mm();
    let (xx, xy, xz) = plane.x_axis().components();
    let (yx, yy, yz) = plane.y_axis().components();
    Point3::from_mm(
        ox + px * xx + py * yx,
        oy + px * xy + py * yy,
        oz + px * xz + py * yz,
    )
}

#[must_use]
fn lift_set(plane: Plane3, set: IntersectionSet2) -> IntersectionSet3 {
    match set {
        IntersectionSet::Empty => IntersectionSet3::Empty,
        IntersectionSet::One(p) => IntersectionSet3::One(lift(plane, p)),
        IntersectionSet::Two(a, b) => IntersectionSet3::Two(lift(plane, a), lift(plane, b)),
        IntersectionSet::Coincident => IntersectionSet3::Coincident,
    }
}

#[must_use]
fn keep_points(set: IntersectionSet3, keep: impl Fn(Point3) -> bool) -> IntersectionSet3 {
    match set {
        IntersectionSet3::Empty => IntersectionSet3::Empty,
        IntersectionSet3::Coincident => IntersectionSet3::Coincident,
        IntersectionSet3::One(p) => {
            if keep(p) {
                IntersectionSet3::One(p)
            } else {
                IntersectionSet3::Empty
            }
        }
        IntersectionSet3::Two(a, b) => match (keep(a), keep(b)) {
            (true, true) => IntersectionSet3::Two(a, b),
            (true, false) => IntersectionSet3::One(a),
            (false, true) => IntersectionSet3::One(b),
            (false, false) => IntersectionSet3::Empty,
        },
    }
}

fn line_line(a: &Line3, b: &Line3, tolerance: Tolerance) -> IntersectionSet3 {
    let (pa, pb) = (a.start(), b.start());
    let dir_a = a.end() - a.start();
    let dir_b = b.end() - b.start();
    let eps = tolerance.value();
    let (la, lb) = (a.length_mm(), b.length_mm());
    let aa = dir_a.dot_mm2(dir_a);
    let ab = dir_a.dot_mm2(dir_b);
    let bb = dir_b.dot_mm2(dir_b);
    let offset = pa - pb;
    let dot_a = dir_a.dot_mm2(offset);
    let dot_b = dir_b.dot_mm2(offset);
    let denom = aa * bb - ab * ab;

    if dir_a.cross(dir_b).norm_mm() <= eps * la.min(lb) {
        let between = pb - pa;
        let along = between.dot_mm2(dir_a) / aa;
        if (between - dir_a * along).norm_mm() > eps {
            return IntersectionSet3::Empty;
        }
        let other_end = (b.end() - pa).dot_mm2(dir_a) / aa;
        let (lo, hi) = if along <= other_end {
            (along, other_end)
        } else {
            (other_end, along)
        };
        let overlap_lo = lo.max(0.0);
        let overlap_hi = hi.min(1.0);
        let eps_param = eps / la;
        if overlap_hi < overlap_lo - eps_param {
            return IntersectionSet3::Empty;
        }
        if overlap_hi <= overlap_lo + eps_param {
            return IntersectionSet3::One(pa + dir_a * f64::midpoint(overlap_lo, overlap_hi));
        }
        return IntersectionSet3::Coincident;
    }

    let param_a = (ab * dot_b - bb * dot_a) / denom;
    let param_b = (aa * dot_b - ab * dot_a) / denom;
    let (eps_a, eps_b) = (eps / la, eps / lb);
    if param_a < -eps_a || param_a > 1.0 + eps_a || param_b < -eps_b || param_b > 1.0 + eps_b {
        return IntersectionSet3::Empty;
    }
    let on_a = pa + dir_a * param_a;
    let on_b = pb + dir_b * param_b;
    if (on_b - on_a).norm_mm() <= eps {
        IntersectionSet3::One(on_a + (on_b - on_a) * 0.5)
    } else {
        IntersectionSet3::Empty
    }
}

fn line_circle(line: &Line3, circle: &Circle3, tolerance: Tolerance) -> IntersectionSet3 {
    let plane = circle.plane();
    let normal = unit_to_vec(plane.normal());
    let u = line.end() - line.start();
    let eps = tolerance.value();
    let r = circle.radius_mm();
    let un = u.dot_mm2(normal);
    let dist_start = (line.start() - plane.origin()).dot_mm2(normal);

    if un.abs() <= eps {
        if dist_start.abs() > eps {
            return IntersectionSet3::Empty;
        }
        let (Ok(l2), Ok(c2)) = (
            Line2::new(
                project(plane, line.start()),
                project(plane, line.end()),
                tolerance,
            ),
            Circle2::new(Point2::origin(), circle.radius(), tolerance),
        ) else {
            return IntersectionSet3::Empty;
        };
        return lift_set(
            plane,
            intersect_curves(&l2.as_kind(), &c2.as_kind(), tolerance),
        );
    }

    let s = -dist_start / un;
    let eps_s = eps / line.length_mm();
    if s < -eps_s || s > 1.0 + eps_s {
        return IntersectionSet3::Empty;
    }
    let x = line.start() + u * s;
    let radial = (x - plane.origin()).norm_mm();
    if (radial - r).abs() <= eps {
        IntersectionSet3::One(x)
    } else {
        IntersectionSet3::Empty
    }
}

fn line_arc(line: &Line3, arc: &Arc3, tolerance: Tolerance) -> IntersectionSet3 {
    let set = line_circle(line, &arc.as_full_circle(), tolerance);
    keep_points(set, |p| arc.contains_point(p, tolerance))
}

fn circle_circle(a: &Circle3, b: &Circle3, tolerance: Tolerance) -> IntersectionSet3 {
    let (pa, pb) = (a.plane(), b.plane());
    let na = unit_to_vec(pa.normal());
    let nb = unit_to_vec(pb.normal());
    let eps = tolerance.value();
    let cross = na.cross(nb);

    if cross.norm_mm() <= eps {
        if (pb.origin() - pa.origin()).dot_mm2(na).abs() > eps {
            return IntersectionSet3::Empty;
        }
        let (Ok(a2), Ok(b2)) = (
            Circle2::new(Point2::origin(), a.radius(), tolerance),
            Circle2::new(project(pa, pb.origin()), b.radius(), tolerance),
        ) else {
            return IntersectionSet3::Empty;
        };
        return lift_set(
            pa,
            intersect_curves(&a2.as_kind(), &b2.as_kind(), tolerance),
        );
    }

    let axis_u = unit_to_vec(pa.x_axis());
    let axis_v = unit_to_vec(pa.y_axis());
    let ra = a.radius_mm();
    let coef_cos = ra * nb.dot_mm2(axis_u);
    let coef_sin = ra * nb.dot_mm2(axis_v);
    let rhs = nb.dot_mm2(b.center() - a.center());
    let amp = (coef_cos * coef_cos + coef_sin * coef_sin).sqrt();
    if rhs.abs() > amp + eps {
        return IntersectionSet3::Empty;
    }
    let phi = coef_sin.atan2(coef_cos);
    let delta = (rhs / amp).clamp(-1.0, 1.0).acos();
    let on_b =
        |candidate: Point3| ((candidate - b.center()).norm_mm() - b.radius_mm()).abs() <= eps;
    let point_at = |t: f64| a.center() + axis_u * (ra * t.cos()) + axis_v * (ra * t.sin());
    let mut hits = [phi - delta, phi + delta]
        .into_iter()
        .map(point_at)
        .filter(|&candidate| on_b(candidate));
    match (hits.next(), hits.next()) {
        (None, _) => IntersectionSet3::Empty,
        (Some(p), None) => IntersectionSet3::One(p),
        (Some(p), Some(q)) => {
            if (q - p).norm_mm() <= eps {
                IntersectionSet3::One(p)
            } else {
                IntersectionSet3::Two(p, q)
            }
        }
    }
}

fn circle_arc(circle: &Circle3, arc: &Arc3, tolerance: Tolerance) -> IntersectionSet3 {
    let set = circle_circle(circle, &arc.as_full_circle(), tolerance);
    keep_points(set, |p| arc.contains_point(p, tolerance))
}

fn arc_arc(a: &Arc3, b: &Arc3, tolerance: Tolerance) -> IntersectionSet3 {
    match circle_circle(&a.as_full_circle(), &b.as_full_circle(), tolerance) {
        IntersectionSet3::Coincident => coincident_arc_arc(a, b, tolerance),
        other => keep_points(other, |p| {
            a.contains_point(p, tolerance) && b.contains_point(p, tolerance)
        }),
    }
}

fn coincident_arc_arc(a: &Arc3, b: &Arc3, tolerance: Tolerance) -> IntersectionSet3 {
    let plane = a.plane();
    let start_b = b.evaluate(Parameter::new(0.0));
    let (bx, by, _) = circular3::local_coords(plane, start_b);
    let alpha0 = by.atan2(bx);
    let winding = unit_to_vec(plane.normal()).dot_mm2(unit_to_vec(b.plane().normal()));
    let sweep_b = b.sweep_rad() * if winding >= 0.0 { 1.0 } else { -1.0 };

    let (Ok(a2), Ok(b2)) = (
        Arc2::new(
            Point2::origin(),
            a.radius(),
            a.start_angle(),
            a.sweep_angle(),
            tolerance,
        ),
        Arc2::new(
            Point2::origin(),
            a.radius(),
            Angle::new::<radian>(alpha0),
            Angle::new::<radian>(sweep_b),
            tolerance,
        ),
    ) else {
        return IntersectionSet3::Coincident;
    };
    lift_set(
        plane,
        intersect_curves(&a2.as_kind(), &b2.as_kind(), tolerance),
    )
}
