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

#define MAHONY_KP 2.5f
#define MAHONY_KI 0.01f

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 IRS* irs)
{
    const Quaternion* q = &irs->q;

    Vector3 g;
    g.x = 2 * (q->x*q->z - q->w*q->y);
    g.y = 2 * (q->w*q->x + q->y*q->z);
    g.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;

    // --- нормализация акселя
    float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);

    if (norm > 1e-6f)
    {
        accel.x /= norm;
        accel.y /= norm;
        accel.z /= norm;

        Vector3 g = IRS_getGravity(irs);

        Vector3 error =
        {
            accel.y*g.z - accel.z*g.y,
            accel.z*g.x - accel.x*g.z,
            accel.x*g.y - accel.y*g.x
        };

        gyro_bias.x += MAHONY_KI * error.x * dt;
        gyro_bias.y += MAHONY_KI * error.y * dt;
        gyro_bias.z += MAHONY_KI * error.z * dt;

        gyro.x += MAHONY_KP * error.x + gyro_bias.x;
        gyro.y += MAHONY_KP * error.y + gyro_bias.y;
        gyro.z += MAHONY_KP * error.z + gyro_bias.z;
    }

    irs->gyro = gyro;
    irs->accel = accel;

    // --- интеграция кватерниона
    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);

    irs->q = q;
}