#include "imu_processing.h"
#include "biquad_filter.h"
#include "math.h"



static float gyro_bias_x = 0.0f;
static float gyro_bias_y = 0.0f;
static float gyro_bias_z = 0.0f;

static float accel_bias_x = 0.0f;
static float accel_bias_y = 0.0f;
static float accel_bias_z = 0.0f;

static biquad_t accel_x_lpf;
static biquad_t accel_y_lpf;
static biquad_t accel_z_lpf;

static biquad_t gyro_x_lpf;
static biquad_t gyro_y_lpf;
static biquad_t gyro_z_lpf;

const float accel_min = -8192.0f;
const float accel_max = 8192.0f;

static int16_t x_calib;
static int16_t y_calib;
static int16_t z_calib;
static long gyro_shift[3] = {0, 0, 0};
static float acc;

void imu_processing_init()
{
	biquad_init_lpf(&accel_x_lpf, 25.0f, 500.0f);
	biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f);
	biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f);
	
	biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f);
	biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f);
	biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
}

void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
{
	static imu_raw_t raw;
	
	imu_read_raw(&raw);
	
	raw.ax = biquad_apply(&accel_x_lpf, raw.ax);
	raw.ay = biquad_apply(&accel_y_lpf, raw.ay);
	raw.az = biquad_apply(&accel_z_lpf, raw.az);
	
	raw.gx = biquad_apply(&gyro_x_lpf, raw.gx);
	raw.gy = biquad_apply(&gyro_y_lpf, raw.gy);
	raw.gz = biquad_apply(&gyro_z_lpf, raw.gz);
	
	if (allowed_calib) imu_calib(&raw, 50, 8192, 1000);
	
	/*float alpha = 0.01;
	
	out->ax = out->ax * (1.0f - alpha) + raw.ax * alpha;
	out->ay = out->ay * (1.0f - alpha) + raw.ay * alpha;
	out->az = out->az * (1.0f - alpha) + raw.az * alpha;
	
	out->gx = out->gx * (1.0f - alpha) + raw.gx * alpha;
	out->gy = out->gy * (1.0f - alpha) + raw.gy * alpha;
	out->gz = out->gz * (1.0f - alpha) + raw.gz * alpha;*/
	
	out->ax = normalize(raw.ax, 1.0f, accel_min, accel_max);
	out->ay = normalize(raw.ay, 1.0f, accel_min, accel_max);
	out->az = normalize(raw.az, 1.0f, accel_min, accel_max);
	
	out->gx = (raw.gx - gyro_shift[0]) / GYRO_SENS_SCALE_FACTOR;
	out->gy = (raw.gy - gyro_shift[1]) / GYRO_SENS_SCALE_FACTOR;
	out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
	
	////////////////////////////////////////////////////////////
	/*out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
	out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
	out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
	
	out->ax = biquad_apply(&accel_x_lpf, out->ax);
	out->ay = biquad_apply(&accel_y_lpf, out->ay);
	out->az = biquad_apply(&accel_z_lpf, out->az);
	
	out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
	out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
	out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
	
	out->gx = biquad_apply(&gyro_x_lpf, out->gx);
	out->gy = biquad_apply(&gyro_y_lpf, out->gy);
	out->gz = biquad_apply(&gyro_z_lpf, out->gz);*/
}

void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
{
	long len = imu->gx*imu->gx + imu->gy*imu->gy + imu->gz*imu->gz;
	if (len > gyrLim*gyrLim) return;
	
	x_calib = imu->gx, y_calib = imu->gy, z_calib = imu->gz;
	const float alpha = 0.001f;
	
	x_calib = x_calib * (1.0f - alpha) + imu->gx * alpha;
	y_calib = y_calib * (1.0f - alpha) + imu->gy * alpha;
	z_calib = z_calib * (1.0f - alpha) + imu->gz * alpha;
	
	gyro_shift[0] = x_calib;
	gyro_shift[1] = y_calib;
	gyro_shift[2] = z_calib;
	
	len = imu->ax*imu->ax + imu->ay*imu->ay + imu->az*imu->az;
	if (absl(len - accZero*accZero) > accMax*accMax) return;
	
	len = sqrtf(len);
	
	acc = len;
	
	acc = acc * (1.0f - alpha) + len * alpha;
}

void imu_calibrate()
{
	const int samples = 1000;
	
	float sum_ax = 0;
	float sum_ay = 0;
	float sum_az = 0;
	
	float sum_gx = 0;
	float sum_gy = 0;
	float sum_gz = 0;
	
	imu_raw_t imu;
	
	for (uint16_t i = 0; i < samples; ++i)
	{
		imu_read_raw(&imu);
		
		sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
		sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
		sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
		
		sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
		sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
		sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
		
		for (volatile uint16_t d = 0; d < 5000; ++d);
	}
	
	accel_bias_x = sum_ax / samples;
	accel_bias_y = sum_ay / samples;
	accel_bias_z = (sum_az / samples) + 1.0f;
	
	gyro_bias_x = sum_gx / samples;
	gyro_bias_y = sum_gy / samples;
	gyro_bias_z = sum_gz / samples;
}

float normalize(float value, float scale, float min, float max)
{
	const float len = (max - min) / 2.0f;
	const float shift = (max + min) / 2.0f;
	return (value - shift) * scale / len;
}

long absl(long value)
{
	if (value < 0) return -value;
	return value;
}