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

static const float PI = 3.14159265359f;
static const float TO_DEG = 180.0f / PI;
static const float TO_RAD = PI / 180.0f;
static const float R = 8.314f;
static const float M = 0.029f;
static const float g = 9.80665f;
static const float ro = 1.189f;

struct Quaternion
{
    float w, x, y, z;
};

static Quaternion qCurrent = { 1, 0, 0, 0 };

static const float period = 0.005f;
static bool isFirst = true;

static void vecNormalize(Vec3& v)
{
    float n = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
    if (n > 1e-9f)
    {
        v.x /= n;
        v.y /= n;
        v.z /= n;
    }
}

static Vec3 vecCross(const Vec3& a, const Vec3& b)
{
    return
    {
      a.y * b.z - a.z * b.y,
      a.z * b.x - a.x * b.z,
      a.x * b.y - a.y * b.x
    };
}

static void normalizeQuaternion(Quaternion& q)
{
    float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
    if (norm > 1e-12f)
    {
        q.w /= norm;
        q.x /= norm;
        q.y /= norm;
        q.z /= norm;
    }
}

static Quaternion quaternionMultiply(const Quaternion& q1, const Quaternion& q2)
{
    Quaternion r;
    r.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
    r.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
    r.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
    r.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
    return r;
}

static Quaternion rotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
{
    Quaternion r = quaternionMultiply(quaternionMultiply(q, Quaternion{ 0, In.x, In.y, In.z }), Quaternion{ q.w, -q.x, -q.y, -q.z });

    return r;
}

static Vec3 backRotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
{
    Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, Quaternion{ 0, In.x, In.y, In.z }), q);

    return { r.x, r.y, r.z };
}

static Quaternion backRotateVectorByQuaternion2(const Quaternion& q, const Quaternion& In)
{
    Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, In), q);

    return r;
}

static Quaternion createYawQuaternion(float angle)
{
    Quaternion q;

    q.w = cosf(angle);
    q.x = 0.0f;
    q.y = 0.0f;
    q.z = sinf(angle);

    return q;
}

static Quaternion AccQuaternion(Quaternion& current, Vec3 a, float gravity)
{
    float acc = sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
    if (acc > (1.0f + gravity) || acc < (1.0f - gravity)) return current;

    vecNormalize(a);

    Vec3 g{ 0, 0, 1 };
    Vec3 axis = vecCross(g, a);
    float w = 1 + (g.x * a.x + g.y * a.y + g.z * a.z);
    Quaternion q = { w, axis.x, axis.y, axis.z };
    normalizeQuaternion(q);

    Quaternion qYaw{ current.w, 0, 0, current.z };

    return quaternionMultiply(q, qYaw); // Востановить оборот по Z
}

static Quaternion GyroQuaternion(Quaternion& current, float wx, float wy, float wz)
{
    Quaternion Mapp = current;
    Quaternion Spd{ 0, wx, wy, wz };
    Quaternion aq = quaternionMultiply(Spd, Mapp);

    Mapp.w -= 0.5f * aq.w;
    Mapp.x -= 0.5f * aq.x;
    Mapp.y -= 0.5f * aq.y;
    Mapp.z -= 0.5f * aq.z;

    normalizeQuaternion(Mapp);
    return Mapp;
}

static Quaternion nlerp(const Quaternion& q1, const Quaternion& q2, float alpha)
{

    float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;

    Quaternion q2_ = q2;
    if (dot < 0)
    {
        q2_.w = -q2.w;
        q2_.x = -q2.x;
        q2_.y = -q2.y;
        q2_.z = -q2.z;
    }

    Quaternion r;
    r.w = (1.0f - alpha) * q1.w + alpha * q2_.w;
    r.x = (1.0f - alpha) * q1.x + alpha * q2_.x;
    r.y = (1.0f - alpha) * q1.y + alpha * q2_.y;
    r.z = (1.0f - alpha) * q1.z + alpha * q2_.z;

    normalizeQuaternion(r);

    return r;
}

inline float GetAngle(float a1, float a2, float az)
{
    if (a2 == 0.0f && az == 0.0f) return a1 > 0.0f ? 90.0f : -90.0f;
    return atanf(a1 / sqrtf(a2 * a2 + az * az)) * TO_DEG;
}

static Vec3 quaternionToPitchRollYaw(const Quaternion& q, float& Upside)
{
    Quaternion pry = rotateVectorByQuaternion(q, { 0, 0, 1 });

    Upside = (pry.z > 0.0f) ? 1.0f : -1.0f;

    float yaw = 2 * atan2f(q.z, q.w) * TO_DEG;
    if (yaw < 0.0f) yaw = 360.0f + yaw;

    return // Sovereign orientation 
    {
      GetAngle(pry.y, pry.x, pry.z),  // Pitch
      GetAngle(-pry.x, pry.y, pry.z), // Roll
      yaw                             // Yaw
    };
}

static void addMagneto(Quaternion& q, Vec3 mag, float alpha, const float shift)
{
    static Quaternion yq = createYawQuaternion(shift * TO_RAD);

    vecNormalize(mag);

    Quaternion mQ = { 0, mag.x, mag.y, mag.z };

    Quaternion mW = backRotateVectorByQuaternion2(q, mQ);

    mW = quaternionMultiply(mW, yq); // Shifting the axes to the true north

    float gamma = mW.x * mW.x + mW.y * mW.y;
    float beta = sqrtf(gamma + mW.x * sqrtf(gamma));

    Quaternion mD
    {
      beta / (sqrtf(2.0f * gamma)),
      0.0f,
      0.0f,
      mW.y / (sqrtf(2.0f) * beta),
    };

    mD.w = (1.0f - alpha) + alpha * mD.w;
    mD.z = alpha * mD.z;

    if (mD.w != mD.w || mD.x != mD.x || mD.y != mD.y || mD.z != mD.z) return;

    q = quaternionMultiply(q, mD);
    normalizeQuaternion(q);
}

// WorkAccGyro({DataIMU.Acc.X, DataIMU.Acc.Y, DataIMU.Acc.Z}, {DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z}, {-DataIMU.Mag.X, DataIMU.Mag.Y, DataIMU.Mag.Z}, 0.01);
ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha)
{
    float wx = gyr.x * 500 / 32768 * 1.21f * (PI / 180) * period;
    float wy = gyr.y * 500 / 32768 * 1.21f * (PI / 180) * period;
    float wz = gyr.z * 500 / 32768 * 1.21f * (PI / 180) * period;

    Vec3 aB = acc;

    Quaternion qAcc = AccQuaternion(qCurrent, aB, 0.05f); // Tolerance for gravity deviation 5%

    qCurrent = GyroQuaternion(qCurrent, wx, wy, wz);

    Quaternion qFused = nlerp(qCurrent, qAcc, alpha);

    if (isFirst)
    {
        qFused = qAcc;
        isFirst = false;
    }

    qCurrent = qFused;

    addMagneto(qCurrent, mag, alpha, mag_shift);

    float up;
    Vec3 pry = quaternionToPitchRollYaw(qCurrent, up);

    Vec3 ine = backRotateVectorByQuaternion(qCurrent, aB);
    return
    {
      sqrtf(pry.x * pry.x + pry.y * pry.y) * up,
      pry.x, pry.y, pry.z,
      ine.x, ine.y, ine.z

    };
}

float calculateHeight(float bar, float temp)
{
    static double firstBar = bar;
    return (R * (temp + 273) / (M * g)) * log(firstBar / bar);

}