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

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

void IRS_update(IRS* irs, float dt)
{
	// gyro update
	Vector3 gyro = 
	{
		irs->gyro.x * dt * DEG2RAD,
		irs->gyro.y * dt * DEG2RAD,
		irs->gyro.z * dt * DEG2RAD
	};
	
	Quaternion g = {gyro.x, gyro.y, gyro.z, 0};
	g = QuatProd(&irs->q, &g);
	g = QuatConstProd(&g, 0.5f);
	irs->q = QuatSum(&irs->q, &g);
	irs->q = QuatNormalize(&irs->q, 1.0f);
	
  // /gyro update
	
	// accel update
	// restoreQuat(irs);
	
	// /accel update
}

void restoreQuat(IRS* irs)
{
	float len = lengthV3(&irs->accel);
	
	static float quat_acc_alpha = 0.03f;
	static float quat_acc_max = 0.02f;
	
	float dyn = fabsf(len - 1.0f);
	if (dyn > quat_acc_max) dyn = 1.0f; else dyn /= quat_acc_max;
	
	float gain = quat_acc_alpha * (1.0f - dyn);
	
	if (gain < 0.0001f) return;
	
	Vector3 acc = normalizeV3(&irs->accel, 1.0f);
	
	Vector3 est = IRS_getGravity(&irs->q);
	
	Vector3 cross = Cross(&acc, &est);
	float dot = DotV3(&acc, &est);
	
	if (dot < 0.0f)
	{
		float error_len = lengthV3(&cross);
		if (error_len < 0.001f) {
			cross.x = 1.0f;
			cross.y = 0.0f;
			cross.z = 0.0f;
		}
		else
			cross = constProdV3(&cross, 1.0f / error_len);
	}
	
	Vector3 axis = constProdV3(&cross, gain * 0.5f);
	
	Quaternion correction =
	{
		axis.x,
		axis.y,
		axis.z,
		1.0f
	};
	
	irs->q = QuatProd(&irs->q, &correction);
	irs->q = QuatNormalize(&irs->q, 1.0f);
}

void setAccelShift(IRS* irs, const float roll, const float pitch, const float yaw)
{
	float h_roll = (roll * DEG2RAD) / 2.0f;
	float h_pitch = (pitch * DEG2RAD) / 2.0f;
	float h_yaw = (yaw * DEG2RAD) / 2.0f;
	
	Quaternion q_roll = {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)};
	Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)};
	Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)};
	
	Quaternion prProd = QuatProd(&q_pitch, &q_roll);
	Quaternion pryProd = QuatProd(&prProd, &q_yaw);
	
	irs->shiftAccelPRY = pryProd;
}

Vector3 IRS_getGravity(const Quaternion* q)
{
  Vector3 g = 
	{
		2.0f * (q->x*q->z - q->w*q->y),
		2.0f * (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;
}