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

// Matrix Multiplikation
std::array<std::array<double,4>,4> gameMatrix::matmul(
    const std::array<std::array<double,4>,4>& A,
    const std::array<std::array<double,4>,4>& B)
{
    std::array<std::array<double,4>,4> result = {0};

    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j)
        {
            result[i][j] = 0.0;
            for (int k = 0; k < 4; ++k)
                result[i][j] += A[i][k] * B[k][j];
        }

    return result;
}

//  Rotationsmatrix
std::array<std::array<double,4>,4> gameMatrix::rot3D(double angle_deg, char axis)
{
    double angle_rad = angle_deg * M_PI / 180.0;
    double c = std::cos(angle_rad);
    double s = std::sin(angle_rad);

    std::array<std::array<double,4>,4> R = identity();

    switch (axis)
    {
        case 'x': case 'X':
            R[1][1] = c; R[1][2] = -s;
            R[2][1] = s; R[2][2] = c;
            break;

        case 'y': case 'Y':
            R[0][0] = c; R[0][2] = s;
            R[2][0] = -s; R[2][2] = c;
            break;

        case 'z': case 'Z':
            R[0][0] = c; R[0][1] = -s;
            R[1][0] = s; R[1][1] = c;
            break;

        default:
            throw std::invalid_argument("Invalid axis for rotation (use x, y, or z)");
    }

    return R;
}


// Einheitsmatrix-Matrix
std::array<std::array<double,4>,4> gameMatrix::identity()
{
    std::array<std::array<double,4>,4> I = {0};
    for (int i = 0; i < 4; ++i)
        I[i][i] = 1.0;
    return I;
}

//  Translation
std::array<std::array<double,4>,4> gameMatrix::translate(const std::array<double,3>& pos)
{
    std::array<std::array<double,4>,4> T = identity();

    T[0][3] = pos[0];
    T[1][3] = pos[1];
    T[2][3] = pos[2];

    return T;
}