#include "core/Quaternion.hpp"

#include <cmath>

Core::Quaternion::Quaternion() : v(0.0f, 0.0f, 0.0f, 1.0f) {
}

Core::Quaternion::Quaternion(const Vector3& axis, float angle) :
    v(axis[0], axis[1], axis[2], 1.0f) {
    v.xyz().normalize();
    float factor = 0.0f;
    sincosf(angle * 0.5f, &factor, &v[3]);
    v.xyz() *= factor;
}

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

Core::Quaternion& Core::Quaternion::operator*=(const Quaternion& other) {
    float dot = v.xyz().dot(other.v.xyz());
    v.xyz() = other.v.xyz() * v[3] + v.xyz() * other.v[3] +
              cross(v.xyz(), other.v.xyz());
    v[3] = v[3] * other.v[3] - 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& v3) const {
    Vector3 qv = v3 * v[3] + cross(v.xyz(), v3);
    return v.xyz() * v.xyz().dot(v3) + qv * v[3] - cross(qv, v.xyz());
}

size_t Core::Quaternion::toString(char* s, size_t n) const {
    int w = snprintf(
        s, n, "(%.3f i + %.3f j + %.3f k + %.3f)", static_cast<double>(v[0]),
        static_cast<double>(v[1]), static_cast<double>(v[2]),
        static_cast<double>(v[3]));
    return w >= 0 ? static_cast<size_t>(w) : 0;
}