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

using namespace Matrix3D;

void test_matmul_basic() {
    std::array<std::array<double,4>,4> A = {{
        {1, 0, 0, 0},
        {0, 1, 0, 0},
        {0, 0, 1, 0},
        {0, 0, 0, 1}
    }};

    std::array<std::array<double,4>,4> B = {{
        {2, 3, 4, 5},
        {1, 1, 1, 1},
        {0, 0, 0, 0},
        {7, 7, 7, 7}
    }};

    auto C = gameMatrix::matmul(A, B);

    assert(C[0][0] == 2);
    assert(C[0][1] == 3);
    assert(C[1][0] == 1);
    assert(C[3][3] == 7);
}

void test_translate_basic() {
    std::array<double, 3> pos = {3, -2, 8};

    auto T = gameMatrix::translate(pos);

    assert(T[0][3] == 3);
    assert(T[1][3] == -2);
    assert(T[2][3] == 8);
}

void test_rot3D_basic() {
    auto R = gameMatrix::rot3D(90, 'x');
    // cos(90°) ≈ 0, sin(90°) ≈ 1
    assert(std::abs(R[1][1]) < 1e-6);
    assert(std::abs(R[1][2] + 1.0) < 1e-6);
}

int main() {
    test_matmul_basic();
    test_translate_basic();
    test_rot3D_basic();

    std::cout << "Test bestanden!\n";
    return 0;
}