#include "core/Matrix.h"

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

#include "core/Utility.h"

Matrix* transposeMatrix(Matrix* m) {
    Matrix c = CORE_ZERO_MATRIX;
    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 (&CORE_VECTOR4)

Matrix* mulSetMatrix(Matrix* m, const Matrix* a) {
    for(int i = 0; i < 4; i++) {
        CoreVector4 d = CORE_VECTOR4;
        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;
}

Matrix* mulMatrix(Matrix* m, const Matrix* a, const Matrix* b) {
    *m = *a;
    mulSetMatrix(m, b);
    return m;
}

CoreVector3* mulMatrixV3(CoreVector3* v, const Matrix* 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;
}

Matrix* scaleMatrix(Matrix* 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;
}

Matrix* scaleMatrixF(Matrix* m, float f) {
    return scaleMatrix(m, &(CoreVector3){{f, f, f}});
}

Matrix* translateMatrix(Matrix* m, const CoreVector3* v) {
    translateMatrixX(m, v->data[0]);
    translateMatrixY(m, v->data[1]);
    translateMatrixZ(m, v->data[2]);
    return m;
}

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

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

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

Matrix* translateMatrixTo(Matrix* 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 Matrix* rotate(Matrix* 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;
}

Matrix* rotateMatrixX(Matrix* m, float degrees) {
    return rotate(m, degrees, 1, 2);
}

Matrix* rotateMatrixY(Matrix* m, float degrees) {
    return rotate(m, -degrees, 0, 2);
}

Matrix* rotateMatrixZ(Matrix* m, float degrees) {
    return rotate(m, degrees, 0, 1);
}

Matrix* rotateMatrix(Matrix* 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]}};
    return m;
}

size_t toStringMatrix(const Matrix* m, char* buffer, size_t n) {
    size_t w = 0;
    coreStringAdd(&w, &buffer, &n, coreToString(buffer, n, "["));
    coreStringAdd(&w, &buffer, &n, coreToStringV4(m->data + 0, buffer, n));
    coreStringAdd(&w, &buffer, &n, coreToString(buffer, n, ", "));
    coreStringAdd(&w, &buffer, &n, coreToStringV4(m->data + 1, buffer, n));
    coreStringAdd(&w, &buffer, &n, coreToString(buffer, n, ", "));
    coreStringAdd(&w, &buffer, &n, coreToStringV4(m->data + 2, buffer, n));
    coreStringAdd(&w, &buffer, &n, coreToString(buffer, n, ", "));
    coreStringAdd(&w, &buffer, &n, coreToStringV4(m->data + 3, buffer, n));
    coreStringAdd(&w, &buffer, &n, coreToString(buffer, n, "]"));
    return w;
}