#include "collision.h"
#include "../lib/ds.h"
#include "../lib/geometry.h"
#include <assert.h>


void phy_collisions_broadphase(phy_world *world, phy_body_pair *pair_list, u32 max_pairs, u32 *num_pairs)
{
	// Update AABBs
	for(u32 i = 0; i < world->bodies_count; i++)
	{
		phy_body *body = world->bodies + i;
		body->aabb = phy_aabb_from_body(body);
	}


	u32 pairs_count = 0;

	for(u32 a = 0; a < world->bodies_count; a++)
	{
		phy_body *body_a = world->bodies + a;
		for(u32 b = a + 1; b < world->bodies_count; b++)
		{
			phy_body *body_b = world->bodies + b;

			if(overlaps(body_a->aabb, body_b->aabb))
			{
				// Add to list of possible collisions.
				if(pairs_count < max_pairs)
				{
					pair_list[pairs_count] = phy_body_pair{
						.body_a = body_a,
						.body_b = body_b
					};
				}
				pairs_count++;
			}
		}
	}

	if(num_pairs)
		*num_pairs = pairs_count;
}


// @Cleanup: put this in the right place
// @Performance: this is probably not a good way of doing this
void project_box_on_axis(phy_body *body, v3 axis, f32 *min, f32 *max)
{
	v3 points[8];
	build_cube_vertices(points);

	m4 transform = translation_v3(body->position) * rotation_v3(body->rotation) * scale_v3(body->box.dimensions);

	*min = *max = project_point_on_vector(axis, body->position);
	for(int i = 0; i < 8; i++)
	{
		f32 projected = project_point_on_vector(axis, (transform * V4(points[i], 1)).xyz);
		*min = minimum(*min, projected);
		*max = maximum(*max, projected);
	}
}

f32 axis_range_distance(f32 a_min, f32 a_max, f32 b_min, f32 b_max)
{
	f32 a_range = a_max - a_min;
	f32 a_center = a_min + a_range / 2;
	f32 b_range = b_max - b_min;
	f32 b_center = b_min + b_range / 2;
	return fabs(a_center - b_center) - (a_range + b_range) / 2;
}

bool sat_box_face_point_collision_test(phy_body *body_a, phy_body *body_b, v3 axis, f32 a_size_on_current_axis, f32 *depth, v3 *furthest_a, v3 *furthest_b)
{
	f32 a_middle = project_point_on_vector(axis, body_a->position);
	f32 b_middle = project_point_on_vector(axis, body_b->position);
	f32 direction = (b_middle - a_middle < 0) ? -1 : 1;

	f32 boundary = a_middle + direction * a_size_on_current_axis / 2;
	f32 dist = +INFINITY;
	v3 point;

	v3 points[8]; build_cube_vertices(points);
	m4 transform = translation_v3(body_b->position) * rotation_v3(body_b->rotation) * scale_v3(body_b->box.dimensions);

	for(int i = 0; i < 8; i++)
	{
		v3 p = (transform * V4(points[i], 1)).xyz;
		f32 projected = project_point_on_vector(axis, p);
		f32 curr_dist = direction * (projected - boundary);
		if(curr_dist < dist)
		{
			dist = curr_dist;
			point = p;
		}
	}

	if(dist < 0) // Possible collision
	{
		v3 furthest_point_b = point;
		v3 furthest_point_a = point + -dist * -direction * axis;

		*depth = -dist * direction;
		*furthest_a = furthest_point_a;
		*furthest_b = furthest_point_b;
		return true;
	}
	// else Separated on this axis. No collision
	return false;
}

bool sat_box_collision_test(phy_body *body_a, phy_body *body_b, phy_collision *res_collision)
{
	// Separating axis test
	// 3 + 3 = 6 face normals
	// 3 * 3 = 9 right angle to pair of edges
	v3 axes[15];
	m4 a_rotation = rotation_v3(body_a->rotation);
	axes[0] = extract_column(a_rotation, 0).xyz;
	axes[1] = extract_column(a_rotation, 1).xyz;
	axes[2] = extract_column(a_rotation, 2).xyz;
	m4 b_rotation = rotation_v3(body_b->rotation);
	axes[3] = -extract_column(b_rotation, 0).xyz;
	axes[4] = -extract_column(b_rotation, 1).xyz;
	axes[5] = -extract_column(b_rotation, 2).xyz;

	for(int a = 0; a < 3; a++)
	for(int b = 0; b < 3; b++)
	{
		(axes+6)[3*a + b] = cross(axes[a], axes[b + 3]);
	}

	phy_collision collision;
	collision.body_a = body_a;
	collision.body_b = body_b;
	collision.depth = +INFINITY;

	// Face axes: body a
	for(int i = 0; i < 3; i++)
	{
		v3 axis = axes[i];
		f32 depth; v3 furthest_point_a, furthest_point_b;
		bool collides = sat_box_face_point_collision_test(body_a, body_b, axis, body_a->box.dimensions.E[i], &depth, &furthest_point_a, &furthest_point_b);

		if(!collides)
			return false;

		if(abs(depth) < abs(collision.depth))
		{
			collision.depth = depth;
			collision.furthest_point_a = furthest_point_a;
			collision.furthest_point_b = furthest_point_b;
			collision.best_separation_direction = axis;
		}
	}
	// Face axes: body b
	for(int i = 0; i < 3; i++)
	{
		v3 axis = axes[3 + i];
		f32 depth; v3 furthest_point_a, furthest_point_b;
		bool collides = sat_box_face_point_collision_test(body_b, body_a, axis, body_b->box.dimensions.E[i], &depth, &furthest_point_b, &furthest_point_a);

		if(!collides)
			return false;

		if(abs(depth) < abs(collision.depth))
		{
			collision.depth = depth;
			collision.furthest_point_a = furthest_point_a;
			collision.furthest_point_b = furthest_point_b;
			collision.best_separation_direction = -axis;
		}
	}

	// @TODO: edge-edge

	*res_collision = collision;
	return true;
}

