#include <cmath>

#include "client/math/Quaternion.h"

Quaternion::Quaternion() : xyz(0.0f, 0.0f, 0.0f), w(0.0f) {
}

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

Quaternion::Quaternion(const Vector& axis, float angle) {
    Vector unit(axis);
    xyz.set(unit.normalize());
    float sin;
    sincosf(angle * (M_PI / 360.0f), &sin, &w);
    xyz.mul(sin);
}

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

Quaternion Quaternion::slerp(float f, const Quaternion& other) const {
    float angle = xyz.dot(other.xyz) + w * other.w;
    if(angle > 1.0f) {
        return *this;
    } else if(angle < -1.0f) {
        angle = -1.0f;
    }
    float arccos = acosf(angle);
    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 {
    Vector interpolated(xyz);
    interpolated.mul(a).addMul(other.xyz, b);
    return Quaternion(interpolated.getX(), interpolated.getY(), interpolated.getZ(), w * a + other.w * b);
}

void Quaternion::normalize() {
    float f = 1.0f / sqrtf(xyz.dot(xyz) + w * w);
    xyz.mul(f);
    w *= f;
}

Matrix Quaternion::toMatrix() const {
    float x = xyz.getX();
    float y = xyz.getY();
    float z = xyz.getZ();

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

void Quaternion::mul(const Quaternion& other) {
    Vector v;
    v.addMul(other.xyz, w).addMul(xyz, other.w).add(Vector(xyz).cross(other.xyz));
    w = w * other.w - xyz.dot(other.xyz);
    xyz.set(v);
}