#![deny(missing_docs)]

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

use collide::{Collider, CollisionInfo};
use inner_space::{InnerSpace, VectorSpace};
use num_traits::{Zero, real::Real};

/// 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(),
        }
    }
}

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
}

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)
}

enum Side {
    Inside,
    Outside,
}

impl<V: Copy + InnerSpace> Convex<V> {
    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 best = None;
                for i in 0..simplex.len() {
                    let face: Vec<V> = (0..simplex.len())
                        .filter(|&j| j != i)
                        .map(|j| simplex[j])
                        .collect();
                    if face.is_empty() {
                        continue;
                    }
                    let off = origin_offset(&face);
                    let mag2 = off.magnitude2();
                    if mag2.is_zero() {
                        continue;
                    }
                    if best
                        .as_ref()
                        .is_some_and(|(m, _): &(V::Scalar, V)| mag2 >= *m)
                    {
                        continue;
                    }
                    best = Some((mag2, off));
                }
                return (best.map_or_else(V::zero, |(_, o)| o), Side::Inside);
            }

            simplex = sub;
            dir = -offset;
        }

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

impl<V: Copy + InnerSpace> Collider for Convex<V> {
    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),
                })
            }
        }
    }
}