void phy_collisions_detection(phy_world *world, phy_body_pair *pair_list, u32 num_pairs, phy_collision *collision_list, u32 max_collisions, u32 *num_collisions)
{
	u32 n_collisions = 0;
	for(u32 i = 0; i < num_pairs; i++)
	{
		phy_body *body_a = pair_list[i].body_a;
		phy_body *body_b = pair_list[i].body_b;

		if(body_b->shape < body_a->shape)
			swap(body_b, body_a);


		switch(body_a->shape)
		{
			case PHY_SHAPE_SPHERE:
			{
				switch(body_b->shape)
				{
					case PHY_SHAPE_SPHERE:
					{
						f32 dist = distance(body_a->position, body_b->position);
						f32 radius_sum = body_a->sphere.radius + body_b->sphere.radius;
						if(dist <= radius_sum)
						{
							phy_collision collision;

							collision.body_a = body_a;
							collision.body_b = body_b;
							collision.depth = radius_sum - dist;
							v3 direction = normalize(body_b->position - body_a->position);
							collision.furthest_point_a = body_a->position + (body_a->sphere.radius - collision.depth) * direction;
							collision.furthest_point_b = body_b->position + (body_b->sphere.radius - collision.depth) * -direction;
							collision.best_separation_direction = direction;

							if(n_collisions < max_collisions)
								collision_list[n_collisions] = collision;
							n_collisions++;
						}
					} break;
					case PHY_SHAPE_BOX:
					{
						// Use box's cordinate space
						m4 box_b_coord_space_transform = rotation_v3(-body_b->rotation) * translation_v3(-body_b->position);

						v4 sphere_center4 = box_b_coord_space_transform * V4(body_a->position, 1.0);
						v3 sphere_center = sphere_center4.xyz / sphere_center4.w;

						Box box = {
							.min = -0.5*body_b->box.dimensions,
							.max =  0.5*body_b->box.dimensions
						};

						v3 closest_point;
						f32 dist = box_SDF(box, sphere_center, &closest_point);

						if(dist - body_a->sphere.radius <= 0)
						{
							phy_collision collision;

							collision.body_a = body_a;
							collision.body_b = body_b;
							collision.depth = dist - body_a->sphere.radius;
							v3 direction = normalize(closest_point - body_a ->position);
							collision.furthest_point_a = body_a->position + (body_a->sphere.radius - collision.depth) * direction;
							collision.furthest_point_b = body_a->position + body_a->sphere.radius * direction;
							collision.best_separation_direction = direction;

							if(n_collisions < max_collisions)
								collision_list[n_collisions] = collision;
							n_collisions++;
						}

					} break;
					case PHY_SHAPE_MESH:
					{
						assert(false && "Unmanaged PHY_SHAPE pair");
					} break;
					default:
					{
						assert(false && "Unmanaged PHY_SHAPE pair");
					} break;
				}
			} break;
			case PHY_SHAPE_BOX:
			{
				switch(body_b->shape)
				{
					// case PHY_SHAPE_SPHERE: // Already managed
					case PHY_SHAPE_BOX:
					{
						phy_collision collision;
						bool collides = sat_box_collision_test(body_a, body_b, &collision);
						if(collides)
						{
							if(n_collisions < max_collisions)
								collision_list[n_collisions] = collision;
							n_collisions++;
						}
					} break;
					case PHY_SHAPE_MESH:
					{
						assert(false && "Unmanaged PHY_SHAPE pair");
					} break;
					default:
					{
						assert(false && "Unmanaged PHY_SHAPE pair");
					} break;
				}
			} break;
			case PHY_SHAPE_MESH:
			{
				switch(body_b->shape)
				{
					// case PHY_SHAPE_SPHERE: // Already managed
					// case PHY_SHAPE_BOX:    // Already managed
					case PHY_SHAPE_MESH:
					{
						assert(false && "Unmanaged PHY_SHAPE pair");
					} break;
					default:
					{
						assert(false && "Unmanaged PHY_SHAPE pair");
					} break;
				}
			} break;
			default:
			{
				assert(false && "Unmanaged PHY_SHAPE");
			} break;
		}
	}
	*num_collisions = n_collisions;
}

void phy_collisions_resolution(phy_world *world, phy_collision *collision_list, u32 num_collisions)
{

	for(u32 i = 0; i < num_collisions; i++)
	{
		phy_body *body_a = collision_list[i].body_a;
		phy_body *body_b = collision_list[i].body_b;
		v3 separation_direction = collision_list[i].best_separation_direction;
		f32 depth = collision_list[i].depth;

		if(body_a->mass != 0) // Not a static body
		{
			body_a->position += -separation_direction * depth;//-normalize(body_a->velocity) * dot(normalize(body_a->velocity), separation_direction * depth);
			body_a->velocity = {0,0,0};//length(body_a->velocity) * separation_direction * body_a->bounciness;
		}
		if(body_b->mass != 0) // Not a static body
		{
			body_b->position += separation_direction * depth;//-normalize(body_b->velocity) * dot(normalize(body_b->velocity), separation_direction * depth);
			body_b->velocity = {0,0,0};//length(body_a->velocity) * -separation_direction * body_b->bounciness;
		}
	}
}