#include <vecmath.h>
#include <physics.h>
#include <stdlib.h>

#include "dat.h"

void
__phxnarrowphase(int n, TestCollision tests[n])
{
	CollisionEvent event;

	for(int i = 0; i < n; i++) {
		TestCollision *c = tests + i;
		Body *a = c->a + phxbodypool;
		Body *b = c->b + phxbodypool;

		if(a < b && phxaabbcheck(a, b)) {
			float p[2], n[2], rvel[2], j;
			phxaabbresolv(a, b, p, n);

			vec2_sub(rvel, a->vel, b->vel);
			j = vec2_dot(rvel, n);

			vec2_sub_scaled(a->vel, a->vel, n,  0.5);
			vec2_sub_scaled(b->vel, b->vel, n, -0.5);

			vec2_sub_scaled(a->pos, a->pos, p,  0.5);
			vec2_sub_scaled(b->pos, b->pos, p, -0.5);

			event.body1 = c->a;
			event.body2 = c->b;
			phxenqevent(&event);
		}
	}
}