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

// Matrix-Vektor Multiplikation (4x4 Matrix auf 4D Vektor anwenden)
std::array<double, 4> matVecMul(const std::array<std::array<double, 4>, 4>& mat, const std::array<double, 4>& vec) {
    std::array<double, 4> result = {0, 0, 0, 0};
    for (size_t i = 0; i < 4; ++i) {
        for (size_t j = 0; j < 4; ++j) {
            result[i] += mat[i][j] * vec[j];
        }
    }
    return result;
}

void test_rotateZ() {
    std::array<double, 4> v = {1, 0, 0, 1};  // Der Vektor (1, 0, 0) wird zu (1, 0, 0, 1)

    // Rotationsmatrix um 90° um die Z-Achse
    auto R = gameMatrix::rot3D(90, 'Z');

    // Vektor mit der Matrix multiplizieren
    std::array<double, 4> result = matVecMul(R, v);

    // Erwartetes Ergebnis
    std::array<double, 4> expected_result = {0, 1, 0, 1};

    // Vergleich der Ergebnisse
    for (size_t i = 0; i < 4; ++i) {
        assert(std::abs(result[i] - expected_result[i]) < 1e-6);
    }

    std::cout << "Test erfolgreich! Rotation um Z-Achse funktioniert." << std::endl;
}

int main() {
    test_rotateZ();
    return 0;
}
