#include "collision.h"
#include "debug.h"
#include "player.h"
#include "vmath.h"
#include "rigidbody.h"

// =====================================================
// Shape overlap test using the separating axis theorem
// =====================================================

typedef struct Range {float min; Vector minpoint; float max; Vector maxpoint; } Range;

static
Range _internal_collision_get_range_on_axis(PhysicsEntity self, Vector axis) {
    Transform* transform = self.transformable->get_transform(self.data);
    Shape* shape = self.tc->get_shape(self.data);
    Vector point = shape_get_point_transformed(shape, 0, *transform);
    float dot = vdotf(axis, point);
    Range range = {dot, point, dot, point};

    for(size_t point_index = 1; point_index < shape_get_points_count(shape); ++point_index) {
        point = shape_get_point_transformed(shape, point_index, *transform);
        dot = vdotf(axis, point);
        if(dot < range.min) {
            range.min = dot;
            range.minpoint = point;
        }
        if(dot > range.max) {
            range.max = dot;
            range.maxpoint = point;
        }
    }

    return range;
}

static
Vector _internal_collision_overlap_on_axis(PhysicsEntity self, PhysicsEntity other, Vector axis, Vector* out_point) {
    Range a_range = _internal_collision_get_range_on_axis(self, axis);
    Range b_range = _internal_collision_get_range_on_axis(other, axis);

    if(a_range.min <= b_range.max && a_range.min <= b_range.max) {
        const float overlap_left = a_range.max - b_range.min;
        const float overlap_right = b_range.min - a_range.max;
        const float shortest = fminf(overlap_left, overlap_right);
        *out_point = overlap_right < overlap_left ? a_range.maxpoint : a_range.minpoint;
        return vmulff(axis, shortest);
    } else {
        *out_point = InfinityVector;
        return ZeroVector;
    }
}

static
int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Collision* out) {
    // get components used
    Shape* self_shape = self.tc->get_shape(self.data);
    Transform* self_transform = self.transformable->get_transform(self.data);

    // the shortest distance to solve collision found so far
    Vector shortest_escape = InfinityVector;
    // the squared length of the shortest escape vector found so far
    float shortest_sqrmag = INFINITY;
    // the first index of the points on the edge
    size_t shortest_escape_edge = 0;
    // the number of points in the shape of self
    size_t self_point_count = shape_get_points_count(self_shape);

    for(size_t point_index = 0; point_index < self_point_count; ++point_index) {
        // the next point on the line
        size_t next_index = (point_index + 1) % self_point_count;
        // get the two points defining the collision edge
        Vector a = shape_get_point_transformed(self.tc->get_shape(self.data), point_index, *self_transform);
        Vector b = shape_get_point_transformed(self.tc->get_shape(self.data), next_index,  *self_transform);
        // the direction of the line
        Vector normal = vperpendicularf(vsubf(b, a));

        Vector overlap_point;
        // the smallest escape vector on this axis
        Vector escape = _internal_collision_overlap_on_axis(self, other, vnormalizedf(normal), &overlap_point);
        float sqr_mag = vsqrmagnitudef(escape);
        if(sqr_mag < shortest_sqrmag) {
            shortest_sqrmag = sqr_mag;
            shortest_escape = escape;
            shortest_escape_edge = point_index;
        }

        if(sqr_mag == 0) {
            out->penetration_vector = InfinityVector;
            return 0;
        }
    }

    RigidBody* rba = self.tc->get_rigidbody(self.data);
    RigidBody* rbb = other.tc->get_rigidbody(other.data);
    const Vector velocity = vsubf(rigidbody_get_velocity(rba), rigidbody_get_velocity(rbb));
    const Vector normal = vnormalizedf(shortest_escape);
    Vector world_point = _internal_collision_get_range_on_axis(self, normal).minpoint;

    *out = (Collision) {
        .other = other,

        .point = inverse_transform_point(rigidbody_get_transform(rba), world_point),
        .normal = normal,

        .velocity = velocity,
        .penetration_vector = shortest_escape,

        .edge_left = shape_get_point_transformed(self_shape, shortest_escape_edge, *self_transform),
        .edge_right = shape_get_point_transformed(self_shape, (1 + shortest_escape_edge) % self_point_count, *self_transform),
    };

    return !veqf(shortest_escape, ZeroVector);
}

static
Collision _internal_collision_invert(Collision collision_a, PhysicsEntity a) {
    Vector world_point = _internal_collision_get_range_on_axis(collision_a.other, collision_a.normal).maxpoint;
    RigidBody* body = collision_a.other.tc->get_rigidbody(collision_a.other.data);
    return (Collision){
        .other = a,
        .point = inverse_transform_point(rigidbody_get_transform(body), world_point),
        .normal = vinvf(collision_a.normal),
        .velocity = vinvf(collision_a.velocity),
        .penetration_vector = vinvf(collision_a.penetration_vector),
        .edge_left = collision_a.edge_left,
        .edge_right = collision_a.edge_right,
    };
}

int collision_check(PhysicsEntity a, PhysicsEntity b, Collision* out_a, Collision* out_b) {
    Collision collision_a, collision_b;
    int collision_a_overlaps = _internal_collision_get_overlap(a, b, &collision_a);
    int collision_b_overlaps = _internal_collision_get_overlap(b, a, &collision_b);

    if(!collision_a_overlaps || !collision_b_overlaps)
        return 0;

    if(vsqrmagnitudef(collision_a.penetration_vector) > vsqrmagnitudef(collision_b.penetration_vector))
        collision_b = _internal_collision_invert(collision_a, a);
    else
        collision_a = _internal_collision_invert(collision_b, b);

    *out_a = collision_a;
    *out_b = collision_b;
    return (collision_b_overlaps << 1) | collision_a_overlaps;
}