#include "collision.h"
#include "vmath.h"
#include "rigidbody.h"
#include "collider.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(PhysicsQuery self, Vector axis) {
    Transform* transform = self.transform;
    Shape* shape = self.shape;
    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(PhysicsQuery self, PhysicsQuery 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);

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

static
int _internal_collision_get_collisions(Collider* self, Collider* other, Collision* out) {
    // get components used
    Shape* self_shape = collider_get_shape(self);
    Transform* self_transform = rigidbody_get_transform(collider_get_rigidbody(self));
    PhysicsQuery self_query = {
        .shape = self_shape,
        .transform = self_transform,
        .mask = 0x0 // not used
    };
    PhysicsQuery other_query = {
        .shape = collider_get_shape(other),
        .transform = rigidbody_get_transform(collider_get_rigidbody(other)),
        .mask = 0x0 // not used
    };

    // 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_dot = 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 edge_lhs = shape_get_point_transformed(self_shape, point_index, *self_transform);
        Vector edge_rhs = shape_get_point_transformed(self_shape, next_index,  *self_transform);
        // the direction of the line
        Vector normal = vnormalizedf(vperpendicularf(vsubf(edge_rhs, edge_lhs)));

        Vector overlap_point;
        // the smallest escape vector on this axis
        Vector escape = _internal_collision_overlap_on_axis(self_query, other_query, normal, &overlap_point);
        float dot = vdotf(vinvf(normal), escape);
        if(dot <= 0.0) {
            return 0;
        }
        if(dot <= shortest_dot) {
            shortest_dot = dot;
            shortest_escape = escape;
            shortest_escape_edge = point_index;
        }
    }

    RigidBody* rba = collider_get_rigidbody(self);
    RigidBody* rbb = collider_get_rigidbody(other);
    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_query, 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);
}

Collision collision_invert(Collision collision_a, Collider* a) {
    RigidBody* body = collider_get_rigidbody(a);
    Shape* shape = collider_get_shape(a);
    Transform* transform = rigidbody_get_transform(body);
    
    Vector world_point = _internal_collision_get_range_on_axis((PhysicsQuery){.shape = shape, .transform = transform }, collision_a.normal).maxpoint;
    
    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(Collider* a, Collider* b, Collision* out_a, Collision* out_b) {
    if(!(collider_get_layers(a) & collider_get_mask(b)) || !(collider_get_mask(a) & collider_get_layers(b)))
        return 0;
    Collision collision_a, collision_b;
    int collision_a_overlaps = _internal_collision_get_collisions(a, b, &collision_a);
    int collision_b_overlaps = _internal_collision_get_collisions(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 = collision_invert(collision_a, a);
    else
        collision_a = collision_invert(collision_b, b);

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

static
int _internal_overlap_check(PhysicsQuery a, PhysicsQuery b) {
    Shape* shape = a.shape;
    Transform* transform = a.transform;
    const size_t shape_point_count = shape_get_points_count(shape);
    
     for(size_t point_index = 0; point_index < shape_point_count; ++point_index) {
        // the next point on the line
        size_t next_index = (point_index + 1) % shape_point_count;
        // get the two points defining the collision edge
        Vector edge_lhs = shape_get_point_transformed(shape, point_index, *transform);
        Vector edge_rhs = shape_get_point_transformed(shape, next_index,  *transform);
        // the direction of the line
        Vector normal = vnormalizedf(vperpendicularf(vsubf(edge_rhs, edge_lhs)));

        Vector overlap_point;
        // the smallest escape vector on this axis
        Vector escape = _internal_collision_overlap_on_axis(a, b, normal, &overlap_point);
        float dot = vdotf(vinvf(normal), escape);
        if(dot <= 0.0) {
            return 0;
        }
    }
    return 1;
}

int overlap_check(PhysicsQuery query, Collider* collider) {
    PhysicsQuery collider_query = collider_to_query(collider);
    return (query.mask & collider_get_layers(collider)) != 0 && (_internal_overlap_check(query, collider_query) || _internal_overlap_check(collider_query, query));
}
