#include "math/mat4.h"
#include <array>
#include <cmath>
#include "math/vector.h"

using namespace engine::math;

Matrix4 Matrix4::idty() {
    return {
        1.f, 0.f, 0.f, 0.f,
        0.f, 1.f, 0.f, 0.f,
        0.f, 0.f, 1.f, 0.f,
        0.f, 0.f, 0.f, 1.f,
    };
}

Matrix4 Matrix4::translate(Vector3 v) {
    return {
        1.f, 0.f, 0.f, v.x,
        0.f, 1.f, 0.f, v.y,
        0.f, 0.f, 1.f, v.z,
        0.f, 0.f, 0.f, 1.f,
    };
}

Matrix4 Matrix4::scale(float fac) {
    return {
        fac, 0.f, 0.f, 0.f,
        0.f, fac, 0.f, 0.f,
        0.f, 0.f, fac, 0.f,
        0.f, 0.f, 0.f, 1.f,
    };
}

Matrix4 Matrix4::scale(Vector3 facs) {
    return {
        facs.x,    0.f,    0.f, 0.f,
           0.f, facs.y,    0.f, 0.f,
           0.f,    0.f, facs.z, 0.f,
           0.f,    0.f,    0.f, 1.f,
    };
}

Matrix4 Matrix4::rot_x(float a) {
    float c = std::cos(a);
    float s = std::sin(a);
    return {
        1.f, 0.f, 0.f, 0.f,
        0.f,   c,  -s, 0.f,
        0.f,   s,   c, 0.f,
        0.f, 0.f, 0.f, 1.f,
    };
}

Matrix4 Matrix4::rot_y(float a) {
    float c = std::cos(a);
    float s = std::sin(a);
    return {
          c, 0.f,   s, 0.f,
        0.f, 1.f, 0.f, 0.f,
         -s, 0.f,   c, 0.f,
        0.f, 0.f, 0.f, 1.f,
    };
}

Matrix4 Matrix4::rot_z(float a) {
    float c = std::cos(a);
    float s = std::sin(a);
    return {
          c,  -s, 0.f, 0.f,
          s,   c, 0.f, 0.f,
        0.f, 0.f, 1.f, 0.f,
        0.f, 0.f, 0.f, 1.f,
    };
}

Matrix4 Matrix4::projection(float aspect_ratio, float min_z, float max_z) {
    return {{
        aspect_ratio, 0.f, 0.f, 0.f,
        0.f, -1.f, 0.f, 0.f,
        0.f, 0.f, -2.f / (max_z - min_z), -(max_z + min_z) / (max_z - min_z),
        0.f, 0.f, -1.f, 0.f,
    }};
}

Matrix4 Matrix4::operator+() const {
    return *this;
}

Matrix4 Matrix4::operator-() const {
    return {
        -values[ 0], -values[ 1], -values[ 2], -values[ 3],
        -values[ 4], -values[ 5], -values[ 6], -values[ 7],
        -values[ 8], -values[ 9], -values[10], -values[11],
        -values[12], -values[13], -values[14], -values[15],
    };
}

Matrix4 Matrix4::operator+(Matrix4 m) const {
    return {
        values[ 0] + m.values[ 0], values[ 1] + m.values[ 1], values[ 2] + m.values[ 2], values[ 3] + m.values[ 3],
        values[ 4] + m.values[ 4], values[ 5] + m.values[ 5], values[ 6] + m.values[ 6], values[ 7] + m.values[ 7],
        values[ 8] + m.values[ 8], values[ 9] + m.values[ 9], values[10] + m.values[10], values[11] + m.values[11],
        values[12] + m.values[12], values[13] + m.values[13], values[14] + m.values[14], values[15] + m.values[15],
    };
}

Matrix4 Matrix4::operator-(Matrix4 m) const {
    return *this + (-m);
}

Matrix4 Matrix4::operator*(Matrix4 m) const {
    Matrix4 ret;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            ret.values[i * 4 + j] = 0.f;
            for (int k = 0; k < 4; k++)
                ret.values[i * 4 + j] += values[i * 4 + k] * m.values[k * 4 + j];
        }
    }
    return ret;
}

Vector4 Matrix4::operator*(Vector4 v) const {
    return {
        values[ 0] * v.x + values[ 1] * v.y + values[ 2] * v.z + values[ 3] * v.w,
        values[ 4] * v.x + values[ 5] * v.y + values[ 6] * v.z + values[ 7] * v.w,
        values[ 8] * v.x + values[ 9] * v.y + values[10] * v.z + values[11] * v.w,
        values[12] * v.x + values[13] * v.y + values[14] * v.z + values[15] * v.w,
    };
}

std::array<Vector4, 4> Matrix4::to_vecs() const {
    return {{
        { values[ 0], values[ 4], values[ 8], values[12] },
        { values[ 1], values[ 5], values[ 9], values[13] },
        { values[ 2], values[ 6], values[10], values[14] },
        { values[ 3], values[ 7], values[11], values[15] },
    }};
}

Matrix4 engine::math::operator*(float fac, Matrix4 m) {
    return {
        fac * m.values[ 0], fac * m.values[ 1], fac * m.values[ 2], fac * m.values[ 3],
        fac * m.values[ 4], fac * m.values[ 5], fac * m.values[ 6], fac * m.values[ 7],
        fac * m.values[ 8], fac * m.values[ 9], fac * m.values[10], fac * m.values[11],
        fac * m.values[12], fac * m.values[13], fac * m.values[14], fac * m.values[15],
    };
}