#include <cmath>

#include "client/math/Quaternion.h"

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

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

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

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

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 {
    Vector3 interpolated = xyz * a + other.xyz * b;
    return Quaternion(interpolated[0], interpolated[1], interpolated[2], w * a + other.w * b);
}

Quaternion Quaternion::squad(float f, const Quaternion& prev, const Quaternion& next, const Quaternion& nextNext) const {
    float diff = static_cast<Vector3>(xyz - next.xyz).squareLength() + (w - next.w) * (w - next.w);
    if(diff < 0.0001f) {
        return *this;
    }
    Quaternion s1 = squadControl(prev, next);
    Quaternion s2 = next.squadControl(*this, nextNext);
    return slerp(f, next).slerp(2.0f * f * (1.0f - f), s1.slerp(f, s2));
}

Quaternion Quaternion::squadControl(const Quaternion& prev, const Quaternion& next) const {
    Quaternion conjugate(*this);
    conjugate.conjugate();
    Quaternion s1(next);
    s1.mul(conjugate).log().add(Quaternion(prev).mul(conjugate).log()).mul(-0.25f).exp().mul(*this);
    return s1;
}

Quaternion& Quaternion::normalize() {
    float f = 1.0f / sqrtf(xyz.squareLength() + w * w);
    xyz *= f;
    w *= f;
    return *this;
}

Matrix Quaternion::toMatrix() const {
    float x = xyz[0];
    float y = xyz[1];
    float z = xyz[2];

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

Quaternion& Quaternion::mul(const Quaternion& other) {
    Vector3 v = other.xyz * w + xyz * other.w + xyz.cross(other.xyz);
    w = w * other.w - xyz.dot(other.xyz);
    xyz = v;
    return *this;
}

Quaternion& Quaternion::mul(float f) {
    xyz *= f;
    w *= f;
    return *this;
}

Quaternion& Quaternion::add(const Quaternion& other) {
    xyz += other.xyz;
    w += other.w;
    return *this;
}

Quaternion& Quaternion::conjugate() {
    xyz = -xyz;
    return *this;
}

Quaternion& Quaternion::log() {
    // general log
    //float length = sqrtf(xyz.dot(xyz) + w * w);
    //float arccos = acos(w / length);
    //w = logf(length);
    //xyz.mul(arccos / xyz.length());

    // optimized use case log
    xyz.normalize();
    xyz *= acos(w);
    w = 0.0f;
    return *this;
}

Quaternion& Quaternion::exp() {
    // general exp
    //float expw = expf(w);
    //float length = xyz.length();
    //w = expw * cosf(length);
    //xyz.mul((expw * sin(length)) / length);

    // optimized use case exp
    float length = xyz.length();
    w = cosf(length);
    xyz *= sinf(length) / length;
    return *this;
}