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

#include "dat.h"

#define POOL_SIZE (1024 * 1024)

Body phxbodypool[POOL_SIZE];
int  phxbodypoolsize;

static BodyID allocbody(void);

BodyID
phxnew(BodyType type)
{
	BodyID b;

	if((b = allocbody()) == -1)
		return b;

	__getbodydata(b)->type = type;
	return b;
}

void
phxdel(BodyID id)
{
	__getbodydata(id)->active = 0;
}

void
phxsetpos(BodyID b, float x, float y)
{
	vec2_dup(__getbodydata(b)->pos, (vec2){ x, y });
}

void
phxsetsize(BodyID b, float w, float h)
{
	vec2_dup(__getbodydata(b)->size, (vec2){ w, h });
}

void
phxgetpos(BodyID b, float p[2])
{
	vec2_dup(p, __getbodydata(b)->pos);
}

void
phxgetsize(BodyID b, float p[2])
{
	vec2_dup(p, __getbodydata(b)->size);
}

void
phxapplyaccel(BodyID b, float a[2])
{
	vec2_add(__getbodydata(b)->accel, __getbodydata(b)->accel, a);
}

BodyID
allocbody(void)
{
	int i;
	for(i = 0; i < phxbodypoolsize; i++) {
		if(!phxbodypool[i].active)
			break;
	}

	if(i >= phxbodypoolsize) {
		if(phxbodypoolsize == POOL_SIZE)
			return -1;
		i = phxbodypoolsize++;
	}

	phxbodypool[i].active = 1;
	phxbodypool[i].generation++;

	return __getbodyid(phxbodypool + i);
}

Body *
__getbodydata(BodyID id)
{
	unsigned int i = id >> 16;
	unsigned int g = id & 0xFFFF;

	assert(i < POOL_SIZE);
	assert((phxbodypool[i].generation & 0xFFFF) == g);

	return phxbodypool + i;
}

BodyID
__getbodyid(Body *b)
{
	unsigned int i = b - phxbodypool;
	return (i << 16) | (phxbodypool[i].generation & 0xFFFF);
}