#include "core/Matrix.h"

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

#include "core/Utility.h"

CoreMatrix* coreTransposeMatrix(CoreMatrix* m) {
    CoreMatrix c = {0};
    for(size_t x = 0; x < 4; x++) {
        for(size_t y = 0; y < 4; y++) {
            c.data[x].data[y] = m->data[y].data[x];
        }
    }
    *m = c;
    return m;
}

#define CV40 (&(CoreVector4){0})

CoreMatrix* coreMulSetMatrix(CoreMatrix* m, const CoreMatrix* a) {
    for(int i = 0; i < 4; i++) {
        CoreVector4 d = {0};
        coreAddSetV4(&d, coreMulV4F(CV40, a->data + 0, m->data[i].data[0]));
        coreAddSetV4(&d, coreMulV4F(CV40, a->data + 1, m->data[i].data[1]));
        coreAddSetV4(&d, coreMulV4F(CV40, a->data + 2, m->data[i].data[2]));
        coreAddSetV4(&d, coreMulV4F(CV40, a->data + 3, m->data[i].data[3]));
        m->data[i] = d;
    }
    return m;
}

CoreMatrix* coreMulMatrix(CoreMatrix* m, const CoreMatrix* a,
                          const CoreMatrix* b) {
    *m = *a;
    coreMulSetMatrix(m, b);
    return m;
}

CoreVector3* coreMulMatrixV3(CoreVector3* v, const CoreMatrix* m,
                             const CoreVector3* a) {
    CoreVector4 v4 = {{a->data[0], a->data[1], a->data[2], 1.0f}};
    v->data[0] = coreDotV4(m->data + 0, &v4);
    v->data[1] = coreDotV4(m->data + 1, &v4);
    v->data[2] = coreDotV4(m->data + 2, &v4);
    coreMulSetV3F(v, 1.0f / coreDotV4(m->data + 3, &v4));
    return v;
}

CoreMatrix* coreScaleMatrix(CoreMatrix* m, const CoreVector3* v) {
    coreMulSetV4F(m->data + 0, v->data[0]);
    coreMulSetV4F(m->data + 1, v->data[1]);
    coreMulSetV4F(m->data + 2, v->data[2]);
    return m;
}

CoreMatrix* coreScaleMatrixF(CoreMatrix* m, float f) {
    return coreScaleMatrix(m, &(CoreVector3){{f, f, f}});
}

CoreMatrix* coreTranslateMatrix(CoreMatrix* m, const CoreVector3* v) {
    coreTranslateMatrixX(m, v->data[0]);
    coreTranslateMatrixY(m, v->data[1]);
    coreTranslateMatrixZ(m, v->data[2]);
    return m;
}

CoreMatrix* coreTranslateMatrixX(CoreMatrix* m, float tx) {
    coreAddSetV4(m->data + 0, coreMulV4F(CV40, m->data + 3, tx));
    return m;
}

CoreMatrix* coreTranslateMatrixY(CoreMatrix* m, float ty) {
    coreAddSetV4(m->data + 1, coreMulV4F(CV40, m->data + 3, ty));
    return m;
}

CoreMatrix* coreTranslateMatrixZ(CoreMatrix* m, float tz) {
    coreAddSetV4(m->data + 2, coreMulV4F(CV40, m->data + 3, tz));
    return m;
}

CoreMatrix* coreTranslateMatrixTo(CoreMatrix* m, const CoreVector3* v) {
    m->data[0] = (CoreVector4){{1.0f, 0.0f, 0.0f, v->data[0]}};
    m->data[1] = (CoreVector4){{0.0f, 1.0f, 0.0f, v->data[1]}};
    m->data[2] = (CoreVector4){{0.0f, 0.0f, 1.0f, v->data[2]}};
    m->data[3] = (CoreVector4){{0.0f, 0.0f, 0.0f, 1.0f}};
    return m;
}

