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

static float accel_bias_x = 0.0f;
static float accel_bias_y = 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 = -4096.0f;
const float accel_max = 4096.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 -= accel_bias_x;
  raw.ay -= accel_bias_y;
	
	raw.ax = (uint16_t) biquad_apply(&accel_x_lpf, raw.ax);
	raw.ay = (uint16_t) biquad_apply(&accel_y_lpf, raw.ay);
	raw.az = (uint16_t) biquad_apply(&accel_z_lpf, raw.az);
	
	raw.gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw.gx);
	raw.gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw.gy);
	raw.gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw.gz);
	
	if (allowed_calib) imu_calib(&raw, 50, 4096, 500);
	
	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;
}

void imu_process_raw(imu_scaled_t* out, imu_raw_t* raw, const uint8_t* allowed_calib)
{
	// bias
	raw->ax -= accel_bias_x;
	raw->ay -= accel_bias_y;

	// фильтры
	raw->ax = (uint16_t) biquad_apply(&accel_x_lpf, raw->ax);
	raw->ay = (uint16_t) biquad_apply(&accel_y_lpf, raw->ay);
	raw->az = (uint16_t) biquad_apply(&accel_z_lpf, raw->az);

	raw->gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw->gx);
	raw->gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw->gy);
	raw->gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw->gz);

	// калибровка
	if (allowed_calib)
		imu_calib(raw, 50, 4096, 500);

	// масштабирование
	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;
}

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;
	
	const float alpha = 0.001f;
  x_calib = imu->gx, y_calib = imu->gy, z_calib = imu->gz;
	
	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;
}

uint16_t Rev16(uint16_t v)
{
  asm("REV16 %1, %0" : "=r" (v) : "r" (v));
  return v;
}

void imu_accel_calibrate()
{
	const int samples = 1000;
	
	float sum_ax = 0;
	float sum_ay = 0;
	
	imu_raw_t imu;
	
	for (uint16_t i = 0; i < samples; ++i)
	{
		imu_read_raw(&imu);
		
		sum_ax += imu.ax;
		sum_ay += imu.ay;
		
		for (volatile uint16_t i = 0; i < 5000; ++i) { asm volatile("NOP"); };
	}
	
	accel_bias_x = sum_ax / samples;
	accel_bias_y = sum_ay / 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;
}