#include "maths.h"
#include "plat.h"

int sin_table[sin_table_count] = {
	0x000, 0x00c, 0x019, 0x025, 0x032, 0x03e, 0x04b, 0x057,
	0x063, 0x070, 0x07c, 0x088, 0x094, 0x0a0, 0x0ac, 0x0b8,
	0x0c3, 0x0cf, 0x0da, 0x0e6, 0x0f1, 0x0fc, 0x107, 0x111,
	0x11c, 0x126, 0x130, 0x13a, 0x144, 0x14e, 0x157, 0x161,
	0x16a, 0x172, 0x17b, 0x183, 0x18b, 0x193, 0x19b, 0x1a2,
	0x1a9, 0x1b0, 0x1b7, 0x1bd, 0x1c3, 0x1c9, 0x1ce, 0x1d4,
	0x1d9, 0x1dd, 0x1e2, 0x1e6, 0x1e9, 0x1ed, 0x1f0, 0x1f3,
	0x1f6, 0x1f8, 0x1fa, 0x1fc, 0x1fd, 0x1fe, 0x1ff, 0x1ff,
	0x200, 0x1ff, 0x1ff, 0x1fe, 0x1fd, 0x1fc, 0x1fa, 0x1f8,
	0x1f6, 0x1f3, 0x1f0, 0x1ed, 0x1e9, 0x1e6, 0x1e2, 0x1dd,
	0x1d9, 0x1d4, 0x1ce, 0x1c9, 0x1c3, 0x1bd, 0x1b7, 0x1b0,
	0x1a9, 0x1a2, 0x19b, 0x193, 0x18b, 0x183, 0x17b, 0x172,
	0x16a, 0x161, 0x157, 0x14e, 0x144, 0x13a, 0x130, 0x126,
	0x11c, 0x111, 0x107, 0x0fc, 0x0f1, 0x0e6, 0x0da, 0x0cf,
	0x0c3, 0x0b8, 0x0ac, 0x0a0, 0x094, 0x088, 0x07c, 0x070,
	0x063, 0x057, 0x04b, 0x03e, 0x032, 0x025, 0x019, 0x00c,
};
int cos_table[sin_table_count];

int sqrt_table[sqrt_table_count] = {
	0x0000, 0x0200, 0x02d4, 0x0376, 0x0400, 0x0478, 0x04e6, 0x054a,
	0x05a8, 0x0600, 0x0653, 0x06a2, 0x06ed, 0x0736, 0x077b, 0x07be,
	0x0800, 0x083f, 0x087c, 0x08b7, 0x08f1, 0x092a, 0x0961, 0x0997,
	0x09cc, 0x0a00, 0x0a32, 0x0a64, 0x0a95, 0x0ac5, 0x0af4, 0x0b22,
	0x0b50, 0x0b7d, 0x0ba9, 0x0bd5, 0x0c00, 0x0c2a, 0x0c54, 0x0c7d,
	0x0ca6, 0x0cce, 0x0cf6, 0x0d1d, 0x0d44, 0x0d6a, 0x0d90, 0x0db6,
	0x0ddb, 0x0e00, 0x0e24, 0x0e48, 0x0e6c, 0x0e8f, 0x0eb2, 0x0ed5,
	0x0ef7, 0x0f19, 0x0f3b, 0x0f5c, 0x0f7d, 0x0f9e, 0x0fbf, 0x0fdf,
	0x1000, 0x101f, 0x103f, 0x105e, 0x107e, 0x109c, 0x10bb, 0x10da,
	0x10f8, 0x1116, 0x1134, 0x1152, 0x116f, 0x118c, 0x11a9, 0x11c6,
	0x11e3, 0x1200, 0x121c, 0x1238, 0x1254, 0x1270, 0x128c, 0x12a7,
	0x12c2, 0x12de, 0x12f9, 0x1314, 0x132e, 0x1349, 0x1364, 0x137e,
	0x1398, 0x13b2, 0x13cc, 0x13e6, 0x1400, 0x1419, 0x1432, 0x144c,
	0x1465, 0x147e, 0x1497, 0x14b0, 0x14c8, 0x14e1, 0x14f9, 0x1512,
	0x152a, 0x1542, 0x155a, 0x1572, 0x158a, 0x15a2, 0x15b9, 0x15d1,
	0x15e8, 0x1600, 0x1617, 0x162e, 0x1645, 0x165c, 0x1673, 0x1689,
};

