#![deny(missing_docs)]

//! This crate contains a convex collider, which implements the collide trait from the `collide` crate.

use collide::{Collider, CollisionInfo, Transform, Transformable};
use inner_space::{InnerSpace, VectorSpace};
use scalars::{Sqrt, Zero};

/// Machine epsilon for floating-point types.
pub trait Epsilon {
    /// Returns the machine epsilon for the type.
    fn epsilon() -> Self;
}

impl Epsilon for f32 {
    fn epsilon() -> Self {
        f32::EPSILON
    }
}

impl Epsilon for f64 {
    fn epsilon() -> Self {
        f64::EPSILON
    }
}

/// The convex collider defined as a convex hull around multiple spheres having the same radius.
#[derive(Clone)]
pub struct Convex<V: VectorSpace> {
    /// The radius of the spheres.
    pub rad: V::Scalar,
    /// The positions of the spheres.
    pub points: Box<[V]>,
}

impl<V: VectorSpace> Convex<V> {
    /// Creates a new convex collider.
    pub fn new(rad: V::Scalar, points: Box<[V]>) -> Self {
        Self { rad, points }
    }

    /// Creates a new convex collider representing a capsule.
    pub fn capsule(rad: V::Scalar, start: V, end: V) -> Self {
        Self {
            rad,
            points: [start, end].into(),
        }
    }

    /// Creates a new convex collider representing a point.
    pub fn point(pos: V) -> Self {
        Self {
            rad: V::Scalar::zero(),
            points: [pos].into(),
        }
    }

    /// Creates a new convex collider representing a line.
    pub fn line(start: V, end: V) -> Self {
        Self {
            rad: V::Scalar::zero(),
            points: [start, end].into(),
        }
    }

    /// Creates a new convex collider representing a sphere.
    pub fn sphere(pos: V, rad: V::Scalar) -> Self {
        Self {
            rad,
            points: [pos].into(),
        }
    }
}

pub(crate) fn origin_offset<V: Copy + InnerSpace>(vertices: &[V]) -> V {
    let Some(&first) = vertices.first() else {
        return V::zero();
    };

    let mut ortho: Vec<V> = Vec::new();
    for &vertex in &vertices[1..] {
        let mut edge = vertex - first;
        for basis in &ortho {
            let mag2 = basis.magnitude2();
            if !mag2.is_zero() {
                edge = edge - *basis * (edge.dot(basis) / mag2);
            }
        }
        if !edge.is_zero() {
            ortho.push(edge);
        }
    }

    let mut result = first;
    for basis in &ortho {
        let mag2 = basis.magnitude2();
        if !mag2.is_zero() {
            result = result - *basis * (result.dot(basis) / mag2);
        }
    }
    result
}

fn is_projection_inside<V: Copy + InnerSpace>(offset: V, simplex: &[V]) -> bool {
    if simplex.len() <= 1 {
        return true;
    }
    let n = simplex.len();
    for i in 0..n {
        let face: Vec<V> = (0..n).filter(|&j| j != i).map(|j| simplex[j]).collect();
        let face_offset = origin_offset(&face);
        let toward_vertex = simplex[i] - face_offset;
        let toward_projection = offset - face_offset;
        if toward_vertex.dot(&toward_projection) < V::Scalar::zero() {
            return false;
        }
    }
    true
}

pub(crate) fn closest_subsimplex<V: Copy + InnerSpace>(points: &[V]) -> (Vec<V>, V) {
    let n = points.len();
    let last_bit = 1u32 << (n - 1);
    let mut best: Option<(u32, V, V::Scalar)> = None;

    for mask in 1u32..(1u32 << n) {
        if mask & last_bit == 0 {
            continue;
        }

        let subset: Vec<V> = (0..n)
            .filter(|&i| mask & (1 << i) != 0)
            .map(|i| points[i])
            .collect();

        let offset = origin_offset(&subset);

        if !is_projection_inside(offset, &subset) {
            continue;
        }

        let mag2 = offset.magnitude2();
        if best.as_ref().is_some_and(|(_, _, m)| mag2 >= *m) {
            continue;
        }

        best = Some((mask, offset, mag2));
    }

    let (mask, offset, _) =
        best.unwrap_or_else(|| (last_bit, points[n - 1], points[n - 1].magnitude2()));
    let subset = (0..n)
        .filter(|&i| mask & (1 << i) != 0)
        .map(|i| points[i])
        .collect();
    (subset, offset)
}

