/* sensors.c */

#include "mpu6050.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_start_fns[])(void) = {
	mpu6050_start_sample,
};

#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;

float sensors_gyro_roll;
float sensors_gyro_pitch;
float sensors_gyro_yaw;

float sensors_temp;

float sensors_accel_x;
float sensors_accel_y;
float sensors_accel_z;

float gyro_yaw_zero;
float gyro_pitch_zero;
float gyro_roll_zero;

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)
{
	next_sensor = 0;

	if (!mpu6050_init())
		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(float roll, float pitch, float yaw)
{
#if 0
	sensors_gyro_roll = roll - gyro_roll_zero;
	sensors_gyro_pitch = pitch - gyro_pitch_zero;
	sensors_gyro_yaw = yaw - gyro_yaw_zero;
#else
	sensors_gyro_roll = roll;
	sensors_gyro_pitch = pitch;
	sensors_gyro_yaw = yaw;
#endif
}

void sensors_write_accel_data(float x, float y, float z)
{
	sensors_accel_x = x;
	sensors_accel_y = y;
	sensors_accel_z = z;
}

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_roll);
	log_put_float(sensors_gyro_pitch);
	log_put_float(sensors_gyro_yaw);
	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
		dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll,
		    -sensors_gyro_yaw);
#else
		dcm_update(0.0, 0.0, 0.0);
#endif
		if (!status_armed()) {
			if ( (abs(sensors_gyro_roll)  < GYRO_RATE_THRESHOLD) &&
			     (abs(sensors_gyro_pitch) < GYRO_RATE_THRESHOLD) &&
			     (abs(sensors_gyro_yaw)   < 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_yaw_zero += sensors_gyro_yaw;
			gyro_pitch_zero += sensors_gyro_pitch;
			gyro_roll_zero += sensors_gyro_roll;
			sensors_generation++;
			if (sensors_generation >= GYRO_ZERO_COUNT) {
				sensors_zero = FALSE;
				sensors_update = TRUE;
				sensors_generation = 0;
				gyro_yaw_zero /= GYRO_ZERO_COUNT;
				gyro_pitch_zero /= GYRO_ZERO_COUNT;
				gyro_roll_zero /= GYRO_ZERO_COUNT;
				putstr("Zero finished\r\n");
				status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
			}
		}
	}
	watchdog_kick(WATCHDOG_GYRO);

#if 1
	dcm_drift_correction(-sensors_accel_y, -sensors_accel_x,
		-sensors_accel_z);
#endif
#if 0
	dcm_drift_correction(sensors_accel_x, sensors_accel_y,
		sensors_accel_z);
#endif

	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_roll  * 1000.0));
	putstr(",");
	putint_s((int)(sensors_gyro_pitch * 1000.0));
	putstr(",");
	putint_s((int)(sensors_gyro_yaw   * 1000.0));
	putstr(")");

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