#![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::{One, 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 point_origin_offset<V: Copy + InnerSpace>(point: V, segment: &[V]) -> V {
    let mut offset = point;
    for &segment_point in segment {
        offset = offset - offset.project(segment_point - offset);
    }
    offset
}

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

    point_origin_offset(point, &segment[1..])
}

fn point_origin_side<V: Copy + InnerSpace>(point: V, new_point: V, segment: &[V]) -> bool {
    point_origin_offset(new_point, segment).dot(&(point - new_point)) <= V::Scalar::zero()
}

struct Simplex<V> {
    points: Vec<V>,
}

impl<V: Copy + InnerSpace> Simplex<V> {
    fn new(initial: V) -> Self {
        Self {
            points: vec![initial],
        }
    }

    fn add(&mut self, new_point: V) -> Option<V> {
        if self.points.contains(&new_point) {
            return None;
        }

        let count = self.points.len();
        let side_count = count - 1;

        let mut removed = Vec::new();
        for (i, &point) in self.points.iter().enumerate().rev() {
            let mut side = vec![V::zero(); side_count];
            for (s, side_point) in side.iter_mut().enumerate() {
                let i = if s < i { s } else { s + 1 };
                *side_point = self.points[i];
            }
            if !point_origin_side(point, new_point, &side) {
                removed.push(i);
            }
        }

        if removed.len() > 1 {
            for i in removed {
                let segment = self.points[i] - new_point;
                let ratio = -new_point.dot(&segment) / segment.magnitude2();
                if !(V::Scalar::zero()..=V::Scalar::one()).contains(&ratio) {
                    self.points.remove(i);
                }
            }

            self.points.push(new_point);

            return None;
        }

        for i in removed {
            self.points.remove(i);
        }

        self.points.push(new_point);

        Some(-origin_offset(&self.points))
    }

    fn without(&self, i: usize) -> Self {
        let mut points = self.points.clone();
        points.remove(i);
        Self { points }
    }

    fn split(&self) -> impl Iterator<Item = Self> {
        (0..self.points.len()).map(|i| self.without(i))
    }
}

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) {
        // Find an initial simplex vertex on the object, choose initial search direction
        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 = Simplex::new(initial);

        let mut dir = -initial;

        // Find the farthest point on the object in the search direction and add it to the simplex
        while let Some(new_dir) = simplex.add(self.support(other, dir)) {
            dir = new_dir;
        }

        // TODO: Use dimension generic solution
        if simplex.points.len() <= 2 {
            let offset = origin_offset(&simplex.points);
            return (offset, Side::Outside);
        }

        let mut result = None;
        for simplex in simplex.split() {
            let offset = origin_offset(&simplex.points);
            let mag2 = offset.magnitude2();
            if let Some((old_mag2, _)) = result
                && mag2 >= old_mag2
            {
                continue;
            }

            result = Some((mag2, offset));
        }

        (
            if let Some((_, offset)) = result {
                offset
            } else {
                V::zero()
            },
            Side::Inside,
        )
    }
}

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