fn epa<V: Copy + InnerSpace>(initial_simplex: &[V], support: impl Fn(V) -> V) -> V
where
    V::Scalar: Epsilon,
{
    if initial_simplex.len() <= 2 {
        return epa_closest_initial_face(initial_simplex);
    }

    let mut vertices: Vec<V> = initial_simplex.to_vec();
    let vertex_count = vertices.len();

    let mut faces: Vec<Vec<usize>> = (0..vertex_count)
        .map(|skip| (0..vertex_count).filter(|&i| i != skip).collect())
        .collect();

    let mut result = epa_closest_initial_face(initial_simplex);

    for _ in 0..64 {
        let Some((_, offset, distance)) = epa_find_closest_face(&vertices, &faces) else {
            break;
        };

        if distance.is_zero() {
            continue;
        }

        result = offset;

        let new_point = support(offset);

        let new_projection = new_point.dot(&offset);
        let tolerance = V::Scalar::epsilon().sqrt();
        if new_projection - distance <= tolerance * distance {
            break;
        }

        let eps = V::Scalar::epsilon();
        if vertices
            .iter()
            .any(|&vertex| (vertex - new_point).magnitude2() <= eps)
        {
            break;
        }

        let new_index = vertices.len();
        vertices.push(new_point);

        let visible: Vec<bool> = faces
            .iter()
            .map(|face| {
                let points: Vec<V> = face.iter().map(|&i| vertices[i]).collect();
                let face_offset = origin_offset(&points);
                if face_offset.magnitude2().is_zero() {
                    return false;
                }
                (new_point - vertices[face[0]]).dot(&face_offset) > V::Scalar::zero()
            })
            .collect();

        let mut ridges: Vec<Vec<usize>> = Vec::new();
        for (face_index, face) in faces.iter().enumerate() {
            if !visible[face_index] {
                continue;
            }
            for skip in 0..face.len() {
                let mut ridge: Vec<usize> = face
                    .iter()
                    .enumerate()
                    .filter(|&(j, _)| j != skip)
                    .map(|(_, &vertex)| vertex)
                    .collect();
                ridge.sort_unstable();
                ridges.push(ridge);
            }
        }

        let horizon: Vec<Vec<usize>> = ridges
            .iter()
            .filter(|ridge| ridges.iter().filter(|other| other == ridge).count() == 1)
            .cloned()
            .collect();

        faces = faces
            .into_iter()
            .enumerate()
            .filter(|(index, _)| !visible[*index])
            .map(|(_, face)| face)
            .collect();

        for ridge in horizon {
            let mut face = ridge;
            face.push(new_index);
            faces.push(face);
        }

        if faces.is_empty() {
            break;
        }
    }

    result
}

fn epa_find_closest_face<V: Copy + InnerSpace>(
    vertices: &[V],
    faces: &[Vec<usize>],
) -> Option<(usize, V, V::Scalar)> {
    let mut best: Option<(usize, V, V::Scalar)> = None;

    for (index, face) in faces.iter().enumerate() {
        let points: Vec<V> = face.iter().map(|&i| vertices[i]).collect();
        let offset = origin_offset(&points);

        if !is_projection_inside(offset, &points) {
            continue;
        }

        let distance = offset.magnitude2();
        if distance.is_zero() {
            continue;
        }
        if best.as_ref().is_some_and(|(_, _, d)| distance >= *d) {
            continue;
        }

        best = Some((index, offset, distance));
    }

    best
}

fn epa_closest_initial_face<V: Copy + InnerSpace>(simplex: &[V]) -> V {
    let mut best: Option<(V::Scalar, V)> = None;
    for skip in 0..simplex.len() {
        let face: Vec<V> = (0..simplex.len())
            .filter(|&j| j != skip)
            .map(|j| simplex[j])
            .collect();
        if face.is_empty() {
            continue;
        }
        let offset = origin_offset(&face);
        let magnitude = offset.magnitude2();
        if magnitude.is_zero() {
            continue;
        }
        if best.as_ref().is_some_and(|(m, _)| magnitude >= *m) {
            continue;
        }
        best = Some((magnitude, offset));
    }
    best.map_or_else(V::zero, |(_, offset)| offset)
}

