#include "physics_world.h"
#include "debug.h"
#include "collision.h"
#include "collider.h"
#include "rigidbody.h"

static List _world_bodies;
static List _world_bodies_add_queue;
static List _world_bodies_remove_queue;

void physics_world_init() {
    _world_bodies = list_from_type(PhysicsEntity);
    _world_bodies_add_queue = list_from_type(PhysicsEntity);
    _world_bodies_remove_queue = list_from_type(PhysicsEntity);
}

void physics_world_clean() {
    list_empty(&_world_bodies);
}

static
size_t _internal_physics_world_find_index(PhysicsEntity entity) {
    for(size_t i = 0; i < _world_bodies.len; ++i) {
        PhysicsEntity* found = list_at_as(PhysicsEntity, &_world_bodies, i);
        if(found->data == entity.data) {
            return i;
        }
    }
    ASSERT_RETURN(0, _world_bodies.len, "Failed to find entity %p in world bodies", entity.data);
}

static
void _internal_physics_world_add_remove() {
    size_t index = 0;
    list_foreach(PhysicsEntity*, entity, &_world_bodies_remove_queue) {
        index = _internal_physics_world_find_index(*entity);
        list_erase(&_world_bodies, index);
    }
    
    list_empty(&_world_bodies_remove_queue);
    
    list_foreach(PhysicsEntity*, entity, &_world_bodies_add_queue) {
        list_add(&_world_bodies, entity);
    }
    list_empty(&_world_bodies_add_queue);
}

void physics_world_add_entity(PhysicsEntity entity) {
    list_add(&_world_bodies_add_queue, &entity);
}

void physics_world_remove_entity(PhysicsEntity entity) {
    list_add(&_world_bodies_remove_queue, &entity);
}

static inline
void _internal_physics_check_entities(PhysicsEntity left, PhysicsEntity right) {
    RigidBody* rbleft = left.tc->get_rigidbody(left.data);
    RigidBody* rbright = right.tc->get_rigidbody(right.data);

    Collision collision_left, collision_right;
    int is_overlap = 0;

    list_foreach(Collider**, left_col, rigidbody_get_colliders(rbleft)) {
        list_foreach(Collider**, right_col, rigidbody_get_colliders(rbright)) {
            is_overlap = collider_is_overlap(*left_col) || collider_is_overlap(*right_col);

            if(collision_check(*left_col, *right_col, &collision_left, &collision_right)) {
                if(is_overlap) {
                    left.tc->on_overlap(left.data, *right_col);
                    right.tc->on_overlap(right.data, *left_col);
                } else {
                    rigidbody_add_contact(rbleft, collision_left);
                    rigidbody_add_contact(rbright, collision_right);
                }
            }
        }
    }
}

static inline
void _internal_physics_narrow_collision() {
    size_t half_end = _world_bodies.len/2;

    PhysicsEntity* right = NULL;
    list_foreach(PhysicsEntity*, left, &_world_bodies) {
        for(size_t right_index = 0; right_index <= half_end; ++right_index) {
            right = list_at_as(PhysicsEntity, &_world_bodies, right_index);

            if(left->data == right->data) continue;
            _internal_physics_check_entities(*left, *right);
        }
    }
}

static inline
void _internal_physics_apply() {
    list_foreach(PhysicsEntity*, entity, &_world_bodies) {
        physics_entity_update(*entity);
    }
}

static inline
void _internal_physics_integrate_forces() {
    list_foreach(PhysicsEntity*, entity, &_world_bodies)
        rigidbody_integrate_forces(entity->tc->get_rigidbody(entity->data));
}

void physics_world_tick() {
    _internal_physics_world_add_remove();
    _internal_physics_integrate_forces();
    _internal_physics_narrow_collision();
    _internal_physics_apply();
}

List physics_world_query_all(PhysicsQuery query, RigidBody* ignore) {
    List result = list_from_type(Collider*);
    list_foreach(PhysicsEntity*, entity, &_world_bodies) {
        RigidBody* body = entity->tc->get_rigidbody(entity->data);
        if(body == ignore) continue;

        list_foreach(Collider**, collider, rigidbody_get_colliders(body))
            if(overlap_check(query, *collider))
                list_add(&result, collider);
    }
    return result;
}

List physics_world_box_query_all(Vector centre, Vector extents, PhysicsMask mask, RigidBody* ignore) {
    Transform transform = {
        .position = centre,
        .scale = OneVector,
        .rotation = 0.f
    };
    Shape* shape = shape_new((Vector[]){
        MakeVector(-extents.x, -extents.y),
        MakeVector(extents.x, -extents.y),
        MakeVector(extents.x, extents.y),
        MakeVector(-extents.x, extents.y)
    }, 4);
    PhysicsQuery query = {
        .shape = shape,
        .transform = &transform,
        .mask = mask
    };
    return physics_world_query_all(query, ignore);
}

Collider* physics_world_query(PhysicsQuery query, RigidBody* ignore) {
    list_foreach(PhysicsEntity*, entity, &_world_bodies) {
        RigidBody* body = entity->tc->get_rigidbody(entity->data);
        if(body == ignore) continue;
        
        list_foreach(Collider**, collider, rigidbody_get_colliders(body))
            if(overlap_check(query, *collider))
                return *collider;
    }
    return NULL;
}

Collider* physics_world_box_query(Vector centre, Vector extents, PhysicsMask mask, RigidBody* ignore) {
    Transform transform = {
        .position = centre,
        .scale = OneVector,
        .rotation = 0.f
    };
    Shape* shape = shape_new((Vector[]){
        MakeVector(-extents.x, -extents.y),
        MakeVector(extents.x, -extents.y),
        MakeVector(extents.x, extents.y),
        MakeVector(-extents.x, extents.y)
    }, 4);
    PhysicsQuery query = {
        .shape = shape,
        .transform = &transform,
        .mask = mask
    };
    return physics_world_query(query, ignore);
}
