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

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

static Tilemap** _world_maps = NULL;
static size_t _world_maps_cap = 0;
static size_t _world_maps_len = 0;

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

    _world_maps = malloc(4 * (sizeof(Tilemap*)));
    _world_maps_cap = 4;
    _world_maps_len = 0;
}

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

    free(_world_maps);
    _world_maps_cap = 0;
    _world_maps_len = 0;
}

void _internal_physics_world_resize_entities(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) {
    _internal_physics_world_resize_entities(_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);
            return;
        }
    }

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

static inline
void _internal_physics_world_resize_maps(size_t minimum) {
    if(minimum < _world_maps_cap) {
        return;
    }

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

    Tilemap** new = realloc(_world_maps, amount * sizeof(Tilemap*));
    ASSERT_RETURN(new != NULL,, "Could not grow physics tilemaps array");

    _world_maps = new;
    _world_maps_cap = amount;
}

void physics_world_add_map(Tilemap* map) {
    _internal_physics_world_resize_maps(_world_maps_len + 1);

    _world_maps[_world_maps_len] = map;
    ++_world_maps_len;
}

static inline
void _internal_physics_world_remove_map(Tilemap** at) {
    Tilemap** from = at + 1;
    memmove(at, from, (at - _world_maps + _world_bodies_len) * sizeof(Tilemap*));
    --_world_bodies_len;
}

void physics_world_remove_map(Tilemap* map) {
    Tilemap** end = _world_maps + _world_maps_len;
    for(Tilemap** itr = _world_maps; itr < end; ++itr) {
        if(*itr == map) {
            _internal_physics_world_remove_map(itr);
            return;
        }
    }

    ASSERT_RETURN(0,, "Physics tilemap with data at %p is not registered in physics world and cannot be removed.", map);
}

static inline
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) {
            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);
            }
        }
    }
}

static inline
void _internal_tilemap_entity_collision_check(Tilemap* map, PhysicsEntity entity) {
    Collision collision_a;
    Collision collision_b;
    PhysicsEntity tileentity; 
    RigidBody* entity_body = entity.tc->get_rigidbody(entity.data);

    size_t len = tilemap_get_tile_count(map);
    for(size_t i = 0; i < len; ++i) {
        tileentity = TileInstance_as_PhysicsEntity(tilemap_get_tile(map, i));
        if(tileentity.tc->get_shape(tileentity.data) == NULL)
            continue;

        if(collision_check(entity, tileentity, &collision_a, &collision_b)) {
            rigidbody_add_contact(entity_body, collision_a);
        }
    }
}

static inline
void _internal_physics_tilemap_collision() {
    PhysicsEntity* bodies_end = _world_bodies + _world_bodies_len;
    Tilemap** maps_end = _world_maps + _world_maps_len;
    for(PhysicsEntity* entity = _world_bodies; entity < bodies_end; ++entity) {
        for(Tilemap** map = _world_maps; map < maps_end; ++map)  {
            _internal_tilemap_entity_collision_check(*map, *entity);
        }
    }
}

static inline
void _internal_physics_apply() {
    PhysicsEntity* end = _world_bodies + _world_bodies_len;

    for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) {
        physics_entity_update(*entity);
    }
}

static inline
void _internal_physics_integrate_forces() {
    PhysicsEntity* end = _world_bodies + _world_bodies_len;
    for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity)
        rigidbody_integrate_forces(entity->tc->get_rigidbody(entity->data));
}

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