#include "irs.h"
#include <math.h>

#define MAHONY_KP 		2.5f
#define MAHONY_KI 		0.01f

#define ACC_ALPHA		0.01f
#define ACC_MAX_ERROR	0.2f

static Vector3 gyro_bias = {0};

void IRS_init(IRS* irs)
{
    irs->q.w = 1.0f;
    irs->q.x = 0.0f;
    irs->q.y = 0.0f;
    irs->q.z = 0.0f;
}

Vector3 IRS_getGravity(const Quaternion* q)
{
    Vector3 g = 
	{
		2 * (q->x*q->z - q->w*q->y),
		2 * (q->w*q->x + q->y*q->z),
		q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
	};

    return g;
}

void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt)
{
    Vector3 gyro = *gyro_in;
	Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z};

    // gyro intergate
	Quaternion q = irs->q;
	
	Quaternion omega = 
	{
		gyro.x * DEG2RAD,
		gyro.y * DEG2RAD,
		gyro.z * DEG2RAD,
		0
	};
	
	Quaternion dq = QuatProd(&q, &omega);
	dq = QuatConstProd(&dq, 0.5f * dt);
	
	q = QuatSum(&q, &dq);
	q = QuatNormalize(&q, 1.0f);
	
	// accel correction
	float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
	
	if (acc_len > 1e-6f)
	{
		Vector3 acc_norm =
		{
			accel.x / acc_len,
			accel.y / acc_len,
			accel.z / acc_len
		};
		
		float dyn = fabsf(acc_len - 1.0f);
		
		float trust;
		if (dyn > ACC_MAX_ERROR)
			trust = 0.0f;
		else
			trust = 1.0f - (dyn / ACC_MAX_ERROR);
		
		float gain = ACC_ALPHA * trust;
		
		if (gain > 1e-5f)
		{
			Vector3 est = IRS_getGravity(&q);
			
			Vector3 error = 
			{
				acc_norm.y*est.z - acc_norm.z*est.y,
				acc_norm.z*est.x - acc_norm.x*est.z,
				acc_norm.x*est.y - acc_norm.y*est.x
			};
			
			Quaternion corretrion = 
			{
				error.x * gain * 0.5f,
				error.y * gain * 0.5f,
				error.z * gain * 0.5f,
				1.0f
			};
			
			q = QuatProd(&corretrion, &q);
			q = QuatNormalize(&q, 1.0f);
		}
	}
	
	irs->q = q;
	irs->gyro = gyro;
	irs->accel = accel;
}