// SPDX-License-Identifier: MIT OR Apache-2.0
// Copyright (c) 2025 lacklustr@protonmail.com https://github.com/eadf

use super::*;
use crate::common::sealed::ScalarType;
use std::f32;
use vector_traits::glam::{Vec3, Vec3A};

fn make_triangle<S: ScalarType>(v0: S::Vec3, v1: S::Vec3, v2: S::Vec3) -> Triangle<S> {
    let edge0 = v1 - v0;
    let edge1 = v2 - v0;
    let normal = edge0.cross(edge1).normalize();

    Triangle { v0, v1, v2, normal }
}

#[test]
fn test_empty_bvh() {
    let bvh = StaticBVH::<f32>::new(&[], &[]);
    let result = bvh.closest_point(Vec3::ZERO, Vec3A::ZERO, -1.0);
    assert!(result.is_none());
}

#[test]
fn test_single_triangle() {
    // Triangle in XY plane at z=0
    let tri = make_triangle(
        Vec3::new(0.0, 0.0, 0.0),
        Vec3::new(1.0, 0.0, 0.0),
        Vec3::new(0.0, 1.0, 0.0),
    );

    let bvh = StaticBVH::<f32>::new_from_triangles(vec![tri.clone()]);

    // Point above the triangle should project down to it
    let query = Vec3::new(0.25, 0.25, 1.0);
    let result = bvh.closest_point(query, Vec3A::ZERO, -1.0).unwrap().0;

    // Should be close to (0.25, 0.25, 0.0)
    assert!(
        (result.z).abs() < 0.001,
        "result: {:?} distance a:{}, distance b:{}, distance c:{}",
        result,
        query.distance(tri.v0),
        query.distance(tri.v1),
        query.distance(tri.v2)
    ); // this passes (but should not)
    assert!(
        (result.x - 0.25).abs() < 0.001,
        "result: {:?} distance a:{}, distance b:{}, distance c:{}",
        result,
        query.distance(tri.v0),
        query.distance(tri.v1),
        query.distance(tri.v2)
    ); // this fails with result: Vec3(0.0, 0.0, 0.0)
    assert!(
        (result.y - 0.25).abs() < 0.001,
        "result: {:?} distance a:{}, distance b:{}, distance c:{}",
        result,
        query.distance(tri.v0),
        query.distance(tri.v1),
        query.distance(tri.v2)
    );
}

#[test]
fn test_project_to_triangle_center() {
    let tri = make_triangle(
        Vec3::new(-1.0, -1.0, 0.0),
        Vec3::new(1.0, -1.0, 0.0),
        Vec3::new(0.0, 1.0, 0.0),
    );

    let bvh = StaticBVH::<f32>::new_from_triangles(vec![tri]);

    // Point directly above center
    let query = Vec3::new(0.0, 0.0, 5.0);
    let result = bvh.closest_point(query, Vec3A::ZERO, -1.0).unwrap().0;

    // Should project to z=0
    assert!((result.z).abs() < 0.001);
}

#[test]
fn test_project_to_triangle_vertex() {
    let tri = make_triangle(
        Vec3::new(0.0, 0.0, 0.0),
        Vec3::new(1.0, 0.0, 0.0),
        Vec3::new(0.0, 1.0, 0.0),
    );

    let bvh = StaticBVH::<f32>::new_from_triangles(vec![tri]);

    // Point near a vertex
    let query = Vec3::new(-1.0, -1.0, 0.0);
    let result = bvh.closest_point(query, Vec3A::ZERO, -1.0).unwrap().0;

    // Should snap to origin vertex
    assert!(result.distance(Vec3::ZERO) < 0.001);
}

#[test]
fn test_project_to_triangle_edge() {
    let tri = make_triangle(
        Vec3::new(0.0, 0.0, 0.0),
        Vec3::new(1.0, 0.0, 0.0),
        Vec3::new(0.0, 1.0, 0.0),
    );

    let bvh = StaticBVH::<f32>::new_from_triangles(vec![tri]);

    // Point outside triangle, nearest to an edge
    let query = Vec3::new(0.5, -1.0, 0.0);
    let result = bvh.closest_point(query, Vec3A::ZERO, -1.0).unwrap().0;

    // Should project to the edge between (0,0,0) and (1,0,0)
    assert!((result.y).abs() < 0.001);
    assert!((result.z).abs() < 0.001);
    assert!(result.x >= 0.0 && result.x <= 1.0);
}

#[test]
fn test_multiple_triangles_closest() {
    // Two triangles at different Z heights
    let tri1 = make_triangle(
        Vec3::new(0.0, 0.0, 0.0),
        Vec3::new(1.0, 0.0, 0.0),
        Vec3::new(0.0, 1.0, 0.0),
    );

    let tri2 = make_triangle(
        Vec3::new(0.0, 0.0, 5.0),
        Vec3::new(1.0, 0.0, 5.0),
        Vec3::new(0.0, 1.0, 5.0),
    );

    let bvh = StaticBVH::<f32>::new_from_triangles(vec![tri1, tri2]);

    // Point closer to first triangle
    let query = Vec3::new(0.25, 0.25, 0.5);
    let result = bvh.closest_point(query, Vec3A::ZERO, -1.0).unwrap().0;

    // Should project to tri1 at z=0
    assert!((result.z).abs() < 0.001);

    // Point closer to second triangle
    let query2 = Vec3::new(0.25, 0.25, 4.5);
    let result2 = bvh.closest_point(query2, Vec3A::ZERO, -1.0).unwrap().0;

    // Should project to tri2 at z=5
    assert!((result2.z - 5.0).abs() < 0.001);
}

