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

static List _world_bodies;

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

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

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

void physics_world_remove_entity(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) {
            list_erase(&_world_bodies, i);
            return;
        }
    }

    ASSERT_RETURN(0,, "Physics entity with data at %p is not registered in physics world", entity.data);
}

PhysicsEntity physics_world_query(PhysicsQuery query) {
    list_foreach(PhysicsEntity*, entity, &_world_bodies) {
        if(overlap_check(query, *entity))
            return *entity;
    }
}

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

    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;

            if(collision_check(*left, *right, &collision_left, &collision_right)) {
                left->tc->on_collision(left->data, collision_left);
                right->tc->on_collision(right->data, collision_right);
                rigidbody_add_contact(left->tc->get_rigidbody(left->data), collision_left);
                rigidbody_add_contact(right->tc->get_rigidbody(right->data), collision_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_integrate_forces();
    _internal_physics_narrow_collision();
    _internal_physics_apply();
}