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

namespace Matrix3D {

    Mat4 identity() {
        Mat4 m{};
        for(int i = 0; i < 4; i++) {
            for(int j = 0; j < 4; j++) {
                m[i][j] = (i == j) ? 1.0 : 0.0;
            }
        }
        return m;
    }

    Mat4 matmul(const Mat4& A, const Mat4& B) {
        Mat4 R{};
        for(int i = 0; i < 4; i++) {
            for(int j = 0; j < 4; j++) {
                double sum = 0.0;
                for(int k = 0; k < 4; k++) {
                    sum += A[i][k] * B[k][j];
                }
                R[i][j] = sum;
            }
        }
        return R;
    }

    Mat4 translate(const Vec3& pos) {
        Mat4 t = identity();
        t[0][3] = pos.x;
        t[1][3] = pos.y;
        t[2][3] = pos.z;
        return t;
    }

    Mat4 rot3D(double angle_deg, char axis) {
        Mat4 r = identity();

        double rad = angle_deg * M_PI / 180.0;
        double c = std::cos(rad);
        double s = std::sin(rad);

        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;
        }
        return r;
    }

    Vec3 operator*(const Mat4& M, const Vec3& v) {
        Vec3 out;
        out.x = M[0][0]*v.x + M[0][1]*v.y + M[0][2]*v.z + M[0][3];
        out.y = M[1][0]*v.x + M[1][1]*v.y + M[1][2]*v.z + M[1][3];
        out.z = M[2][0]*v.x + M[2][1]*v.y + M[2][2]*v.z + M[2][3];
        return out;
    }

    Mat4 operator*(const Mat4& A, const Mat4& B) {
        return matmul(A, B);
    }

}