#include "base.h"
#include "simulation.h"
#include "../lib/geometry.h"
#include "collision.h"

void phy_integrate(phy_world *world, f64 delta_t)
{
	for(u32 i = 0; i < world->bodies_count; i++)
	{
		phy_body *body = world->bodies + i;

		body->velocity += world->gravity * body->gravity_multiplier * delta_t;

		body->position += body->velocity * delta_t;
		body->rotation += body->angular_velocity * delta_t;
	}
}


void phy_simulate(phy_world *world, f64 delta_t)
{
	// @Performance: Grouping for optimizations (active/inactive, constraints)

	phy_integrate(world, delta_t);

	u32 max_pairs = 1000; // @Correctness: this should be dynamic and should reuse memory when possible
	u32 num_pairs = 0;
	phy_body_pair *pair_list = PHY_ALLOC(phy_body_pair, max_pairs);
	phy_collisions_broadphase(world, pair_list, max_pairs, &num_pairs);

	u32 max_collisions = 1000;
	u32 num_collisions = 0;
	phy_collision *collision_list = PHY_ALLOC(phy_collision, max_collisions);
	phy_collisions_detection(world, pair_list, minimum(max_pairs, num_pairs), collision_list, max_collisions, &num_collisions);

	phy_collisions_resolution(world, collision_list, minimum(max_collisions, num_collisions));

	PHY_FREE(pair_list);
	PHY_FREE(collision_list);
}