/* dcm.c */

#ifdef WE_HAVE_SQRT
#include <math.h>
#endif
#include "matrix.h"
#include "dcm.h"
#include "uart.h"

/* Implementation of the DCM IMU concept as described by Premerlani
 * and Bizard
 */

float dcm[3*3] = {1, 0, 0,
		  0, 1, 0,
		  0, 0, 1};

float delta_t = 0.01;

void dcm_update(float omega_x, float omega_y, float omega_z)
{
	float tx = delta_t * omega_x;
	float ty = delta_t * omega_y;
	float tz = delta_t * omega_z;

	float update_matrix[3*3] = { 0, -tz,  ty,
				    tz,   0, -tx,
				   -ty,  tx,   0};
	float temp_matrix[3*3];

	matrix_multiply(temp_matrix, dcm, update_matrix, 3, 3, 3);
	matrix_add(dcm, dcm, temp_matrix, 3, 3);

	dcm_normalise();
}

void dcm_normalise(void)
{
	float error;
	float tmp[6];
	int i;

	/* dot product of first two rows */
	error = dcm[0]*dcm[3] + dcm[1]*dcm[4] + dcm[2]*dcm[5];

	/* printf("error is %f\n", error); */

	tmp[0] = dcm[3];
	tmp[1] = dcm[4];
	tmp[2] = dcm[5];
	tmp[3] = dcm[0];
	tmp[4] = dcm[1];
	tmp[5] = dcm[2];

	for (i = 0; i < 6; i++)
		dcm[i] = dcm[i] - (tmp[i] * (0.5f * error));

	/* third row = cross product of first two rows */
	dcm[6] = dcm[1]*dcm[5] - dcm[2]*dcm[4];
	dcm[7] = dcm[2]*dcm[3] - dcm[0]*dcm[5];
	dcm[8] = dcm[0]*dcm[4] - dcm[1]*dcm[3];

	if (!(dcm_renormalise(dcm+0) &&
	      dcm_renormalise(dcm+3) &&
	      dcm_renormalise(dcm+6))) {
		/* Shit. I've been shot. */
		dcm[0] = dcm[4] = dcm[8] = 1.0f;
		dcm[1] = dcm[2] = dcm[3] = 0.0f;
		dcm[5] = dcm[6] = dcm[7] = 0.0f;
	}
}

bool dcm_renormalise(float *v)
{
	float f = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];

	/* printf("f is %f\n", f); */

	if (f < 1.5625f && f > 0.64f) {
		f = 0.5 * (3 - f);
#ifdef WE_HAVE_SQRT
	} else if (f < 100.0f && f > 0.01f) {
		f = 1.0 / sqrt(f);
		/* XXX log this event? */
#endif
	} else {
		putstr("problem\r\n");
		return FALSE;
	}

	v[0] = v[0] * f;
	v[1] = v[1] * f;
	v[2] = v[2] * f;

	return TRUE;
}

void dcm_dump(void)
{
	putstr("dcm: ");
	putint_s((int)(dcm[0] * 100000.0f));
	putstr(", ");
	putint_s((int)(dcm[1] * 100000.0f));
	putstr(", ");
	putint_s((int)(dcm[2] * 100000.0f));
	putstr("\r\n     ");
	putint_s((int)(dcm[3] * 100000.0f));
	putstr(", ");
	putint_s((int)(dcm[4] * 100000.0f));
	putstr(", ");
	putint_s((int)(dcm[5] * 100000.0f));
	putstr("\r\n     ");
	putint_s((int)(dcm[6] * 100000.0f));
	putstr(", ");
	putint_s((int)(dcm[7] * 100000.0f));
	putstr(", ");
	putint_s((int)(dcm[8] * 100000.0f));
	putstr("\r\n");
}

void puthexfloat(float f)
{
	union {
		float f;
		unsigned int i;
	} u;

	u.f = f;
	puthex(u.i);
}

void dcm_send_packet(void)
{
	putstr("D:(");
	puthexfloat(dcm[0]);
	putstr(",");
	puthexfloat(dcm[1]);
	putstr(",");
	puthexfloat(dcm[2]);
	putstr(",");
	puthexfloat(dcm[3]);
	putstr(",");
	puthexfloat(dcm[4]);
	putstr(",");
	puthexfloat(dcm[5]);
	putstr(",");
	puthexfloat(dcm[6]);
	putstr(",");
	puthexfloat(dcm[7]);
	putstr(",");
	puthexfloat(dcm[8]);
	putstr(")\r\n");
}