use bevy::prelude::*;

/// Ray-sphere intersection. Returns the smallest positive `t` where
/// `ray_origin + ray_direction * t` lies on the sphere's surface, or `None`.
///
/// `ray_direction` should be normalized.
pub fn ray_vs_sphere(
    ray_origin: Vec3,
    ray_direction: Vec3,
    sphere_center: Vec3,
    sphere_radius: f32,
) -> Option<f32> {
    let to_sphere = sphere_center - ray_origin;
    let proj = to_sphere.dot(ray_direction);
    if proj < 0.0 {
        return None; // sphere is behind the ray origin
    }
    let closest = ray_origin + ray_direction * proj;
    let dist_sq = (closest - sphere_center).length_squared();
    let r_sq = sphere_radius * sphere_radius;
    if dist_sq > r_sq {
        return None; // ray misses the sphere
    }
    let offset = (r_sq - dist_sq).sqrt();
    Some(proj - offset)
}

/// Check if two spheres overlap. Use length-squared for performance (no sqrt).
pub fn overlaps(a_center: Vec3, a_radius: f32, b_center: Vec3, b_radius: f32) -> bool {
    let combined = a_radius + b_radius;
    (a_center - b_center).length_squared() < combined * combined
}

/// Compute the vector to push `a` out of `b` so they no longer overlap.
/// Returns `None` if they don't overlap, or if their centers coincide (ambiguous direction).
pub fn separate(a_center: Vec3, a_radius: f32, b_center: Vec3, b_radius: f32) -> Option<Vec3> {
    let delta = a_center - b_center;
    let combined = a_radius + b_radius;
    let dist = delta.length();
    if dist >= combined || dist == 0.0 {
        return None;
    }
    Some(delta / dist * (combined - dist))
}

/// Segment (finite line from `seg_start` to `seg_end`) vs sphere intersection.
/// Returns `t` in `[0, segment_length]` measured from `seg_start`, or `None`.
///
/// Useful for projectile hit detection (treat projectile last-frame → current-frame
/// positions as the segment to avoid tunneling at high speeds).
pub fn segment_vs_sphere(
    seg_start: Vec3,
    seg_end: Vec3,
    sphere_center: Vec3,
    sphere_radius: f32,
) -> Option<f32> {
    let seg = seg_end - seg_start;
    let seg_len = seg.length();
    if seg_len == 0.0 {
        return if (seg_start - sphere_center).length_squared() < sphere_radius * sphere_radius {
            Some(0.0)
        } else {
            None
        };
    }
    let dir = seg / seg_len;
    let t = ray_vs_sphere(seg_start, dir, sphere_center, sphere_radius)?;
    if t > seg_len {
        return None;
    }
    Some(t)
}

/// Sphere-sphere overlap test. Returns `(penetration, normal)` where `normal`
/// is the unit vector pointing from `a` toward `b`, or `None` if they do not
/// overlap or their centers coincide (ambiguous direction).
///
/// `penetration` is always positive and equals the distance the two spheres
/// must be separated along `normal` to just stop overlapping.
pub fn sphere_overlap(
    a_pos: Vec3,
    a_radius: f32,
    b_pos: Vec3,
    b_radius: f32,
) -> Option<(f32, Vec3)> {
    let delta = b_pos - a_pos;
    let dist_sq = delta.length_squared();
    let combined = a_radius + b_radius;
    if dist_sq >= combined * combined || dist_sq == 0.0 {
        return None;
    }
    let dist = dist_sq.sqrt();
    Some((combined - dist, delta / dist))
}

/// Split a positional correction between two bodies by inverse mass.
///
/// `normal` points from body `a` toward body `b`; `penetration` is the overlap
/// depth. Returns `(push_a, push_b)` where `push_a` moves `a` away from `b`
/// and `push_b` moves `b` away from `a`. `inv_a` / `inv_b` are inverse masses
/// — pass `0.0` for an immovable body so it absorbs no correction.
pub fn split_by_inverse_mass(
    normal: Vec3,
    penetration: f32,
    inv_a: f32,
    inv_b: f32,
) -> (Vec3, Vec3) {
    let inv_sum = inv_a + inv_b;
    if inv_sum <= 0.0 {
        return (Vec3::ZERO, Vec3::ZERO);
    }
    let push_a = -normal * penetration * (inv_a / inv_sum);
    let push_b = normal * penetration * (inv_b / inv_sum);
    (push_a, push_b)
}

