#include "simulation.h"

#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>

tc_physics_simulation_s * tc_new_physics_simulation()
{
    tc_physics_simulation_s        *simulation      = malloc(sizeof(tc_physics_simulation_s));
    simulation->root                    = tc_new_physics_group(simulation);
    
    return simulation;
}



void tc_add_physics_object(tc_physics_simulation_s *simulation, tc_physics_entity_s *object)
{
    tc_add_physics_object_to(simulation->root, object);
}

void tc_add_physics_object_to(tc_physics_entity_s *container, tc_physics_entity_s *object)
{
    if(container != NULL) object->super = container;
    
    if((container->value.group.num_entities+1)
        >=
       container->value.group.entities_capacity
    ) {
        container->value.group.entities_capacity     *= 2;
        container->value.group.entities =
            realloc(container->value.group.entities, sizeof(tc_physics_entity_s *) * container->value.group.entities_capacity);
    }
    container->value.group.entities[container->value.group.num_entities] = object;
    ++container->value.group.num_entities;
}



tc_physics_entity_s * tc_new_physics_entity(tc_physics_simulation_s *simulation)
{
    tc_physics_entity_s        *entity      = malloc(sizeof(tc_physics_entity_s));;
    entity->super                   = NULL;
    entity->simulation              = simulation;
    entity->relative_position.x     = 0.0f;
    entity->relative_position.y     = 0.0f;
    entity->relative_position.z     = 0.0f;
    entity->type                    = TC_PHYSICS_ENTITY;
    
    entity->value.entity.mesh.num_primitives    = 0;
    entity->value.entity.name                   = NULL;
    
    return entity;
}

tc_physics_entity_s * tc_new_physics_group(tc_physics_simulation_s *simulation)
{
    tc_physics_entity_s        *entity      = malloc(sizeof(tc_physics_entity_s));;
    entity->super                   = NULL;
    entity->simulation              = simulation;
    entity->relative_position.x     = 0.0f;
    entity->relative_position.y     = 0.0f;
    entity->relative_position.z     = 0.0f;
    entity->type                    = TC_PHYSICS_ENTITY_GROUP;
    
    entity->value.group.entities_capacity       = 8;
    entity->value.group.num_entities            = 0;
    entity->value.group.entities =
        calloc(sizeof(tc_physics_entity_s), entity->value.group.entities_capacity);
    
    return entity;
}



i32_t tc_physics_find_entity_in_group(tc_physics_entity_s *group, tc_physics_entity_s *entity)
{
    i32_t index = 0;
    while(index < group->value.group.num_entities)
    {
        if(group->value.group.entities[group->value.group.num_entities]
            ==
           entity
          ) return index;
        ++index;
    }
    return -1;
}

void tc_free_physics_entity(tc_physics_entity_s *entity)
{
    if(entity->type == TC_PHYSICS_ENTITY_GROUP)
    {
        for(u32_t index = 0; index < entity->value.group.num_entities; ++index)
            tc_free_physics_entity(entity->value.group.entities[index]);
    }
    free(entity);
}

void tc_remove_physics_entity(tc_physics_entity_s *entity)
{
    if(entity->super == NULL) return;
    
    // Fill up the gap created inside the entities-array.
    
    tc_physics_entity_s        *super           = entity->super;
    
    i32_t                       index           = 0;
    if((index = tc_physics_find_entity_in_group(super, entity)) == -1) return;
    
    super->value.group.entities[index]  = 
        super->value.group.entities[super->value.group.num_entities-1];
    --super->value.group.num_entities;
    
    tc_free_physics_entity(entity);
}



void tc_tick_physics(tc_physics_simulation_s *simulation, f32_t delta)
{
    
}