module Core.Quaternion;

import Core.Math;
import Core.Std;
import Core.ToString;

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

Core::Quaternion::Quaternion(const Vector3& axis, float angle) noexcept :
    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 noexcept {
    Quaternion q;
    q.v = interpolate(v, other.v, f);
    q.v.normalize();
    return q;
}

Core::Quaternion& Core::Quaternion::operator*=(
    const Quaternion& other) noexcept {
    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 noexcept {
    Quaternion q(*this);
    q *= other;
    return q;
}

Core::Vector3 Core::Quaternion::operator*(const Vector3& v3) const noexcept {
    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(StringBase& b) const noexcept {
    return b.addFormat(
        "({.3} i + {.3} j + {.3} k + {.3})", v[0], v[1], v[2], v[3]);
}