#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#include "QF/mathlib.h"
#include "QF/mersenne.h"
#include "QF/sys.h"

vec3_t points[] = {
	{-1, -1,  1},
	{ 1,  1,  1},
	{-1,  1, -1},
	{ 1, -1, -1},
	{-1, -1, -1},
	{ 1,  1, -1},
	{-1,  1,  1},
	{ 1, -1,  1},
	{ 0,  0,  0},
};
const vec_t *line[] = {points[0], points[1]};
const vec_t *tri[] = {points[0], points[1], points[2]};
const vec_t *tetra[] = {points[0], points[1], points[2], points[3]};

struct {
	const vec_t **points;
	int         num_points;
	vec_t      *x;
	vec_t       expect[4];
} tests[] = {
	{line, 2, points[0], {1, 0}},
	{line, 2, points[1], {0, 1}},
	{line, 2, points[2], {0.5, 0.5}},
	{line, 2, points[3], {0.5, 0.5}},
	{line, 2, points[8], {0.5, 0.5}},
	{tri, 3, points[0], {1, 0, 0}},
	{tri, 3, points[1], {0, 1, 0}},
	{tri, 3, points[2], {0, 0, 1}},
	{tri, 3, points[3], {0.333333333, 0.333333333, 0.333333333}},
	{tri, 3, points[8], {0.333333333, 0.333333333, 0.333333333}},
	{tetra, 4, points[0], {1, 0, 0, 0}},
	{tetra, 4, points[1], {0, 1, 0, 0}},
	{tetra, 4, points[2], {0, 0, 1, 0}},
	{tetra, 4, points[3], {0, 0, 0, 1}},
	{tetra, 4, points[4], { 0.5, -0.5,  0.5,  0.5}},
	{tetra, 4, points[5], {-0.5,  0.5,  0.5,  0.5}},
	{tetra, 4, points[6], { 0.5,  0.5,  0.5, -0.5}},
	{tetra, 4, points[7], { 0.5,  0.5, -0.5,  0.5}},
	{tetra, 4, points[8], {0.25, 0.25, 0.25, 0.25}},
};
#define num_tests (sizeof (tests) / sizeof (tests[0]))

static inline float
rnd (mtstate_t *mt)
{
	union {
		uint32_t    u;
		float       f;
	}           uf;

	do {
		uf.u = mtwist_rand (mt) & 0x007fffff;
	} while (!uf.u);
	uf.u |= 0x40000000;

	return uf.f - 3.0;
}

int
main (int argc, const char **argv)
{
	int         res = 0;
	size_t      i;
	int         j;
	vec_t       lambda[4];
	mtstate_t   mt;
	double      start, end;

	for (i = 0; i < num_tests; i ++) {
		BarycentricCoords (tests[i].points, tests[i].num_points, tests[i].x,
						   lambda);
		for (j = 0; j < tests[i].num_points; j++) {
			if (tests[i].expect[j] != lambda[j])
				break;
		}
		if (j != tests[i].num_points) {
			res = 1;
			printf ("test %d failed\n", (int) i);
			printf ("expect:");
			for (j = 0; j < tests[i].num_points; j++)
				printf (" %.9g", tests[i].expect[j]);
			printf ("\ngot   :");
			for (j = 0; j < tests[i].num_points; j++)
				printf (" %.9g", lambda[j]);
			printf ("\n");
		}
	}

	mtwist_seed (&mt, 0);
	start = Sys_DoubleTime ();
	for (i = 0; i < 1000000; i++) {
		vec3_t      p, x;
		VectorSet (rnd (&mt), rnd (&mt), rnd (&mt), p);
		BarycentricCoords (tetra, 4, p, lambda);
		VectorZero (x);
		for (j = 0; j < 4; j++)
			VectorMultAdd (x, lambda[j], tetra[j], x);
		if (VectorDistance_fast (x, p) > 1e-4) {
			res = 1;
			printf ("[%.9g %.9g %.9g] != [%.9g %.9g %.9g]: [%g %g %g %g]\n",
					VectorExpand (x), VectorExpand (p), QuatExpand (lambda));
			break;
		}
	}
	end = Sys_DoubleTime ();
	printf ("%d itterations in %gs: %g itters/second\n", (int) i, end - start,
			i / (end - start));
	return res;
}