/* mpu6050.c */

#include "sensors.h"
#include "mpu6050.h"
#include "i2c.h"
#include "uart.h"
#include "dcm.h"
#include "fisqrt.h"
#include "stick.h"
#include "watchdog.h"
#include "status.h"
#include "abs.h"
#include "event.h"
#include "timer.h"
#include "panic.h"
#include "log.h"

i2c_result mpu6050_result;
unsigned int mpu6050_generation;

signed int gyro_zero_roll;
signed int gyro_zero_pitch;
signed int gyro_zero_yaw;

#define GYRO_ZERO_COUNT 1000

unsigned int mpu6050_gyro_zero_count;

unsigned char mpu6050_sample_data[14];

/*unsigned char mpu6050_whoami_command[1] = {0x75}; */
unsigned char mpu6050_whoami_command[1] = {0x6B};

struct i2c_transaction mpu6050_whoami_transaction2;

struct i2c_transaction mpu6050_whoami_transaction = {
	(0x68 << 1) + 0, /* write */
	1,
	mpu6050_whoami_command,
	&mpu6050_result,
	EVENT_MPU6050_I2C_COMPLETE,
	&mpu6050_whoami_transaction2
};

struct i2c_transaction mpu6050_whoami_transaction2 = {
	(0x68 << 1) + 1, /* read */
	1,
	mpu6050_sample_data,
	&mpu6050_result,
	EVENT_MPU6050_I2C_COMPLETE,
	NULL
};

/* Accelerometer scaling */
#define AFS_SEL 2


unsigned char mpu6050_init_command[] = {0x6B, 0x01};
unsigned char mpu6050_accel_init_command[] = {0x1c, (AFS_SEL<<3)};

struct i2c_transaction mpu6050_accel_init_transaction = {
	(0x68 << 1) + 0, /* write */
	2,
	mpu6050_accel_init_command,
	&mpu6050_result,
	EVENT_MPU6050_I2C_COMPLETE,
	NULL
};

struct i2c_transaction mpu6050_init_transaction = {
	(0x68 << 1) + 0, /* write */
	2,
	mpu6050_init_command,
	&mpu6050_result,
	EVENT_MPU6050_I2C_COMPLETE,
	&mpu6050_accel_init_transaction
};

unsigned char mpu6050_sample_command[] = {0x3B};

struct i2c_transaction mpu6050_sample_transaction2;

struct i2c_transaction mpu6050_sample_transaction = {
	(0x68 << 1) + 0, /* write */
	1,
	mpu6050_sample_command,
	&mpu6050_result,
	EVENT_MPU6050_I2C_COMPLETE,
	&mpu6050_sample_transaction2
};

struct i2c_transaction mpu6050_sample_transaction2 = {
	(0x68 << 1) + 1, /* read */
	14,
	mpu6050_sample_data,
	&mpu6050_result,
	EVENT_MPU6050_I2C_COMPLETE,
	NULL
};

void mpu6050_event_handler(void);

bool mpu6050_init(void)
{
	event_register(EVENT_MPU6050_I2C_COMPLETE, mpu6050_event_handler);

	if (!i2c_start_transaction(&mpu6050_init_transaction))
		return FALSE;
	while (i2c_busy()) ;

	return (mpu6050_result == I2C_SUCCESS);
}

/* LSB / g */
/* 1 for +- 2g */
/* 2 for +- 4g */
/* 4 for +- 8g */
/* 8 for +- 16g */
#define ACCEL_STEP	(16384.0 / (float)(1<<AFS_SEL))

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

/* A step of 131 = 1 degree. */
/* Overall, this is LSB / rad/s */
#define GYRO_STEP	(131.0 / DEG_TO_RAD)

/* LSB / degree C */
#define TEMP_STEP	340.0
#define TEMP_OFFSET	36.53

#define ACCEL_SCALE (1.0 / ACCEL_STEP)
#define GYRO_SCALE  (1.0 / GYRO_STEP)
#define TEMP_SCALE  (1.0 / TEMP_STEP)