static CoreMatrix* rotate(CoreMatrix* m, float degrees, int a, int b) {
    degrees = coreDegreeToRadian(degrees);
    float sin = sinf(degrees);
    float cos = cosf(degrees);
    CoreVector4 v = m->data[a];
    coreSubV4(m->data + a, coreMulV4F(CV40, &v, cos),
              coreMulV4F(CV40, m->data + b, sin));
    coreAddV4(m->data + b, coreMulV4F(CV40, &v, sin),
              coreMulV4F(CV40, m->data + b, cos));
    return m;
}

CoreMatrix* coreRotateMatrixX(CoreMatrix* m, float degrees) {
    return rotate(m, degrees, 1, 2);
}

CoreMatrix* coreRotateMatrixY(CoreMatrix* m, float degrees) {
    return rotate(m, -degrees, 0, 2);
}

CoreMatrix* coreRotateMatrixZ(CoreMatrix* m, float degrees) {
    return rotate(m, degrees, 0, 1);
}

CoreMatrix* coreRotateMatrix(CoreMatrix* m, const CoreQuaternion* q) {
    CoreVector3 a;
    CoreVector3 b;
    CoreVector3 c;
    CoreVector3 d;
    coreMulQV3(&a, q,
               &(CoreVector3){{m->data[0].data[0], m->data[1].data[0],
                               m->data[2].data[0]}});
    coreMulQV3(&b, q,
               &(CoreVector3){{m->data[0].data[1], m->data[1].data[1],
                               m->data[2].data[1]}});
    coreMulQV3(&c, q,
               &(CoreVector3){{m->data[0].data[2], m->data[1].data[2],
                               m->data[2].data[2]}});
    coreMulQV3(&d, q,
               &(CoreVector3){{m->data[0].data[3], m->data[1].data[3],
                               m->data[2].data[3]}});
    m->data[0] = (CoreVector4){{a.data[0], b.data[0], c.data[0], d.data[0]}};
    m->data[1] = (CoreVector4){{a.data[1], b.data[1], c.data[1], d.data[1]}};
    m->data[2] = (CoreVector4){{a.data[2], b.data[2], c.data[2], d.data[2]}};

    //    Vector3 a = q * Vector3(data[0][0], data[1][0], data[2][0]);
    //    Vector3 b = q * Vector3(data[0][1], data[1][1], data[2][1]);
    //    Vector3 c = q * Vector3(data[0][2], data[1][2], data[2][2]);
    //    Vector3 d = q * Vector3(data[0][3], data[1][3], data[2][3]);
    //    set(0, Vector4(a[0], b[0], c[0], d[0]));
    //    set(1, Vector4(a[1], b[1], c[1], d[1]));
    //    set(2, Vector4(a[2], b[2], c[2], d[2]));
    return m;
}

static void add(size_t* w, char** buffer, size_t* n, size_t shift) {
    *w += shift;
    if(*n > shift) {
        *buffer += shift;
        *n -= shift;
    } else {
        *n = 0;
    }
}

static void addI(size_t* w, char** buffer, size_t* n, int shift) {
    add(w, buffer, n, shift < 0 ? 0 : (size_t)shift);
}

size_t coreToStringMatrix(const CoreMatrix* m, char* buffer, size_t n) {
    size_t w = 0;
    addI(&w, &buffer, &n, snprintf(buffer, n, "["));
    add(&w, &buffer, &n, coreToStringV4(m->data + 0, buffer, n));
    addI(&w, &buffer, &n, snprintf(buffer, n, ", "));
    add(&w, &buffer, &n, coreToStringV4(m->data + 1, buffer, n));
    addI(&w, &buffer, &n, snprintf(buffer, n, ", "));
    add(&w, &buffer, &n, coreToStringV4(m->data + 2, buffer, n));
    addI(&w, &buffer, &n, snprintf(buffer, n, ", "));
    add(&w, &buffer, &n, coreToStringV4(m->data + 3, buffer, n));
    addI(&w, &buffer, &n, snprintf(buffer, n, "]"));
    return w;
}