/* dcm.c */

#ifdef WE_HAVE_SQRT
#include <math.h>
#else
#include "fisqrt.h"
#endif
#include "matrix.h"
#include "dcm.h"
#include "uart.h"
#include "motor.h"
#include "status.h"
#include "abs.h"
#include "log.h"

#if 0
#define GRAVITY 9.80665f
#endif

#define GRAVITY 1.0f

#define KP_ROLLPITCH 0.05967
#define KI_ROLLPITCH 0.00001278

#define ERROR_LIMIT 1.17f

/* Maximum allowed error for arming */
#define ERROR_THRESHOLD 0.20f

#define LOG_MAGIC_DCM_UPDATE 0x00DC111A
#define LOG_MAGIC_DCM_DRIFT 0x00DC111B


/* 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 omega_p[3] = {0.0, 0.0, 0.0};
float omega_i[3] = {0.0, 0.0, 0.0};

float omega_x, omega_y, omega_z;

float delta_t = 0.005;

void dcm_log(unsigned int magic)
{
	int i;
	log_put_uint(magic);
	for (i = 0; i < 9; i++)
		log_put_float(dcm[i]);
}

void dcm_update(float x, float y, float z)
{
	omega_x = x + omega_i[0] + omega_p[0];
	omega_y = y + omega_i[1] + omega_p[1];
	omega_z = z + omega_i[2] + omega_p[2];

	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();

/*	dcm_log(LOG_MAGIC_DCM_UPDATE); */
}

void dcm_setvector(float x, float y, float z)
{
	/* We're given the Z axis */
	dcm[6] = x;
	dcm[7] = y;
	dcm[8] = z;

	/* Second row = cross product of unit X and third rows */
	dcm[3] = 0.0;
	dcm[4] = -dcm[8];
	dcm[5] = dcm[7];

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

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

	dcm_renormalise(dcm+0);
	dcm_renormalise(dcm+3);
	dcm_renormalise(dcm+6);
#if 0
	dcm_normalise();
#endif
}

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);
	} else if (f < 100.0f && f > 0.01f) {
#ifdef WE_HAVE_SQRT
		f = 1.0 / sqrt(f);
#else
		f = fisqrt(f);
#endif
		/* XXX log this event? */
		putstr("sqrt\r\n");
	} 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_drift_correction(float x, float y, float z)
{
	float mag;
	float weight;
	float error[3];
	int i;

	mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY;

	mag = 1-mag;
	if (mag < 0.0)
		mag = -mag;

	weight = 1 - 3*mag;

	if (weight < 0.0)
		weight = 0.0;
	if (weight > 1.0)
		weight = 1.0;

	/* error = cross product of dcm last row and acceleration vector */
	/* third row = cross product of first two rows */
	error[0] = dcm[7]*z - dcm[8]*y;
	error[1] = dcm[8]*x - dcm[6]*z;
	error[2] = dcm[6]*y - dcm[7]*x;

	if (!status_armed()) {
		if ((abs(error[0]) < ERROR_THRESHOLD) &&
		    (abs(error[1]) < ERROR_THRESHOLD) &&
		    (abs(error[2]) < ERROR_THRESHOLD))
			status_set_ready(STATUS_MODULE_DCM_ERROR, TRUE);
		else
			status_set_ready(STATUS_MODULE_DCM_ERROR, FALSE);
	}

	for (i = 0; i < 3; i++) {
		if (error[i] > ERROR_LIMIT)
			error[i] = ERROR_LIMIT;
		if (error[i] < -ERROR_LIMIT)
			error[i] = -ERROR_LIMIT;
	}

	for (i = 0; i < 3; i++) {
		omega_p[i] = error[i] * (KP_ROLLPITCH * weight);
		omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
	}

	dcm_log(LOG_MAGIC_DCM_DRIFT);

#if 0
	putstr("w: ");
	putint_s((int)(weight * 100000.0f));
	putstr("\r\n");
#endif

#if 0
	putstr("p: ");
	putint_s((int)(omega_p[0] * 100000.0f));
	putstr(", ");
	putint_s((int)(omega_p[1] * 100000.0f));
	putstr(", ");
	putint_s((int)(omega_p[2] * 100000.0f));
	putstr(" i: ");
	putint_s((int)(omega_i[0] * 100000.0f));
	putstr(", ");
	putint_s((int)(omega_i[1] * 100000.0f));
	putstr(", ");
	putint_s((int)(omega_i[2] * 100000.0f));
	putstr("\r\n");
#endif
}

/* Maximum angle to the horizontal for arming: 30 degrees */
#define ATTITUDE_THRESHOLD (0.5)

void dcm_attitude_error(float roll, float pitch, float yaw)
{
	/* dcm[6] = sine of pitch */
	/* dcm[7] = sine of roll */

	/* pitch error = pitch - dcm[6] */
	/* roll error = roll - dcm[7] */

	/* That was the theory. In practice, there appears to be some
	   confusion over axes. Pitch and roll seem.. reversed. */

	/* TODO: What if we are upside down? */

	if (!status_armed()) {
		if ((abs(dcm[6]) < ATTITUDE_THRESHOLD) &&
		    (abs(dcm[7]) < ATTITUDE_THRESHOLD))
			status_set_ready(STATUS_MODULE_ATTITUDE, TRUE);
		else
			status_set_ready(STATUS_MODULE_ATTITUDE, FALSE);
	}

	motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z);
}

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");
}