#include "core/Matrix.h"

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

#include "core/Utility.h"

#define M(m, x, y) ((m)->data[x].data[y])
#define CV40 (&VECTOR4)

Matrix* transposeMatrix(Matrix* m) {
    Matrix c = ZERO_MATRIX;
    for(size_t x = 0; x < 4; x++) {
        for(size_t y = 0; y < 4; y++) {
            M(&c, x, y) = M(m, y, x);
        }
    }
    *m = c;
    return m;
}

Matrix* mulSetMatrix(Matrix* m, const Matrix* a) {
    for(int i = 0; i < 4; i++) {
        Vector4 d = VECTOR4;
        addSetV4(&d, mulV4F(CV40, a->data + 0, m->data[i].data[0]));
        addSetV4(&d, mulV4F(CV40, a->data + 1, m->data[i].data[1]));
        addSetV4(&d, mulV4F(CV40, a->data + 2, m->data[i].data[2]));
        addSetV4(&d, mulV4F(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;
}

Vector3* mulMatrixV3(Vector3* v, const Matrix* m, const Vector3* a) {
    Vector4 v4 = {{a->data[0], a->data[1], a->data[2], 1.0f}};
    v->data[0] = dotV4(m->data + 0, &v4);
    v->data[1] = dotV4(m->data + 1, &v4);
    v->data[2] = dotV4(m->data + 2, &v4);
    mulSetV3F(v, 1.0f / dotV4(m->data + 3, &v4));
    return v;
}

Matrix* scaleMatrix(Matrix* m, const Vector3* v) {
    mulSetV4F(m->data + 0, v->data[0]);
    mulSetV4F(m->data + 1, v->data[1]);
    mulSetV4F(m->data + 2, v->data[2]);
    return m;
}

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

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

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

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

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

Matrix* translateMatrixTo(Matrix* m, const Vector3* v) {
    m->data[0] = (Vector4){{1.0f, 0.0f, 0.0f, v->data[0]}};
    m->data[1] = (Vector4){{0.0f, 1.0f, 0.0f, v->data[1]}};
    m->data[2] = (Vector4){{0.0f, 0.0f, 1.0f, v->data[2]}};
    m->data[3] = (Vector4){{0.0f, 0.0f, 0.0f, 1.0f}};
    return m;
}

static Matrix* rotate(Matrix* m, float degrees, int a, int b) {
    degrees = degreeToRadian(degrees);
    float sin = sinf(degrees);
    float cos = cosf(degrees);
    Vector4 v = m->data[a];
    subV4(m->data + a, mulV4F(CV40, &v, cos), mulV4F(CV40, m->data + b, sin));
    addV4(m->data + b, mulV4F(CV40, &v, sin), mulV4F(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 Quaternion* q) {
    Vector3 a;
    Vector3 b;
    Vector3 c;
    Vector3 d;
    mulQV3(&a, q, &(Vector3){{M(m, 0, 0), M(m, 1, 0), M(m, 2, 0)}});
    mulQV3(&b, q, &(Vector3){{M(m, 0, 1), M(m, 1, 1), M(m, 2, 1)}});
    mulQV3(&c, q, &(Vector3){{M(m, 0, 2), M(m, 1, 2), M(m, 2, 2)}});
    mulQV3(&d, q, &(Vector3){{M(m, 0, 3), M(m, 1, 3), M(m, 2, 3)}});
    m->data[0] = (Vector4){{a.data[0], b.data[0], c.data[0], d.data[0]}};
    m->data[1] = (Vector4){{a.data[1], b.data[1], c.data[1], d.data[1]}};
    m->data[2] = (Vector4){{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;
    stringAdd(&w, &buffer, &n, toString(buffer, n, "["));
    stringAdd(&w, &buffer, &n, toStringV4(m->data + 0, buffer, n));
    stringAdd(&w, &buffer, &n, toString(buffer, n, ", "));
    stringAdd(&w, &buffer, &n, toStringV4(m->data + 1, buffer, n));
    stringAdd(&w, &buffer, &n, toString(buffer, n, ", "));
    stringAdd(&w, &buffer, &n, toStringV4(m->data + 2, buffer, n));
    stringAdd(&w, &buffer, &n, toString(buffer, n, ", "));
    stringAdd(&w, &buffer, &n, toStringV4(m->data + 3, buffer, n));
    stringAdd(&w, &buffer, &n, toString(buffer, n, "]"));
    return w;
}