#include <cmath>

#include "math/MathConstants.h"
#include "math/Quaternion.h"

Quaternion::Quaternion() : w(1.0f) {
}

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

Quaternion Quaternion::lerp(float f, const Quaternion& other) const {
    Quaternion q;
    q.xyz = xyz * (1.0f - f) + other.xyz * f;
    q.w = w * (1.0f - f) + other.w * f;
    float iLength = 1.0f / sqrtf(q.xyz.squareLength() + q.w * q.w);
    q.xyz *= iLength;
    q.w *= iLength;
    return q;
}

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

Quaternion Quaternion::operator*(const Quaternion& other) const {
    Quaternion q(*this);
    q *= other;
    return q;
}

Vector3 Quaternion::operator*(const Vector3& v) const {
    Vector3 qv = v * w + xyz.cross(v);
    Vector3 qvq = xyz * xyz.dot(v) + qv * w - qv.cross(xyz);
    return qvq;
}