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

static PhysicsEntity* _world_bodies = NULL;
static size_t _world_bodies_cap = 0;
static size_t _world_bodies_len = 0;

void physics_world_init() {
    _world_bodies = malloc(8 * sizeof(PhysicsEntity));
    _world_bodies_cap = 0;
}

void physics_world_clean() {
    free(_world_bodies);
    _world_bodies_cap = 0;
    _world_bodies_cap = 0;
}

void _physics_world_resize(size_t minimum) {
    if(minimum < _world_bodies_cap) {
        return;
    }

    size_t amount = _world_bodies_cap;
    while(amount < minimum) {
        amount *= 2;
    }

    PhysicsEntity* new = realloc(_world_bodies, amount * sizeof(PhysicsEntity));
    ASSERT_RETURN(new == NULL, , "Failed to grow world bodies array");

    _world_bodies = new;
    _world_bodies_cap = amount;
}

void physics_world_add_entity(PhysicsEntity entity) {
    _physics_world_resize(_world_bodies_len + 1);

    _world_bodies[_world_bodies_len] = entity;
    ++_world_bodies_len;
}

static
void _internal_physics_world_remove_entity(PhysicsEntity* at) {
    PhysicsEntity* from = at + 1;
    memmove(at, from, (from - _world_bodies + _world_bodies_len) * sizeof(PhysicsEntity));
    --_world_bodies_len;
}

void physics_world_remove_entity(PhysicsEntity entity) {
    PhysicsEntity* end = _world_bodies + _world_bodies_len;
    for(PhysicsEntity* itr = _world_bodies; itr < end; ++itr) {
        if(itr->data == entity.data) {
            _internal_physics_world_remove_entity(itr);
        }
    }

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

static
void _internal_physics_narrow_collision() {
    PhysicsEntity* end = _world_bodies + _world_bodies_len;
    PhysicsEntity* half_end = _world_bodies + _world_bodies_len/2;
    Collision collision_left, collision_right;

    for(PhysicsEntity* left = _world_bodies; left < end; ++left) {
        for(PhysicsEntity* right = _world_bodies; right < half_end; ++right) {
            collision_check(*left, *right, &collision_left, &collision_right);

            left->tc->on_collision(left->data, collision_left);
            right->tc->on_collision(right->data, collision_right);
        }
    }
}

static
void _internal_physics_move() {
    PhysicsEntity* end = _world_bodies + _world_bodies_len;
    RigidBody* body = NULL;

    for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) {
        body = entity->tc->get_rigidbody(entity->data);
        rigidbody_apply_physics(body);
    }
}

void physics_world_tick() {
    // _internal_physics_broad_collision();
    _internal_physics_narrow_collision();
    _internal_physics_move();
}