#define mtx_stack_max 0x20

int mtx_stack[mtx_stack_max][0x10];
int mtx_stack_top;

void init_maths(void) {
	int i, hs = sin_table_count >> 1;
	int m[0x10];
	for (
		i = hs;
		i <  sin_table_count;
		i++
	) {
		sin_table[i] = -sin_table[i - hs];
	}
	hs >>= 1;
	for (i = 0; i < sin_table_count; i++) {
		cos_table[i] = sin_table[(i + hs) & sin_table_mask];
	}
	mtx_stack_top = 1;
	mtx_iden(m);
	mtx_cpy(mtx_stack[0], m);
}

int fpow(int a, int p) {
	int r = a;
	p--;
	while (p--) {
		r = (r * a) >> fbits;
	}
	return r;
}

int* mtx_iden(int* m) {
	register int o = f1;
	m[0]  = o;
	m[1]  = 0;
	m[2]  = 0;
	m[3]  = 0;
	m[4]  = 0;
	m[5]  = o;
	m[6]  = 0;
	m[7]  = 0;
	m[8]  = 0;
	m[9]  = 0;
	m[10] = o;
	m[11] = 0;
	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = o;
	return m;
}

int* mtx_cpy(int* d, const int* s) {
	d[0]=s[0];d[1]=s[1];d[2]=s[2];d[3]=s[3];
	d[4]=s[4];d[5]=s[5];d[6]=s[6];d[7]=s[7];
	d[8]=s[8];d[9]=s[9];d[10]=s[10];d[11]=s[11];
	d[12]=s[12];d[13]=s[13];d[14]=s[14];d[15]=s[15];
	return d;
}

int* mtx_mul(int* d, const int* a, const int* b) {
#define mul(a, b) \
	((a * b) >> fbits)
#define dot(x1, y1, z1, w1, x2, y2, z2, w2) \
	mul(x1, x2) + mul(y1, y2) + mul(z1, z2) + mul(w1, w2)
	d[0]  = dot(a[0], a[1], a[2], a[3],  b[0],b[4],b[8], b[12]);
	d[1]  = dot(a[0], a[1], a[2], a[3],  b[1],b[5],b[9], b[13]);
	d[2]  = dot(a[0], a[1], a[2], a[3],  b[2],b[6],b[10],b[14]);
	d[3]  = dot(a[0], a[1], a[2], a[3],  b[3],b[7],b[11],b[15]);
	d[4]  = dot(a[4], a[5], a[6], a[7],  b[0],b[4],b[8], b[12]);
	d[5]  = dot(a[4], a[5], a[6], a[7],  b[1],b[5],b[9], b[13]);
	d[6]  = dot(a[4], a[5], a[6], a[7],  b[2],b[6],b[10],b[14]);
	d[7]  = dot(a[4], a[5], a[6], a[7],  b[3],b[7],b[11],b[15]);
	d[8]  = dot(a[8], a[9], a[10],a[11], b[0],b[4],b[8], b[12]);
	d[9]  = dot(a[8], a[9], a[10],a[11], b[1],b[5],b[9], b[13]);
	d[10] = dot(a[8], a[9], a[10],a[11], b[2],b[6],b[10],b[14]);
	d[11] = dot(a[8], a[9], a[10],a[11], b[3],b[7],b[11],b[15]);
	d[12] = dot(a[12],a[13],a[14],a[15], b[0],b[4],b[8], b[12]);
	d[13] = dot(a[12],a[13],a[14],a[15], b[1],b[5],b[9], b[13]);
	d[14] = dot(a[12],a[13],a[14],a[15], b[2],b[6],b[10],b[14]);
	d[15] = dot(a[12],a[13],a[14],a[15], b[3],b[7],b[11],b[15]);
#undef dot
#undef mul
	return d;
}

