#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <kalmia.h>
#include <ezxml.h>
#include "transform.h"


#define TWO_PI 6.2831853071796f


int kai_identity(ka_matrix_t *m)
{
	memset(*m, 0, sizeof(ka_matrix_t));
	(*m)[0] = 1.0f;
	(*m)[5] = 1.0f;
	(*m)[10] = 1.0f;
	(*m)[15] = 1.0f;

	return 0;
}


int kai_multiply(ka_matrix_t *dest, ka_matrix_t A, ka_matrix_t B)
{
	ka_matrix_t C;
	memset(C, 0, sizeof(ka_matrix_t));

	int i, j, k;
	int a_ind, b_ind, c_ind;
	for (i=0; i<4; i++) {
		for (j=0; j<4; j++) {
			c_ind = (4*i) + j;
			for (k=0; k<4; k++) {
				a_ind = (4*i) + k;
				b_ind = (4*k) + j;
				C[c_ind] += A[a_ind] * B[b_ind];
			}
		}
	}

	memcpy(*dest, C, sizeof(ka_matrix_t));
	return 0;
}


int kai_parse_matrix(ka_matrix_t *m, ezxml_t tag)
{
	if (strcmp("matrix", ezxml_name(tag)) != 0)
		return -1;
	
	char *str = ezxml_txt(tag);
	char *end;
	int i;
	for (i=0; i<16; i++) {
		(*m)[i] = strtod(str, &end);
		str = end;
	}
	
	return 0;
}


int kai_parse_rotate(ka_matrix_t *m, ezxml_t tag)
{
	if (strcmp("rotate", ezxml_name(tag)) != 0)
		return -1;
	
	char *str = ezxml_txt(tag);
	char *end;
	int i;
	double rot[4];
	for (i=0; i<4; i++) {
		rot[i] = strtod(str, &end);
		str = end;
	}

	double x, y, z, angle;
	x = rot[0];
	y = rot[1];
	z = rot[2];
	angle = rot[3];

	/* normalize length */
	double axis_len = (x*x) + (y*y) + (z*z);
	axis_len = sqrt(axis_len);
	x /= axis_len;
	y /= axis_len;
	z /= axis_len;

	/* convert angle to radians */
	angle *= TWO_PI / 360;

	/* compute rotation matrix entries */
	double co = cos(angle);
	double unco = 1 - co;
	double si = sin(angle);

	(*m)[0] = (x*x*unco) + co;
	(*m)[1] = (x*y*unco) - (z*si);
	(*m)[2] = (x*z*unco) + (y*si);
	(*m)[3] = 0.0f;
	(*m)[4] = (y*x*unco) + (z*si);
	(*m)[5] = (y*y*unco) + co;
	(*m)[6] = (y*z*unco) - (x*si);
	(*m)[7] = 0.0f;
	(*m)[8] = (z*x*unco) - (y*si);
	(*m)[9] = (z*y*unco) + (x*si);
	(*m)[10] = (z*z*unco) + co;
	(*m)[11] = 0.0f;
	(*m)[12] = 0.0f;
	(*m)[13] = 0.0f;
	(*m)[14] = 0.0f;
	(*m)[15] = 1.0f;

	return 0;
}


int kai_parse_scale(ka_matrix_t *m, ezxml_t tag)
{
	if (strcmp("scale", ezxml_name(tag)) != 0)
		return -1;
	
	char *str = ezxml_txt(tag);
	char *end;
	int i;
	double scale[3];
	for (i=0; i<3; i++) {
		scale[i] = strtod(str, &end);
		str = end;
	}

	memset(*m, 0, 16*sizeof(double));
	(*m)[0] = scale[0];
	(*m)[5] = scale[1];
	(*m)[10] = scale[2];
	(*m)[15] = 1.0f;
	
	return 0;
}


int kai_parse_translate(ka_matrix_t *m, ezxml_t tag)
{
	if (strcmp("translate", ezxml_name(tag)) != 0)
		return -1;

	char *str = ezxml_txt(tag);
	char *end;
	int i;
	double move[3];
	for (i=0; i<3; i++) {
		move[i] = strtod(str, &end);
		str = end;
	}

	memset(*m, 0, sizeof(ka_matrix_t));

	(*m)[0] = 1.0f;
	(*m)[3] = move[0];
	(*m)[5] = 1.0f;
	(*m)[7] = move[1];
	(*m)[10] = 1.0f;
	(*m)[11] = move[2];
	(*m)[15] = 1.0f;
	
	return 0;
}