//
// Created by marco on 03.11.2025.
// Changeb by marco on 16.11.2025

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

using namespace Matrix3D;

bool nearlyEqual(double a, double b, double eps = 1e-6) {
    return std::abs(a - b) < eps;
}

void test_matmul_identity() {
    Mat4 A = identity();
    Mat4 B = identity();
    Mat4 R = matmul(A, B);

    std::cout << "Test matmul: Identity * Identity -> Identity: ";

    bool ok = true;
    for(int i=0;i<4;i++){
        for(int j=0;j<4;j++){
            double expected = (i==j ? 1.0 : 0.0);
            if(!nearlyEqual(R[i][j], expected)) ok = false;
        }
    }

    std::cout << (ok ? "OK\n" : "FAIL\n");
}

void test_matmul_example() {
    Mat4 A = {{
        {{1, 2, 3, 4}},
        {{0, 1, 2, 3}},
        {{0, 0, 1, 2}},
        {{0, 0, 0, 1}}
    }};

    Mat4 B = {{
        {{1, 0, 0, 0}},
        {{1, 1, 0, 0}},
        {{1, 1, 1, 0}},
        {{1, 1, 1, 1}}
    }};

    Mat4 R = matmul(A, B);

    std::cout << "Test matmul: Beispielmatrizen A * B: ";

    // Erwartete Matrix mit Handrechnung:
    Mat4 Expected = {{
        {{1+2+3+4,   2+3+4,   3+4,   4}},
        {{0+1+2+3,   1+2+3,   2+3,   3}},
        {{0+0+1+2,   0+1+2,   1+2,   2}},
        {{1,         1,       1,     1}}
    }};

    bool ok = true;
    for(int i=0;i<4;i++){
        for(int j=0;j<4;j++){
            if(!nearlyEqual(R[i][j], Expected[i][j])) ok = false;
        }
    }

    std::cout << (ok ? "OK\n" : "FAIL\n");
}

void test_translate() {
    Vec3 v{1,2,3};
    Mat4 T = translate({5,-2,1});
    Vec3 out = T * v;

    std::cout << "Test translate (Vec3{1,2,3} + {5,-2,1}): ";

    if(nearlyEqual(out.x, 6) && nearlyEqual(out.y, 0) && nearlyEqual(out.z, 4))
        std::cout << "OK\n";
    else
        std::cout << "FAIL (" << out.x << "," << out.y << "," << out.z << ")\n";
}

void test_rotZ_90() {
    Vec3 v{1,0,0};
    Mat4 Rm = rot3D(90, 'z');
    Vec3 out = Rm * v;

    std::cout << "Test rot3D Z 90° (1,0,0 -> 0,1,0): ";

    if(nearlyEqual(out.x, 0) && nearlyEqual(out.y, 1))
        std::cout << "OK\n";
    else
        std::cout << "FAIL (" << out.x << "," << out.y << "," << out.z << ")\n";
}

void test_rotX_180() {
    Vec3 v{0,1,0};
    Mat4 Rm = rot3D(180, 'x');
    Vec3 out = Rm * v;

    std::cout << "Test rot3D X 180° (0,1,0 -> 0,-1,0): ";

    if(nearlyEqual(out.y, -1))
        std::cout << "OK\n";
    else
        std::cout << "FAIL (" << out.x << "," << out.y << "," << out.z << ")\n";
}

void test_rotY_270() {
    Vec3 v{1,0,0};
    Mat4 Rm = rot3D(270, 'y');
    Vec3 out = Rm * v;

    std::cout << "Test rot3D Y 270° (1,0,0 -> 0,0,-1): ";

    if(nearlyEqual(out.x, 0) && nearlyEqual(out.z, -1))
        std::cout << "OK\n";
    else
        std::cout << "FAIL (" << out.x << "," << out.y << "," << out.z << ")\n";
}

int main() {
    std::cout << "===== Matrix3D Tests =====\n";

    test_matmul_identity();
    test_matmul_example();

    test_translate();

    test_rotZ_90();
    test_rotX_180();
    test_rotY_270();

    std::cout << "==========================\n";
    return 0;
}