#include <cmath>

#include "client/math/Quaternion.h"

Quaternion::Quaternion(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {
}

Quaternion::Quaternion(Vector unitAxis, float angle) {
    float sin;
    sincosf(angle * (M_PI / 360.0f), &sin, &w);
    x = unitAxis.getX() * sin;
    y = unitAxis.getY() * sin;
    z = unitAxis.getZ() * sin;
}

Quaternion Quaternion::lerp(float f, const Quaternion& other) const {
    Quaternion q = interpolate(1 - f, f, other);
    q.normalize();
    return q;
}

Quaternion Quaternion::slerp(float f, const Quaternion& other) const {
    float arccos = acosf(x * other.x + y * other.y + z * other.z + w * other.w);
    float sin = 1.0f / sinf(arccos);
    return interpolate(sinf((1 - f) * arccos) * sin, sinf(f * arccos) * sin, other);
}

Quaternion Quaternion::interpolate(float a, float b, const Quaternion& other) const {
    return Quaternion(x * a + other.x * b, y * a + other.y * b, z * a + other.z * b, w * a + other.w * b);
}

void Quaternion::normalize() {
    float f = 1.0f / sqrtf(x * x + y * y + z * z + w * w);
    x *= f;
    y *= f;
    z *= f;
    w *= f;
}

Matrix Quaternion::toMatrix() const {
    float x2 = 2 * x * x;
    float y2 = 2 * y * y;
    float z2 = 2 * z * z; 
    
    float xy = 2 * x * y;
    float xz = 2 * x * z;
    float xw = 2 * x * w;
    float zw = 2 * z * w;
    float yz = 2 * y * z;
    float yw = 2 * y * w;

    Matrix m;
    m.set(0, 1 - y2 - z2).set(1, xy - zw).set(2, xz + yw);
    m.set(4, xy + zw).set(5, 1 - x2 - z2).set(6, yz - xw);
    m.set(8, xz - yw).set(9, yz + xw).set(10, 1 - x2 - y2);
    return m;
}