#![deny(missing_docs)]

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

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

trait ClampedVector: Copy + InnerSpace {
    fn clamped_project(self, dis: Self) -> Self {
        let mag = dis.magnitude();
        let dir = dis / mag;
        let new_mag = self.dot(&dir);
        if new_mag <= Self::Scalar::zero() {
            Self::zero()
        } else if new_mag >= mag {
            dis
        } else {
            dir * new_mag
        }
    }
}

impl<V: Copy + InnerSpace> ClampedVector for V {}

/// The capsule collider defined as a convex hull around two spheres having the same radius.
#[derive(Copy, Clone)]
pub struct Capsule<V: VectorSpace> {
    /// The position of one sphere.
    pub start: V,
    /// The position of the other sphere.
    pub end: V,
    /// The radius of the spheres.
    pub rad: V::Scalar,
}

impl<V: Copy + InnerSpace> Capsule<V> {
    /// Creates a new capsule collider.
    pub fn new(rad: V::Scalar, start: V, end: V) -> Self {
        Self { start, end, rad }
    }

    /// Creates a new capsule collider representing a point.
    pub fn point(pos: V) -> Self {
        Self {
            start: pos,
            end: pos,
            rad: V::Scalar::zero(),
        }
    }

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

    /// Creates a new capsule collider representing a sphere.
    pub fn sphere(pos: V, rad: V::Scalar) -> Self {
        Self {
            start: pos,
            end: pos,
            rad,
        }
    }

    fn points(&self, other: &Self) -> [V; 2] {
        let p = self.end - self.start;
        let q = other.end - other.start;
        let d = self.start - other.start;

        let p0 = p.is_zero();
        let q0 = q.is_zero();

        if p0 != q0 {
            return if p0 {
                [self.start, other.start + d.clamped_project(q)]
            } else {
                [self.start + (-d).clamped_project(p), other.start]
            };
        }

        if p0 {
            return [self.start, other.start];
        }

        let p_mag2 = p.magnitude2();
        let q_mag2 = q.magnitude2();
        let angle = p.dot(&q);

        let div = angle * angle - p_mag2 * q_mag2;
        if div.is_zero() {
            let add = d.reject(p);
            return [self.start, self.start - add];
        }

        let a = (p * q_mag2 - q * angle).dot(&d) / div;
        let b = (p * angle - q * p_mag2).dot(&d) / div;

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

        let a_neg = a < zero;
        let b_neg = b < zero;

        let a_centered = !a_neg && a <= one;
        let b_centered = !b_neg && b <= one;

        if a_centered && b_centered {
            return [self.start + p * a, other.start + q * b];
        }

        if !a_centered {
            let a = if a_neg { zero } else { one };
            let b = (angle * a + q.dot(&d)) / q_mag2;
            if zero <= b && b <= one {
                return [self.start + p * a, other.start + q * b];
            }
        }
        if !b_centered {
            let b = if b_neg { zero } else { one };
            let a = (angle * b - p.dot(&d)) / p_mag2;
            if zero <= a && a <= one {
                return [self.start + p * a, other.start + q * b];
            }
        }
        [
            if a_neg { self.start } else { self.end },
            if b_neg { other.start } else { other.end },
        ]
    }
}

impl<V: Copy + InnerSpace> Collider for Capsule<V> {
    type Vector = V;

    fn collision_info(&self, other: &Self) -> Option<CollisionInfo<Self::Vector>> {
        let [point, other_point] = self.points(other);

        let dis = other_point - point;
        let mag2 = dis.magnitude2();
        let rad = self.rad + other.rad;
        if mag2 > rad * rad {
            return None;
        }

        let mag = mag2.sqrt();
        let dir = dis / mag;
        Some(CollisionInfo {
            self_contact: point + dir * self.rad,
            other_contact: other_point - dir * other.rad,
            vector: dir * (mag - rad),
        })
    }
}