void mtx_push(const int* m) {
	int* d, i;
	assert(mtx_stack_top < mtx_stack_max);
	i = mtx_stack_top++;
	d = mtx_stack[i];
	mtx_iden(d);
	mtx_mul(d, mtx_stack[i - 1], m);
}

void mtx_push_trans(int* v) {
	int m[0x10];
	mtx_trans(m, v);
	mtx_push(m);
}

void mtx_push_rot_x(int a) {
	int m[0x10];
	mtx_rot_x(m, a);
	mtx_push(m);
}

void mtx_push_rot_y(int a) {
	int m[0x10];
	mtx_rot_y(m, a);
	mtx_push(m);
}

void mtx_push_rot_z(int a) {
	int m[0x10];
	mtx_rot_z(m, a);
	mtx_push(m);
}

int* mtx_pop(void) {
	assert(mtx_stack_top > 0);
	return mtx_stack[--mtx_stack_top];
}

int* mtx_popn(int n) {
	int* t = mtx_pop(); n--;
	while (n--)
		mtx_pop();
	return t;
}

int* mtx_peek(void) {
	return mtx_stack[mtx_stack_top - 1];
}

int* mtx_trans(int* d, int* v) {
	mtx_iden(d);
	d[3]  = v[0];
	d[7]  = v[1];
	d[11] = v[2];
	return d;
}

int* mtx_rot_x(int* d, int a) {
	mtx_iden(d);
	a &= sin_table_mask;
	d[5]  =  cos_table[a];
	d[6]  = -sin_table[a];
	d[9]  =  sin_table[a];
	d[10] =  cos_table[a];
	return d;
}

int* mtx_rot_y(int* d, int a) {
	mtx_iden(d);
	a &= sin_table_mask;
	d[0]  =  cos_table[a];
	d[2]  =  sin_table[a];
	d[8]  = -sin_table[a];
	d[10] =  cos_table[a];
	return d;
}

int* mtx_rot_z(int* d, int a) {
	mtx_iden(d);
	a &= sin_table_mask;
	d[0]  =  cos_table[a];
	d[1]  = -sin_table[a];
	d[4]  =  sin_table[a];
	d[5]  =  cos_table[a];
	return d;
}

int* mtx_apply(const int* m, int* v) {
	int s[4];
	vec_cpy(s, v, 4);
#define mul(a, b) \
	((a * b) >> fbits)
#define dot(x1, y1, z1, w1, x2, y2, z2, w2) \
	mul(x1, x2) + mul(y1, y2) + mul(z1, z2) + mul(w1, w2)
	v[0] = dot(s[0],s[1],s[2],s[3], m[0], m[1], m[2], m[3]);
	v[1] = dot(s[0],s[1],s[2],s[3], m[4], m[5], m[6], m[7]);
	v[2] = dot(s[0],s[1],s[2],s[3], m[8], m[9], m[10],m[11]);
	v[3] = dot(s[0],s[1],s[2],s[3], m[12],m[13],m[14],m[15]);
#undef mul
#undef dot
	return v;
}

int* vec_cpy(int* d, const int* s, int c) {
	int i;
	for (i = 0; i < c; i++) d[i] = s[i];
	return d;
}

int* vec_ref(int* d, const int* i, const int* n, int c) {
	int j;
	int dp = 2 * vec_dot(n, i, c);
	for (j = 0; j < c; j++) {
		d[j] = i[j] - ((dp * n[j]) >> fbits);
	}
	return d;
}

int vec_dot(const int* a, const int* b, int d) {
	int r, i;
	for (i = r = 0; i < d; i++) {
		r += a[i] * b[i] >> fbits;
	}
	return r;
}

int* persp(int* v, int asp) {
	v[2] += !v[2];
	v[0] = ((v[0] << fbits) / v[2]);
	v[1] = ((v[1] << fbits) / v[2]) * asp >> fbits;
	return v;
}

int* ndc2clip(const int* c, int* p) {
	register int hw = c[2] >> 1;
	register int hh = c[3] >> 1;
	p[0] = ((hw << fbits) *  p[0]) >> fbits;
	p[1] = ((hh << fbits) * -p[1]) >> fbits;
	p[0] >>= fbits;
  p[1] >>= fbits;
	p[0] += hw;
	p[1] += hh;
	return p;
}