#include "core/math/Quaternion.hpp"

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

Core::Quaternion::Quaternion(const Vector3& axis, float angle)
    : xyz(axis), w(1.0f) {
    xyz.normalize();
    float factor = 0.0f;
    sincosf(Core::Math::degreeToRadian(angle) * 0.5f, &factor, &w);
    xyz *= factor;
}

Core::Quaternion Core::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;
}

Core::Quaternion& Core::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;
}

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

Core::Vector3 Core::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;
}

void Core::Quaternion::toString(BufferString& s) const {
    s.append("(");
    s.append(xyz[0]).append(" i + ");
    s.append(xyz[1]).append(" j + ");
    s.append(xyz[2]).append(" k + ");
    s.append(w);
    s.append(')');
}