#include "core/Quaternion.h"

#include <math.h>
#include <stdio.h>

#include "core/Utility.h"

#define CV30 (&CORE_VECTOR3)

CoreQuaternion* coreAxisAngleQ(CoreQuaternion* q, const CoreVector3* axis,
                               float angle) {
    q->xyz = *axis;
    coreNormalizeV3(&q->xyz);
    angle = coreDegreeToRadian(angle) * 0.5f;
    q->w = cosf(angle);
    coreMulSetV3F(&q->xyz, sinf(angle));
    return q;
}

CoreQuaternion* coreLerpQ(CoreQuaternion* q, const CoreQuaternion* a, float f,
                          const CoreQuaternion* b) {
    coreAddV3(&q->xyz, coreMulV3F(CV30, &a->xyz, 1.0f - f),
              coreMulV3F(CV30, &b->xyz, f));
    q->w = a->w * (1.0f - f) + b->w * f;
    float iLength = 1.0f / sqrtf(coreSquareLengthV3(&q->xyz) + q->w * q->w);
    coreMulSetV3F(&q->xyz, iLength);
    q->w *= iLength;
    return q;
}

CoreQuaternion* coreMulSetQ(CoreQuaternion* q, const CoreQuaternion* other) {
    float dot = coreDotV3(&q->xyz, &other->xyz);
    coreAddV3(&q->xyz, coreMulV3F(CV30, &other->xyz, q->w),
              coreAddV3(CV30, coreMulV3F(CV30, &q->xyz, other->w),
                        coreCross(CV30, &q->xyz, &other->xyz)));
    q->w = q->w * other->w - dot;
    return q;
}

CoreQuaternion* coreMulQ(CoreQuaternion* q, const CoreQuaternion* a,
                         const CoreQuaternion* b) {
    *q = *a;
    coreMulSetQ(q, b);
    return q;
}

CoreVector3* coreMulQV3(CoreVector3* r, const CoreQuaternion* q,
                        const CoreVector3* v) {
    CoreVector3 qv;
    coreAddV3(&qv, coreMulV3F(CV30, v, q->w), coreCross(CV30, &q->xyz, v));

    coreAddV3(r, coreMulV3F(CV30, &q->xyz, coreDotV3(&q->xyz, v)),
              coreSubV3(CV30, coreMulV3F(CV30, &qv, q->w),
                        coreCross(CV30, &qv, &q->xyz)));
    return r;
}

size_t coreToStringQ(const CoreQuaternion* q, char* buffer, size_t n) {
    int w = snprintf(buffer, n, "(%.3f i + %.3f j + %.3f k + %.3f)",
                     (double)q->xyz.data[0], (double)q->xyz.data[1],
                     (double)q->xyz.data[2], (double)q->w);
    return w < 0 ? 0 : (size_t)w;
}