#[test]
fn test_separated_triangles() {
    // Two triangles far apart
    let tri1 = make_triangle(
        Vec3::new(-10.0, 0.0, 0.0),
        Vec3::new(-9.0, 0.0, 0.0),
        Vec3::new(-10.0, 1.0, 0.0),
    );

    let tri2 = make_triangle(
        Vec3::new(10.0, 0.0, 0.0),
        Vec3::new(11.0, 0.0, 0.0),
        Vec3::new(10.0, 1.0, 0.0),
    );

    let bvh = StaticBVH::<f32>::new_from_triangles(vec![tri1, tri2]);

    // Point near first triangle
    let query = Vec3::new(-10.0, 0.0, 0.0);
    let result = bvh.closest_point(query, Vec3A::ZERO, -1.0).unwrap().0;

    // Should be very close to first triangle
    let dist_to_tri1 = result.distance(Vec3::new(-10.0, 0.0, 0.0));
    assert!(dist_to_tri1 < 0.001);
}

#[test]
fn test_many_triangles() {
    // Create a grid of triangles
    let mut triangles = Vec::new();

    for i in 0..10 {
        for j in 0..10 {
            let x = i as f32;
            let y = j as f32;

            // Two triangles per grid cell
            triangles.push(make_triangle(
                Vec3::new(x, y, 0.0),
                Vec3::new(x + 1.0, y, 0.0),
                Vec3::new(x, y + 1.0, 0.0),
            ));

            triangles.push(make_triangle(
                Vec3::new(x + 1.0, y, 0.0),
                Vec3::new(x + 1.0, y + 1.0, 0.0),
                Vec3::new(x, y + 1.0, 0.0),
            ));
        }
    }

    let bvh = StaticBVH::<f32>::new_from_triangles(triangles);

    // Query point above the grid
    let query = Vec3::new(5.5, 5.5, 10.0);
    let result = bvh.closest_point(query, Vec3A::ZERO, -1.0).unwrap().0;

    // Should project down to z=0
    assert!((result.z).abs() < 0.001);
    assert!((result.x - 5.5).abs() < 0.001);
    assert!((result.y - 5.5).abs() < 0.001);
}

#[test]
fn test_aabb_union() {
    let aabb1 = Aabb::<f32>::new(Vec3::new(0.0, 0.0, 0.0), Vec3::new(1.0, 1.0, 1.0));

    let aabb2 = Aabb::<f32>::new(Vec3::new(0.5, 0.5, 0.5), Vec3::new(2.0, 2.0, 2.0));

    let union = aabb1.union(&aabb2);

    assert_eq!(union.min, Vec3::new(0.0, 0.0, 0.0));
    assert_eq!(union.max, Vec3::new(2.0, 2.0, 2.0));
}

#[test]
fn test_aabb_distance_inside() {
    let aabb = Aabb::<f32>::new(Vec3::new(0.0, 0.0, 0.0), Vec3::new(1.0, 1.0, 1.0));

    // Point inside the Aabb
    let point = Vec3::new(0.5, 0.5, 0.5);
    let dist = aabb.distance_sq(point);

    // Distance should be 0
    assert_eq!(dist, 0.0);
}

#[test]
fn test_aabb_distance_outside() {
    let aabb = Aabb::<f32>::new(Vec3::new(0.0, 0.0, 0.0), Vec3::new(1.0, 1.0, 1.0));

    // Point outside the Aabb
    let point = Vec3::new(2.0, 0.5, 0.5);
    let dist = aabb.distance_sq(point);

    // Distance should be 1.0 (squared distance to closest face)
    assert!((dist - 1.0).abs() < 0.001);
}

#[test]
fn test_projection_preserves_distance_to_plane() {
    // Create a simple triangle in XY plane
    let tri = make_triangle::<f32>(
        Vec3::new(-5.0, -5.0, 0.0),
        Vec3::new(5.0, -5.0, 0.0),
        Vec3::new(0.0, 5.0, 0.0),
    );

    // Test the raw projection function
    let point = Vec3::new(0.0, 0.0, 3.0);
    let projected = project_point_to_triangle::<f32>(point, &tri);

    // Should project straight down to z=0
    assert!((projected.z).abs() < 0.001);
    assert!((projected.x).abs() < 0.001);
    assert!((projected.y).abs() < 0.001);
}

#[test]
fn test_bvh_matches_brute_force() {
    // Create some random triangles
    let triangles = vec![
        make_triangle(
            Vec3::new(0.0, 0.0, 0.0),
            Vec3::new(1.0, 0.0, 0.0),
            Vec3::new(0.0, 1.0, 0.0),
        ),
        make_triangle(
            Vec3::new(2.0, 2.0, 0.0),
            Vec3::new(3.0, 2.0, 0.0),
            Vec3::new(2.0, 3.0, 0.0),
        ),
        make_triangle(
            Vec3::new(-2.0, -2.0, 1.0),
            Vec3::new(-1.0, -2.0, 1.0),
            Vec3::new(-2.0, -1.0, 1.0),
        ),
    ];

    let bvh = StaticBVH::<f32>::new_from_triangles(triangles.clone());

    // Test query point
    let query = Vec3::new(1.0, 1.0, 5.0);

    // BVH result
    let bvh_result = bvh.closest_point(query, Vec3A::ZERO, -1.0).unwrap().0;

    // Brute force result
    let mut brute_force_result = Vec3::ZERO;
    let mut min_dist = f32::MAX;

    for tri in &triangles {
        let proj = project_point_to_triangle(query, tri);
        let dist = query.distance_squared(proj);
        if dist < min_dist {
            min_dist = dist;
            brute_force_result = proj;
        }
    }

    // Results should match
    assert!(bvh_result.distance(brute_force_result) < 0.001);
}