enum Side {
    Inside,
    Outside,
}

impl<V: Copy + InnerSpace> Convex<V>
where
    V::Scalar: Epsilon,
{
    fn support(&self, other: &Self, dir: V) -> V {
        let mut found: Option<V> = None;
        for &point in &self.points {
            for &other_point in &other.points {
                let diff = other_point - point;
                if found.is_some_and(|found| diff.dot(&dir) <= found.dot(&dir)) {
                    continue;
                }
                found = Some(diff)
            }
        }

        found.unwrap_or_else(V::zero)
    }

    fn offset(&self, other: &Self) -> (V, Side) {
        let isd = other.points[0] - self.points[0];
        if isd.is_zero() {
            return (V::zero(), Side::Outside);
        }
        let initial = self.support(other, isd);
        let mut simplex = vec![initial];
        let mut dir = -initial;

        for _ in 0..64 {
            let new_point = self.support(other, dir);

            if simplex.contains(&new_point) {
                break;
            }

            simplex.push(new_point);
            let (sub, offset) = closest_subsimplex(&simplex);

            let inside = if offset.is_zero() {
                true
            } else if sub.len() == simplex.len() {
                let scale = simplex
                    .iter()
                    .copied()
                    .map(|v| v.magnitude2())
                    .fold(V::Scalar::zero(), |a, b| if a > b { a } else { b });
                let eps = V::Scalar::epsilon();
                offset.magnitude2() <= scale * eps * eps
            } else {
                false
            };
            if inside {
                let mut expanded = sub.clone();
                'expand: for &self_point in self.points.iter() {
                    for &other_point in other.points.iter() {
                        let diff = other_point - self_point;
                        if expanded.contains(&diff) {
                            continue;
                        }
                        expanded.push(diff);
                        let (new_sub, new_offset) = closest_subsimplex(&expanded);
                        if new_offset.is_zero() {
                            expanded = new_sub;
                            if expanded.len() > 2 {
                                break 'expand;
                            }
                        } else {
                            expanded.pop();
                        }
                    }
                }
                let result = epa(&expanded, |direction| self.support(other, direction));
                return (result, Side::Inside);
            }

            simplex = sub;
            dir = -offset;
        }

        (origin_offset(&simplex), Side::Outside)
    }
}

impl<V: Copy + InnerSpace, T: Transform<V>> Transformable<T> for Convex<V> {
    fn transformed(&self, transform: &T) -> Self {
        Self {
            rad: self.rad,
            points: self
                .points
                .iter()
                .map(|&p| transform.apply_point(p))
                .collect(),
        }
    }
}

#[cfg(feature = "sphere")]
mod bounding_sphere;

#[cfg(feature = "ray")]
mod ray;

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

    fn collision_info(&self, other: &Self) -> Option<CollisionInfo<Self::Vector>> {
        let (offset, side) = self.offset(other);
        let mag2 = offset.magnitude2();
        let rad = self.rad + other.rad;
        match side {
            Side::Inside => {
                if mag2.is_zero() {
                    return Some(CollisionInfo {
                        self_contact: V::zero(),
                        other_contact: V::zero(),
                        vector: V::zero(),
                    });
                }
                let mag = mag2.sqrt();
                let dir = offset / mag;
                Some(CollisionInfo {
                    self_contact: V::zero(),
                    other_contact: V::zero(),
                    vector: dir * (mag + rad),
                })
            }
            Side::Outside => {
                if mag2 > rad * rad {
                    return None;
                }
                if mag2.is_zero() {
                    return Some(CollisionInfo {
                        self_contact: V::zero(),
                        other_contact: V::zero(),
                        vector: V::zero(),
                    });
                }
                let mag = mag2.sqrt();
                let dir = offset / mag;
                Some(CollisionInfo {
                    self_contact: V::zero(),
                    other_contact: V::zero(),
                    vector: dir * (mag - rad),
                })
            }
        }
    }
}