#if 0
bool mpu6050_sample(void)
{
	unsigned int x, y, z;
	unsigned int temp;
	unsigned int roll, pitch, yaw;

	if (!i2c_start_transaction(&mpu6050_sample_transaction))
		return FALSE;

	while (i2c_busy());

	if (mpu6050_result != I2C_SUCCESS)
		return FALSE;

	mpu6050_result = I2C_IN_PROGRESS;

	x = (mpu6050_sample_data[0] << 8) + mpu6050_sample_data[1];
	y = (mpu6050_sample_data[2] << 8) + mpu6050_sample_data[3];
	z = (mpu6050_sample_data[4] << 8) + mpu6050_sample_data[5];

	temp = (mpu6050_sample_data[6] << 8) + mpu6050_sample_data[7];

	roll  = (mpu6050_sample_data[ 8] << 8) + mpu6050_sample_data[ 9];
	pitch = (mpu6050_sample_data[10] << 8) + mpu6050_sample_data[11];
	yaw   = (mpu6050_sample_data[12] << 8) + mpu6050_sample_data[13];

	putstr("MPU6050 sample data:\r\n");
	putstr("x: ");
	puthex(x);
	putstr(", y: ");
	puthex(y);
	putstr(", z: ");
	puthex(z);
	putstr("\r\ntemperature:");
	puthex(temp);
	putstr("\r\nroll:");
	puthex(roll);
	putstr(", pitch:");
	puthex(pitch);
	putstr(", yaw:");
	puthex(yaw);
	putstr("\r\n\r\n");
#if 0
	sensors_write_gyro_data(roll, pitch, yaw);
	sensors_write_accel_data(x, y, z);
#endif

	return TRUE;
}
#endif

bool mpu6050_start_sample(void)
{
	return i2c_start_transaction(&mpu6050_sample_transaction);
}

void mpu6050_start_zero(void)
{
	mpu6050_gyro_zero_count = GYRO_ZERO_COUNT;
	gyro_zero_roll = 0;
	gyro_zero_pitch = 0;
	gyro_zero_yaw = 0;
}

void mpu6050_event_handler(void)
{
	signed short int xi, yi, zi;
	signed short int tempi;
	signed short int rolli, pitchi, yawi;

	float x, y, z;
	float temp;
	float roll, pitch, yaw;

	CHECKPOINT(9);

	if (mpu6050_result != I2C_SUCCESS)
		return;

	mpu6050_result = I2C_IN_PROGRESS;

	sensors_sample_done();

	xi = (mpu6050_sample_data[0] << 8) + mpu6050_sample_data[1];
	yi = (mpu6050_sample_data[2] << 8) + mpu6050_sample_data[3];
	zi = (mpu6050_sample_data[4] << 8) + mpu6050_sample_data[5];

	tempi = (mpu6050_sample_data[6] << 8) + mpu6050_sample_data[7];

	rolli  = (mpu6050_sample_data[ 8] << 8)+mpu6050_sample_data[ 9];
	pitchi = (mpu6050_sample_data[10] << 8)+mpu6050_sample_data[11];
	yawi   = (mpu6050_sample_data[12] << 8)+mpu6050_sample_data[13];

	if (mpu6050_gyro_zero_count) {
		gyro_zero_roll  += rolli;
		gyro_zero_pitch += pitchi;
		gyro_zero_yaw   += yawi;
		if (--mpu6050_gyro_zero_count == 0) {
			gyro_zero_roll  /= GYRO_ZERO_COUNT;
			gyro_zero_pitch /= GYRO_ZERO_COUNT;
			gyro_zero_yaw   /= GYRO_ZERO_COUNT;
		}
	} else {
		rolli  -= gyro_zero_roll;
		pitchi -= gyro_zero_pitch;
		yawi   -= gyro_zero_yaw;
	}

	x = ((float)xi) * ACCEL_SCALE;
	y = ((float)yi) * ACCEL_SCALE;
	z = ((float)zi) * ACCEL_SCALE;

	temp = ((float)tempi) * TEMP_SCALE + TEMP_OFFSET;

	roll  = ((float)rolli)  * GYRO_SCALE;
	pitch = ((float)pitchi) * GYRO_SCALE;
	yaw   = ((float)yawi)   * GYRO_SCALE;

	sensors_write_gyro_data(roll, pitch, yaw);
	sensors_write_accel_data(x, y, z);
	sensors_write_temp_data(temp);

	log_put_array((char *)mpu6050_sample_data, 14);

	CHECKPOINT(10);
}