/* sensors.c */

#include "mpu6050.h"
#include "hmc5883l.h"
#include "mpl3115a2.h"
#include "dcm.h"
#include "fisqrt.h"
#include "watchdog.h"
#include "status.h"
#include "abs.h"
#include "panic.h"
#include "uart.h"
#include "log.h"
#include "stick.h"

bool (*sensor_init_fns[])(void) = {
	mpu6050_init,
	hmc5883l_init,
	mpl3115a2_init,
};

bool (*sensor_start_fns[])(void) = {
	mpu6050_start_sample,
	hmc5883l_start_sample,
	mpl3115a2_start_sample,
};

#define SENSOR_INIT_FNS (sizeof(sensor_init_fns)/sizeof(sensor_init_fns[0]))
#define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0]))

static unsigned int next_sensor;

static bool sensors_zero;
static bool sensors_update;
static unsigned int sensors_discard;
static unsigned int sensors_generation;

/* x = roll
   y = pitch
   z = yaw
 */
vec3f sensors_gyro;
vec3f gyro_zero;

float sensors_temp;

vec3f sensors_accel;

void sensors_write_log(void);
void sensors_process(void);

#define TWO_PI 6.28318531f
#define DEG_TO_RAD (TWO_PI/360.0f)

/* The gyro has to stay within this limit in each axis in order to arm */
#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD)

#define GYRO_ZERO_COUNT 1000

void sensors_dump(void);

bool sensors_init(void)
{
	unsigned int i;

	next_sensor = 0;

	for (i = 0; i < SENSOR_INIT_FNS; i++)
		if (!(sensor_init_fns[i])())
			return FALSE;

	return TRUE;
}

bool sensors_next_sample(void)
{
	bool result;

	result = (sensor_start_fns[next_sensor++])();
	if (next_sensor >= SENSOR_START_FNS)
		next_sensor = 0;

	return result;
}

void sensors_sample_done(void)
{
	if (next_sensor == 0) {
		sensors_write_log();
		return;
	}

	if (!sensors_next_sample())
		panic(PANIC_SENSOR_FAIL);
}

bool sensors_start_sample(void)
{
	next_sensor = 0;
	return sensors_next_sample();
}

void sensors_write_gyro_data(vec3f gyro)
{
#if 0
	sensors_gyro.x = gyro.x - gyro_zero.x;
	sensors_gyro.y = gyro.y - gyro_zero.y;
	sensors_gyro.z = gyro.z - gyro_zero.z;
#else
	sensors_gyro = gyro;
#endif
}

void sensors_write_accel_data(vec3f accel)
{
	sensors_accel = accel;
}

void sensors_write_temp_data(float temp)
{
	sensors_temp = temp;
	/* XXX HACK find a better place for this call */
	sensors_process();
}

#define LOG_SIGNATURE_SENSORS 0xDA7ADA7A
#define LOG_SIGNATURE_SENSORS2 0xDA7AF173
void sensors_write_log(void)
{
#if 0
	log_put_uint(LOG_SIGNATURE_SENSORS);
	log_put_float(sensors_accel.x);
	log_put_float(sensors_accel.y);
	log_put_float(sensors_accel.z);
	log_put_float(sensors_gyro.x);
	log_put_float(sensors_gyro.y);
	log_put_float(sensors_gyro.z);
	log_put_float(sensors_temp);
#else
	/* XXX this just about comes out in the right place, but by luck */
	log_put_uint(LOG_SIGNATURE_SENSORS2);
#endif
}

void sensors_start_zero(void)
{
	sensors_zero = TRUE;
	sensors_update = FALSE;
	sensors_discard = 100;
	sensors_generation = 0;
	putstr("Starting zero\r\n");
	mpu6050_start_zero();
}

void sensors_process(void)
{
	if (sensors_update) {
#if 1
		vec3f dcm_gyro = {-sensors_gyro.y, -sensors_gyro.x, -sensors_gyro.z};
#else
		vec3f dcm_gyro = {0.0, 0.0, 0.0};
#endif
		dcm_update(dcm_gyro);
		if (!status_armed()) {
			if ( (abs(sensors_gyro.x)  < GYRO_RATE_THRESHOLD) &&
			     (abs(sensors_gyro.y)  < GYRO_RATE_THRESHOLD) &&
			     (abs(sensors_gyro.z)  < GYRO_RATE_THRESHOLD)) {
			    status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE);
			} else {
			    status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE);
			}
		}

		sensors_generation++;

#if SEND_DCM
		if ((sensors_generation % 40) == 0) {
			dcm_send_packet();
			sensors_dump();
		}
#endif

	} else if (sensors_zero) {
		if (sensors_discard) {
			sensors_discard--;
		} else {
			gyro_zero.z += sensors_gyro.z;
			gyro_zero.y += sensors_gyro.y;
			gyro_zero.x += sensors_gyro.x;
			sensors_generation++;
			if (sensors_generation >= GYRO_ZERO_COUNT) {
				sensors_zero = FALSE;
				sensors_update = TRUE;
				sensors_generation = 0;
				gyro_zero.z /= GYRO_ZERO_COUNT;
				gyro_zero.y /= GYRO_ZERO_COUNT;
				gyro_zero.x /= GYRO_ZERO_COUNT;
				putstr("Zero finished\r\n");
				status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
			}
		}
	}
	watchdog_kick(WATCHDOG_GYRO);

#if 1
	vec3f dcm_accel = {-sensors_accel.y, -sensors_accel.x, -sensors_accel.z};
#endif
#if 0
	vec3f dcm_accel = {sensors_accel.x, sensors_accel.y, sensors_accel.z};
#endif
	dcm_drift_correction(dcm_accel);

	watchdog_kick(WATCHDOG_ACCEL);
	stick_input();
}

void sensors_dump(void)
{
	putstr("(");
	putint_s((int)(sensors_accel.x * 1000.0));
	putstr(",");
	putint_s((int)(sensors_accel.y * 1000.0));
	putstr(",");
	putint_s((int)(sensors_accel.z * 1000.0));
	putstr(")");

	putstr("(");
	putint_s((int)(sensors_gyro.x * 1000.0));
	putstr(",");
	putint_s((int)(sensors_gyro.y * 1000.0));
	putstr(",");
	putint_s((int)(sensors_gyro.z * 1000.0));
	putstr(")");

	putstr("(");
	putint_s((int)(sensors_temp  * 1000.0));
	putstr(")\r\n");
}