#include "gamematrix.h"


namespace Matrix3D {
    class gameMatrix {
      public:
        ///
        /// @param matrix1 - First matrix
        /// @param matrix2 - Second matrix
        /// @return Returns the multiplication of matrix 1 and matrix 2.
        static std::array<std::array<double,4>,4> matmul(const std::array<std::array<double,4>,4>& matrix1,
                                                   const std::array<std::array<double,4>,4>& matrix2) {

            std::array<std::array<double,4>,4> matrix3 = {};
            if (matrix1[0].size() != matrix2.size()) {
                  throw std::invalid_argument("Number of columns of matrix 1 must be equal to number of rows of matrix 2");
            };
            for (int i = 0; i < matrix1.size(); i++) {
              std::array<double,4> temp_array = {};
              for (int j = 0; j < matrix2[i].size(); j++) {
                double temp = 0;
                for (int k = 0; k < matrix2.size(); k++) {
                  temp += matrix1[i][k] * matrix2[k][j];
                }
                temp_array[j] = temp;
              }
              matrix3[i] = temp_array;
            }
            return matrix3;
        }

        ///
        /// @param angle_deg Rotation angle
        /// @param axis Rotation axis (x, y, z)
        /// @return Returns a rotation matrix based on a given angle and axis.
        static std::array<std::array<double,4>,4> rot3D(double angle_deg, char axis) {
          if (axis != 'x' && axis != 'y' && axis != 'z') {
            throw std::invalid_argument("Invalid axis");
          };
          const auto angle_rad = angle_deg * M_PI / 180;
          std::array<std::array<double,4>,4> rotationMatrix = {};
          switch (axis) {
            case 'x': {
              std::array<std::array<double,4>,4> rMx = {{
                {1, 0, 0, 0},
                {0, cos(angle_rad), -sin(angle_rad), 0},
                {0, sin(angle_rad), cos(angle_rad), 0},
                {0, 0, 0, 1}
              }};
              rotationMatrix = rMx;
            }
              break;
            case 'y': {
              std::array<std::array<double,4>,4> rMy = {{
                {cos(angle_rad), 0, sin(angle_rad), 0},
                {0, 1, 0, 0},
                {-sin(angle_rad), 0, cos(angle_rad), 0},
                {0, 0, 0, 1}
              }};
              rotationMatrix = rMy;
            }
              break;

            case 'z': {
              std::array<std::array<double,4>,4> rMz = {{
                {cos(angle_rad), -sin(angle_rad), 0, 0},
                {sin(angle_rad), cos(angle_rad), 0, 0},
                {0, 0, 1, 0},
                {0, 0, 0, 1}
              }};
              rotationMatrix = rMz;
            }
              break;
          }
          return rotationMatrix;
        };


        ///
        /// @param pos 3 point vector (x, y, z)
        /// @return Returns a 4x4 translation matrix base on a given vector.
        static std::array<std::array<double,4>,4> translate(const std::array<double, 3>& pos) {
          std::array<std::array<double,4>,4> transMatrix = {{{1, 0, 0, pos[0]}, {0, 1, 0, pos[1]}, {0, 0, 1, pos[2]}, {0, 0, 0, 1}}};
          return transMatrix;
        };

    };
};