use collide::{Collider, CollisionInfo};
use collide_ray::Ray;
use inner_space::InnerSpace;
use scalars::Zero;

use crate::{Convex, Epsilon, closest_subsimplex};

fn hull_support<V: Copy + InnerSpace>(points: &[V], direction: V) -> V {
    let mut best = points[0];
    let mut best_dot = best.dot(&direction);
    for &point in &points[1..] {
        let dot = point.dot(&direction);
        if dot > best_dot {
            best_dot = dot;
            best = point;
        }
    }
    best
}

fn expanded_support<V: Copy + InnerSpace>(points: &[V], radius: V::Scalar, direction: V) -> V {
    let hull = hull_support(points, direction);
    let magnitude = direction.magnitude();
    if magnitude.is_zero() {
        hull
    } else {
        hull + direction * (radius / magnitude)
    }
}

impl<V: Copy + InnerSpace> Collider<Ray<V>> for Convex<V>
where
    V::Scalar: Epsilon,
{
    type Vector = V;

    fn collision_info(&self, ray: &Ray<V>) -> Option<CollisionInfo<V>> {
        if self.points.is_empty() {
            return None;
        }

        let zero = V::Scalar::zero();
        let epsilon = V::Scalar::epsilon();

        let mut lambda = zero;
        let mut position = ray.origin;
        let mut normal = V::zero();

        let initial_direction = position - self.points[0];
        if initial_direction.is_zero() {
            return Some(CollisionInfo {
                self_contact: position,
                other_contact: position,
                vector: V::zero(),
            });
        }

        let first_support = expanded_support(&self.points, self.rad, initial_direction);
        let mut search = position - first_support;
        let mut simplex: Vec<V> = vec![first_support];

        for _ in 0..64 {
            if search.magnitude2() <= epsilon {
                break;
            }

            let support = expanded_support(&self.points, self.rad, search);
            let witness = position - support;

            if search.dot(&witness) > zero {
                let along_ray = search.dot(&ray.direction);
                if along_ray >= zero {
                    return None;
                }
                lambda = lambda - search.dot(&witness) / along_ray;
                position = ray.origin + ray.direction * lambda;
                normal = search;
                simplex = vec![support];
            } else {
                simplex.push(support);
            }

            let translated: Vec<V> = simplex.iter().map(|&s| s - position).collect();
            let (sub, offset) = closest_subsimplex(&translated);
            simplex = sub.into_iter().map(|s| s + position).collect();
            search = V::zero() - offset;
        }

        let hit_point = ray.origin + ray.direction * lambda;
        let normal_magnitude = normal.magnitude();

        if normal_magnitude <= epsilon {
            return Some(CollisionInfo {
                self_contact: hit_point,
                other_contact: hit_point,
                vector: V::zero(),
            });
        }

        let surface_direction = normal / normal_magnitude;
        Some(CollisionInfo {
            self_contact: hit_point + surface_direction * self.rad,
            other_contact: hit_point,
            vector: V::zero() - ray.direction * lambda,
        })
    }
}

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

    type Vec3 = Vector<f32, 3>;

    #[test]
    fn ray_hits_sphere() {
        let ray = Ray::new(Vec3::from([-5.0, 0.0, 0.0]), Vec3::from([1.0, 0.0, 0.0]));
        let convex = Convex::sphere(Vec3::from([0.0, 0.0, 0.0]), 1.0);

        let info = convex.collision_info(&ray).unwrap();
        let distance = info.vector.magnitude();
        assert!((distance - 4.0).abs() < 0.01);
    }

    #[test]
    fn ray_misses_sphere() {
        let ray = Ray::new(Vec3::from([-5.0, 0.0, 0.0]), Vec3::from([0.0, 1.0, 0.0]));
        let convex = Convex::sphere(Vec3::from([0.0, 0.0, 0.0]), 1.0);

        assert!(convex.collision_info(&ray).is_none());
    }

    #[test]
    fn ray_hits_triangle() {
        let ray = Ray::new(Vec3::from([0.0, 0.0, 5.0]), Vec3::from([0.0, 0.0, -1.0]));
        let convex = Convex::new(
            0.1,
            vec![
                Vec3::from([-1.0, -1.0, 0.0]),
                Vec3::from([1.0, -1.0, 0.0]),
                Vec3::from([0.0, 1.0, 0.0]),
            ]
            .into(),
        );

        let info = convex.collision_info(&ray).unwrap();
        let distance = info.vector.magnitude();
        assert!((distance - 4.9).abs() < 0.1);
    }

    #[test]
    fn ray_misses_triangle() {
        let ray = Ray::new(Vec3::from([5.0, 5.0, 5.0]), Vec3::from([0.0, 0.0, -1.0]));
        let convex = Convex::new(
            0.1,
            vec![
                Vec3::from([-1.0, -1.0, 0.0]),
                Vec3::from([1.0, -1.0, 0.0]),
                Vec3::from([0.0, 1.0, 0.0]),
            ]
            .into(),
        );

        assert!(convex.collision_info(&ray).is_none());
    }

    #[test]
    fn ray_behind_origin_misses() {
        let ray = Ray::new(Vec3::from([5.0, 0.0, 0.0]), Vec3::from([1.0, 0.0, 0.0]));
        let convex = Convex::sphere(Vec3::from([0.0, 0.0, 0.0]), 1.0);

        assert!(convex.collision_info(&ray).is_none());
    }

    #[test]
    fn ray_hits_capsule_shape() {
        let ray = Ray::new(Vec3::from([0.0, 5.0, 0.0]), Vec3::from([0.0, -1.0, 0.0]));
        let convex = Convex::capsule(
            1.0,
            Vec3::from([-2.0, 0.0, 0.0]),
            Vec3::from([2.0, 0.0, 0.0]),
        );

        let info = convex.collision_info(&ray).unwrap();
        let distance = info.vector.magnitude();
        assert!((distance - 4.0).abs() < 0.1);
    }
}
