#include <iostream>
#include <array>
#include <cmath>
#include "gamematrix.h"

using Matrix4 = std::array<std::array<double,4>,4>;
using Vec3    = std::array<double,3>;

bool eq(double a, double b) {
    return std::fabs(a - b) < 0.0001;
}

Matrix4 identity() {
    Matrix4 I{};
    for (int i = 0; i < 4; i++) {
        I[i][i] = 1.0;
    }
    return I;
}

Vec3 applyMatrix(const Matrix4& M, const Vec3& v) {
    Vec3 r{0,0,0};

    r[0] = M[0][0]*v[0] + M[0][1]*v[1] + M[0][2]*v[2] + M[0][3];
    r[1] = M[1][0]*v[0] + M[1][1]*v[1] + M[1][2]*v[2] + M[1][3];
    r[2] = M[2][0]*v[0] + M[2][1]*v[1] + M[2][2]*v[2] + M[2][3];

    return r;
}

void testMatmulIdentity() {
    Matrix4 A = identity();
    Matrix4 B = identity();
    Matrix4 C = Matrix3D::gameMatrix::matmul(A, B);

    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            double expected = (i == j ? 1.0 : 0.0);
            if (!eq(C[i][j], expected)) {
                std::cout << "[FAIL] matmul Identity\n";
                return;
            }
        }
    }

    std::cout << "[OK] matmul Identity\n";
}

void testTranslate() {
    Vec3 pos{1,2,3};
    Matrix4 T = Matrix3D::gameMatrix::translate(pos);

    if (eq(T[0][3], 1) && eq(T[1][3], 2) && eq(T[2][3], 3))
        std::cout << "[OK] translate\n";
    else
        std::cout << "[FAIL] translate\n";
}

void testRotZ90() {
    Vec3 v{1,0,0};
    Matrix4 R = Matrix3D::gameMatrix::rot3D(90, 'z');
    Vec3 r = applyMatrix(R, v);

    if (eq(r[0], 0) && eq(r[1], 1) && eq(r[2], 0))
        std::cout << "[OK] rotZ 90°\n";
    else
        std::cout << "[FAIL] rotZ 90°\n";
}

void testRotX180() {
    Vec3 v{0,1,0};
    Matrix4 R = Matrix3D::gameMatrix::rot3D(180, 'x');
    Vec3 r = applyMatrix(R, v);

    if (eq(r[0], 0) && eq(r[1], -1) && eq(r[2], 0))
        std::cout << "[OK] rotX 180°\n";
    else
        std::cout << "[FAIL] rotX 180°\n";
}

void testRotY270() {
    Vec3 v{1,0,0};
    Matrix4 R = Matrix3D::gameMatrix::rot3D(270, 'y');
    Vec3 r = applyMatrix(R, v);

    if (eq(r[0], 0) && eq(r[1], 0) && eq(r[2], 1))
        std::cout << "[OK] rotY 270°\n";
    else
        std::cout << "[FAIL] rotY 270°\n";
}

int main() {
    testMatmulIdentity();
    testTranslate();
    testRotZ90();
    testRotX180();
    testRotY270();
    return 0;
}