// ── Tests ───────────────────────────────────────────────────────────────────

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn ray_hits_sphere_directly() {
        let t = ray_vs_sphere(
            Vec3::new(0.0, 0.0, -5.0),
            Vec3::new(0.0, 0.0, 1.0),
            Vec3::ZERO,
            1.0,
        );
        assert_eq!(t, Some(4.0));
    }

    #[test]
    fn ray_misses_sphere() {
        let t = ray_vs_sphere(
            Vec3::new(10.0, 0.0, -5.0),
            Vec3::new(0.0, 0.0, 1.0),
            Vec3::ZERO,
            1.0,
        );
        assert_eq!(t, None);
    }

    #[test]
    fn ray_with_sphere_behind_returns_none() {
        let t = ray_vs_sphere(
            Vec3::new(0.0, 0.0, 5.0),
            Vec3::new(0.0, 0.0, 1.0),
            Vec3::ZERO,
            1.0,
        );
        assert_eq!(t, None);
    }

    #[test]
    fn overlapping_spheres_detected() {
        assert!(overlaps(Vec3::ZERO, 1.0, Vec3::new(1.5, 0.0, 0.0), 1.0));
        assert!(!overlaps(Vec3::ZERO, 1.0, Vec3::new(3.0, 0.0, 0.0), 1.0));
    }

    #[test]
    fn separate_pushes_a_out_of_b() {
        let sep = separate(Vec3::new(0.5, 0.0, 0.0), 1.0, Vec3::ZERO, 1.0);
        // Combined radius 2.0, current distance 0.5, so push by 1.5 along +X.
        assert_eq!(sep, Some(Vec3::new(1.5, 0.0, 0.0)));
    }

    #[test]
    fn segment_hits_sphere() {
        let t = segment_vs_sphere(
            Vec3::new(0.0, 0.0, -5.0),
            Vec3::new(0.0, 0.0, 5.0),
            Vec3::ZERO,
            1.0,
        );
        assert_eq!(t, Some(4.0));
    }

    #[test]
    fn segment_misses_when_sphere_off_line() {
        let t = segment_vs_sphere(
            Vec3::new(0.0, 10.0, -5.0),
            Vec3::new(0.0, 10.0, 5.0),
            Vec3::ZERO,
            1.0,
        );
        assert_eq!(t, None);
    }

    #[test]
    fn sphere_overlap_reports_penetration_and_normal() {
        let (pen, normal) =
            sphere_overlap(Vec3::ZERO, 1.0, Vec3::new(1.0, 0.0, 0.0), 1.0).unwrap();
        assert!((pen - 1.0).abs() < 1e-5);
        assert!((normal - Vec3::new(1.0, 0.0, 0.0)).length() < 1e-5);
    }

    #[test]
    fn sphere_overlap_none_when_apart() {
        assert!(sphere_overlap(Vec3::ZERO, 1.0, Vec3::new(5.0, 0.0, 0.0), 1.0).is_none());
    }

    #[test]
    fn sphere_overlap_none_when_coincident() {
        assert!(sphere_overlap(Vec3::ZERO, 1.0, Vec3::ZERO, 1.0).is_none());
    }

    #[test]
    fn split_gives_full_correction_to_movable_body() {
        // normal a→b = +X, penetration 2.0, b immovable (inv_b = 0).
        let (push_a, push_b) = split_by_inverse_mass(Vec3::new(1.0, 0.0, 0.0), 2.0, 1.0, 0.0);
        assert!((push_a - Vec3::new(-2.0, 0.0, 0.0)).length() < 1e-5);
        assert!(push_b.length() < 1e-5);
    }

    #[test]
    fn split_halves_correction_for_equal_mass() {
        let (push_a, push_b) = split_by_inverse_mass(Vec3::new(1.0, 0.0, 0.0), 2.0, 1.0, 1.0);
        assert!((push_a - Vec3::new(-1.0, 0.0, 0.0)).length() < 1e-5);
        assert!((push_b - Vec3::new(1.0, 0.0, 0.0)).length() < 1e-5);
    }

    #[test]
    fn split_is_zero_when_both_immovable() {
        let (push_a, push_b) = split_by_inverse_mass(Vec3::new(1.0, 0.0, 0.0), 2.0, 0.0, 0.0);
        assert!(push_a.length() < 1e-5);
        assert!(push_b.length() < 1e-5);
    }
}
