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

#include "core/Generic.h"
#include "core/ToString.h"

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

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;
        addSet(&d, mul(a->data + 0, m->data[i].x));
        addSet(&d, mul(a->data + 1, m->data[i].y));
        addSet(&d, mul(a->data + 2, m->data[i].z));
        addSet(&d, mul(a->data + 3, m->data[i].w));
        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 = V(a->x, a->y, a->z, 1.0f);
    v->x = dot(m->data + 0, &v4);
    v->y = dot(m->data + 1, &v4);
    v->z = dot(m->data + 2, &v4);
    mulSet(v, 1.0f / dot(m->data + 3, &v4));
    return v;
}

Matrix* scaleMatrix(Matrix* m, const Vector3* v) {
    mulSet(m->data + 0, v->x);
    mulSet(m->data + 1, v->y);
    mulSet(m->data + 2, v->z);
    return m;
}

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

Matrix* translateMatrix(Matrix* m, const Vector3* v) {
    translateMatrixX(m, v->x);
    translateMatrixY(m, v->y);
    translateMatrixZ(m, v->z);
    return m;
}

Matrix* translateMatrixX(Matrix* m, float tx) {
    addSet(m->data + 0, mul(m->data + 3, tx));
    return m;
}

Matrix* translateMatrixY(Matrix* m, float ty) {
    addSet(m->data + 1, mul(m->data + 3, ty));
    return m;
}

Matrix* translateMatrixZ(Matrix* m, float tz) {
    addSet(m->data + 2, mul(m->data + 3, tz));
    return m;
}

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

static Matrix* rotate(Matrix* m, float radians, int a, int b) {
    float sin = sinf(radians);
    float cos = cosf(radians);
    Vector4 v = m->data[a];
    sub(m->data + a, mul(&v, cos), mul(m->data + b, sin));
    add(m->data + b, mul(&v, sin), mul(m->data + b, cos));
    return m;
}

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

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

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

Matrix* rotateMatrix(Matrix* m, const Quaternion* q) {
    Vector3 a;
    Vector3 b;
    Vector3 c;
    Vector3 d;
    mul(&a, q, &V(M(m, 0, 0), M(m, 1, 0), M(m, 2, 0)));
    mul(&b, q, &V(M(m, 0, 1), M(m, 1, 1), M(m, 2, 1)));
    mul(&c, q, &V(M(m, 0, 2), M(m, 1, 2), M(m, 2, 2)));
    mul(&d, q, &V(M(m, 0, 3), M(m, 1, 3), M(m, 2, 3)));
    m->data[0] = V(a.x, b.x, c.x, d.x);
    m->data[1] = V(a.y, b.y, c.y, d.y);
    m->data[2] = V(a.z, b.z, c.z, d.z);
    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;
}