#include "core/Quaternion.h"

#include <math.h>

#include "core/Utility.h"

#define mulC(a, b) mulV3F(&VECTOR3, (a), (b))
#define crossC(a, b) cross(&VECTOR3, (a), (b))
#define addC(a, b) addV3(&VECTOR3, (a), (b))
#define subC(a, b) subV3(&VECTOR3, (a), (b))

Quaternion* axisAngleQ(Quaternion* q, const Vector3* axis, float angle) {
    q->xyz = *axis;
    normalizeV3(&q->xyz);
    angle = degreeToRadian(angle) * 0.5f;
    q->w = cosf(angle);
    mulSetV3F(&q->xyz, sinf(angle));
    return q;
}

Quaternion* lerpQ(Quaternion* q, const Quaternion* a, float f,
                  const Quaternion* b) {
    addV3(&q->xyz, mulC(&a->xyz, 1.0f - f), mulC(&b->xyz, f));
    q->w = a->w * (1.0f - f) + b->w * f;
    float iLength = 1.0f / sqrtf(squareLengthV3(&q->xyz) + q->w * q->w);
    mulSetV3F(&q->xyz, iLength);
    q->w *= iLength;
    return q;
}

Quaternion* mulSetQ(Quaternion* q, const Quaternion* o) {
    float dot = dotV3(&q->xyz, &o->xyz);
    addV3(&q->xyz, mulC(&o->xyz, q->w),
          addC(mulC(&q->xyz, o->w), crossC(&q->xyz, &o->xyz)));
    q->w = q->w * o->w - dot;
    return q;
}

Quaternion* mulQ(Quaternion* q, const Quaternion* a, const Quaternion* b) {
    *q = *a;
    mulSetQ(q, b);
    return q;
}

Vector3* mulQV3(Vector3* r, const Quaternion* q, const Vector3* v) {
    Vector3 qv;
    addV3(&qv, mulC(v, q->w), crossC(&q->xyz, v));
    addV3(r, mulC(&q->xyz, dotV3(&q->xyz, v)),
          subC(mulC(&qv, q->w), crossC(&qv, &q->xyz)));
    return r;
}

size_t toStringQ(const Quaternion* q, char* buffer, size_t n) {
    return toString(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);
}