#include "component_physics.h"

static void _component_physics_collide_tick(ComponentPhysics* self, ECS* ecs);
static void _component_physics_collide_handle(ComponentPhysics* self, ComponentPhysics* collided);
static void _component_physics_collide_pushback(ComponentPhysics* self, ComponentPhysics* collided);

/* Full pushback collision response. */
/* This is functional, but unideal. I hate math. */
static void
_component_physics_collide_pushback(ComponentPhysics* self, ComponentPhysics* collided)
{
	Rectangle rectangle;
	Rectangle collidedRectangle;
	Rectangle intersection;

	component_physics_rectangle_get(self, &rectangle);
	component_physics_rectangle_get(collided, &collidedRectangle);
	
	rectangle_collide_intersection(&rectangle, &collidedRectangle, &intersection);

	/* Horizontal */
	if (intersection.h >= intersection.w)
	{
		if (self->velocity[0] > 0.0f)
			self->position[0] -= intersection.w;
		else if (self->velocity[0] < 0.0f)
			self->position[0] += intersection.w;
	}
	/* Vertical */
	else if (intersection.w > intersection.h)
	{
		if (self->velocity[1] > 0.0f)
			self->position[1] -= intersection.h;
		else if (self->velocity[1] < 0.0f)
			self->position[1] += intersection.h;
	}
}

/* Handles a collision. */
static void
_component_physics_collide_handle(ComponentPhysics* self, ComponentPhysics* collided)
{
	vector_push(&self->collisions, (void*)&collided->component.id);

	if (collided->isSolid)
		_component_physics_collide_pushback(self, collided);
}

/* Check collisions. */
static void
_component_physics_collide_tick(ComponentPhysics* self, ECS* ecs)
{
	Rectangle rectangle;

	component_physics_rectangle_get(self, &rectangle);

	for (s32 i = 0; i < (s32)ecs->lists[ECS_COMPONENT_PHYSICS].components.count; i++)
	{
		ComponentPhysics* physics;
		Rectangle collidedRectangle;

		physics = vector_get(&ecs->lists[ECS_COMPONENT_PHYSICS].components, i);

		if (physics->component.isDisabled)
			continue;

		if (physics->component.id == self->component.id)
			continue;

		component_physics_rectangle_get(physics, &collidedRectangle);

		if (rectangle_collide(&rectangle, &collidedRectangle))
			_component_physics_collide_handle(self, physics);
	}
}

/* Obtains the colliding rectangle of a physics object. */
void
component_physics_rectangle_get(ComponentPhysics* self, Rectangle* rectangle)
{
	rectangle->x = self->position[0] - (self->size[0] / 2);
	rectangle->y = self->position[1] - (self->size[1] / 2);
	rectangle->w = self->size[0];
	rectangle->h = self->size[1];
}

/* Sets physics component info. */
void
component_physics_init
(
	ComponentPhysics* self, 
	vec2 size,
	vec3 position, 
	f32 velocityMax, 
	f32 friction,
	bool isSolid
)
{
	glm_vec2_copy(size, self->size);
	glm_vec3_copy(position, self->position);
	glm_vec3_zero(self->velocity);
	self->friction = friction;
	self->velocityMax = velocityMax;
	self->isSolid = isSolid;
}

/* Adds a physics component. */
void
component_physics_add(ComponentPhysics* self, ECS* ecs)
{
	vector_init(&self->collisions, sizeof(u32));
}

/* Deletes a physics component. */
void
component_physics_delete(ComponentPhysics* self, ECS* ecs)
{
	vector_free(&self->collisions);
}

/* Ticks physics component. */
void
component_physics_tick(ComponentPhysics* self, ECS* ecs)
{
	if (self->collisions.count > 0)
		vector_clear(&self->collisions);

	self->velocity[0] = CLAMP(self->velocity[0], -self->velocityMax, self->velocityMax);
	self->velocity[1] = CLAMP(self->velocity[1], -self->velocityMax, self->velocityMax);
	self->velocity[2] = CLAMP(self->velocity[2], -self->velocityMax, self->velocityMax);
	
	glm_vec3_add(self->velocity, self->position, self->position);

	glm_vec3_scale(self->velocity, self->friction, self->velocity);

	_component_physics_collide_tick(self, ecs);
}
