// include the unit test framework first
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>

// include the module you're going to test next
#include "Matrix.hpp"
#include "SVD.hpp"

// any other libraries
#include <array>
#include <cmath>
#include <iostream>

// Helper functions
template <uint8_t rows, uint8_t columns>
float matrixSum(const Matrix<rows, columns> &matrix) {
  float sum = 0;
  for (uint32_t i = 0; i < rows * columns; i++) {
    float number = matrix.ToArray()[i];
    sum += number * number;
  }
  return std::sqrt(sum);
}

template <uint8_t rows, uint8_t columns>
void printLabeledMatrix(const std::string &label,
                        const Matrix<rows, columns> &matrix) {
  std::string strBuf = "";
  matrix.ToString(strBuf);
  std::cout << label << ":\n" << strBuf << std::endl;
}

TEST_CASE("Initialization", "Matrix") {
  SECTION("Array Initialization") {
    std::array<float, 4> arr2{5, 6, 7, 8};
    Matrix<2, 2> mat2{arr2};
    // array initialization
    REQUIRE(mat2.Get(0, 0) == 5);
    REQUIRE(mat2.Get(0, 1) == 6);
    REQUIRE(mat2.Get(1, 0) == 7);
    REQUIRE(mat2.Get(1, 1) == 8);
  }

  SECTION("Argument Pack Initialization") {
    Matrix<2, 2> mat1{1, 2, 3, 4};
    // template pack initialization
    REQUIRE(mat1.Get(0, 0) == 1);
    REQUIRE(mat1.Get(0, 1) == 2);
    REQUIRE(mat1.Get(1, 0) == 3);
    REQUIRE(mat1.Get(1, 1) == 4);
  }

  SECTION("Single Argument Pack Initialization") {
    Matrix<2, 2> mat1{2};
    // template pack initialization
    REQUIRE(mat1.Get(0, 0) == 2);
    REQUIRE(mat1.Get(0, 1) == 2);
    REQUIRE(mat1.Get(1, 0) == 2);
    REQUIRE(mat1.Get(1, 1) == 2);
  }
}

TEST_CASE("Elementary Matrix Operations", "Matrix") {
  std::array<float, 4> arr2{5, 6, 7, 8};
  Matrix<2, 2> mat1{1, 2, 3, 4};
  Matrix<2, 2> mat2{arr2};
  Matrix<2, 2> mat3{};

  SECTION("Fill") {
    mat1.Fill(0);
    REQUIRE(mat1.Get(0, 0) == 0);
    REQUIRE(mat1.Get(0, 1) == 0);
    REQUIRE(mat1.Get(1, 0) == 0);
    REQUIRE(mat1.Get(1, 1) == 0);

    mat2.Fill(100000);
    REQUIRE(mat2.Get(0, 0) == 100000);
    REQUIRE(mat2.Get(0, 1) == 100000);
    REQUIRE(mat2.Get(1, 0) == 100000);
    REQUIRE(mat2.Get(1, 1) == 100000);

    mat3.Fill(-20);
    REQUIRE(mat3.Get(0, 0) == -20);
    REQUIRE(mat3.Get(0, 1) == -20);
    REQUIRE(mat3.Get(1, 0) == -20);
    REQUIRE(mat3.Get(1, 1) == -20);
  }

  SECTION("Addition") {
    mat1.Add(mat2, mat3);

    REQUIRE(mat3.Get(0, 0) == 6);
    REQUIRE(mat3.Get(0, 1) == 8);
    REQUIRE(mat3.Get(1, 0) == 10);
    REQUIRE(mat3.Get(1, 1) == 12);

    // try out addition with overloaded operators
    mat3.Fill(0);
    mat3 = mat1 + mat2;
    REQUIRE(mat3.Get(0, 0) == 6);
    REQUIRE(mat3.Get(0, 1) == 8);
    REQUIRE(mat3.Get(1, 0) == 10);
    REQUIRE(mat3.Get(1, 1) == 12);
  }

  SECTION("Subtraction") {
    mat1.Sub(mat2, mat3);

    REQUIRE(mat3.Get(0, 0) == -4);
    REQUIRE(mat3.Get(0, 1) == -4);
    REQUIRE(mat3.Get(1, 0) == -4);
    REQUIRE(mat3.Get(1, 1) == -4);

    // try out subtraction with operators
    mat3.Fill(0);
    mat3 = mat1 - mat2;
    REQUIRE(mat3.Get(0, 0) == -4);
    REQUIRE(mat3.Get(0, 1) == -4);
    REQUIRE(mat3.Get(1, 0) == -4);
    REQUIRE(mat3.Get(1, 1) == -4);
  }

  SECTION("Multiplication") {
    mat1.Mult(mat2, mat3);

    REQUIRE(mat3.Get(0, 0) == 19);
    REQUIRE(mat3.Get(0, 1) == 22);
    REQUIRE(mat3.Get(1, 0) == 43);
    REQUIRE(mat3.Get(1, 1) == 50);

    // try out multiplication with operators
    mat3.Fill(0);
    mat3 = mat1 * mat2;
    REQUIRE(mat3.Get(0, 0) == 19);
    REQUIRE(mat3.Get(0, 1) == 22);
    REQUIRE(mat3.Get(1, 0) == 43);
    REQUIRE(mat3.Get(1, 1) == 50);

    // Non-square multiplication
    Matrix<2, 4> mat4{1, 2, 3, 4, 5, 6, 7, 8};
    Matrix<4, 2> mat5{9, 10, 11, 12, 13, 14, 15, 16};
    Matrix<2, 2> mat6{};
    mat6 = mat4 * mat5;
    REQUIRE(mat6.Get(0, 0) == 130);
    REQUIRE(mat6.Get(0, 1) == 140);
    REQUIRE(mat6.Get(1, 0) == 322);
    REQUIRE(mat6.Get(1, 1) == 348);

    // One more non-square multiplicaiton
    Matrix<4, 4> mat7{};
    mat7 = mat5 * mat4;
    REQUIRE(mat7.Get(0, 0) == 59);
    REQUIRE(mat7.Get(0, 1) == 78);
    REQUIRE(mat7.Get(0, 2) == 97);
    REQUIRE(mat7.Get(0, 3) == 116);
    REQUIRE(mat7.Get(1, 0) == 71);
    REQUIRE(mat7.Get(1, 1) == 94);
    REQUIRE(mat7.Get(1, 2) == 117);
    REQUIRE(mat7.Get(1, 3) == 140);
    REQUIRE(mat7.Get(2, 0) == 83);
    REQUIRE(mat7.Get(2, 1) == 110);
    REQUIRE(mat7.Get(2, 2) == 137);
    REQUIRE(mat7.Get(2, 3) == 164);
    REQUIRE(mat7.Get(3, 0) == 95);
    REQUIRE(mat7.Get(3, 1) == 126);
    REQUIRE(mat7.Get(3, 2) == 157);
    REQUIRE(mat7.Get(3, 3) == 188);
  }

  SECTION("Scalar Multiplication") {
    mat1.Mult(2, mat3);

    REQUIRE(mat3.Get(0, 0) == 2);
    REQUIRE(mat3.Get(0, 1) == 4);
    REQUIRE(mat3.Get(1, 0) == 6);
    REQUIRE(mat3.Get(1, 1) == 8);
  }

  SECTION("Element Multiply") {
    mat1.ElementMultiply(mat2, mat3);

    REQUIRE(mat3.Get(0, 0) == 5);
    REQUIRE(mat3.Get(0, 1) == 12);
    REQUIRE(mat3.Get(1, 0) == 21);
    REQUIRE(mat3.Get(1, 1) == 32);
  }

  SECTION("Element Divide") {
    mat1.ElementDivide(mat2, mat3);

    REQUIRE_THAT(mat3.Get(0, 0), Catch::Matchers::WithinRel(0.2f, 1e-6f));
    REQUIRE_THAT(mat3.Get(0, 1), Catch::Matchers::WithinRel(0.3333333f, 1e-6f));
    REQUIRE_THAT(mat3.Get(1, 0), Catch::Matchers::WithinRel(0.4285714f, 1e-6f));
    REQUIRE_THAT(mat3.Get(1, 1), Catch::Matchers::WithinRel(0.5f, 1e-6f));
  }

  SECTION("Minor Matrix") {
    // what about matrices of 0,0 or 1,1?
    // minor matrix for 2x2 matrix
    Matrix<1, 1> minorMat1{};
    mat1.MinorMatrix(minorMat1, 0, 0);
    REQUIRE(minorMat1.Get(0, 0) == 4);
    mat1.MinorMatrix(minorMat1, 0, 1);
    REQUIRE(minorMat1.Get(0, 0) == 3);
    mat1.MinorMatrix(minorMat1, 1, 0);
    REQUIRE(minorMat1.Get(0, 0) == 2);
    mat1.MinorMatrix(minorMat1, 1, 1);
    REQUIRE(minorMat1.Get(0, 0) == 1);

    // minor matrix for 3x3 matrix
    Matrix<3, 3> mat4{1, 2, 3, 4, 5, 6, 7, 8, 9};
    Matrix<2, 2> minorMat4{};

    mat4.MinorMatrix(minorMat4, 0, 0);
    REQUIRE(minorMat4.Get(0, 0) == 5);
    REQUIRE(minorMat4.Get(0, 1) == 6);
    REQUIRE(minorMat4.Get(1, 0) == 8);
    REQUIRE(minorMat4.Get(1, 1) == 9);

    mat4.MinorMatrix(minorMat4, 1, 1);
    REQUIRE(minorMat4.Get(0, 0) == 1);
    REQUIRE(minorMat4.Get(0, 1) == 3);
    REQUIRE(minorMat4.Get(1, 0) == 7);
    REQUIRE(minorMat4.Get(1, 1) == 9);

    mat4.MinorMatrix(minorMat4, 2, 2);
    REQUIRE(minorMat4.Get(0, 0) == 1);
    REQUIRE(minorMat4.Get(0, 1) == 2);
    REQUIRE(minorMat4.Get(1, 0) == 4);
    REQUIRE(minorMat4.Get(1, 1) == 5);
  }

  SECTION("Determinant") {
    float det1 = mat1.Det();

    REQUIRE_THAT(det1, Catch::Matchers::WithinRel(-2.0F, 1e-6f));

    Matrix<3, 3> mat4{1, 2, 3, 4, 5, 6, 7, 8, 9};

    float det4 = mat4.Det();

    REQUIRE_THAT(det4, Catch::Matchers::WithinRel(0.0F, 1e-6f));

    Matrix<3, 3> mat5{1, 0, 0, 0, 2, 0, 0, 0, 3};

    float det5 = mat5.Det();
    REQUIRE_THAT(det5, Catch::Matchers::WithinRel(6.0F, 1e-6f));
  }

  SECTION("Matrix of Minors") {
    mat1.MatrixOfMinors(mat3);
    REQUIRE_THAT(mat3.Get(0, 0), Catch::Matchers::WithinRel(4.0F, 1e-6f));
    REQUIRE_THAT(mat3.Get(0, 1), Catch::Matchers::WithinRel(3.0F, 1e-6f));
    REQUIRE_THAT(mat3.Get(1, 0), Catch::Matchers::WithinRel(2.0F, 1e-6f));
    REQUIRE_THAT(mat3.Get(1, 1), Catch::Matchers::WithinRel(1.0F, 1e-6f));

    Matrix<3, 3> mat4{1, 2, 3, 4, 5, 6, 7, 8, 9};
    Matrix<3, 3> mat5{0};
    mat4.MatrixOfMinors(mat5);
    REQUIRE_THAT(mat5.Get(0, 0), Catch::Matchers::WithinRel(-3.0F, 1e-6f));
    REQUIRE_THAT(mat5.Get(0, 1), Catch::Matchers::WithinRel(-6.0F, 1e-6f));
    REQUIRE_THAT(mat5.Get(0, 2), Catch::Matchers::WithinRel(-3.0F, 1e-6f));
    REQUIRE_THAT(mat5.Get(1, 0), Catch::Matchers::WithinRel(-6.0F, 1e-6f));
    REQUIRE_THAT(mat5.Get(1, 1), Catch::Matchers::WithinRel(-12.0F, 1e-6f));
    REQUIRE_THAT(mat5.Get(1, 2), Catch::Matchers::WithinRel(-6.0F, 1e-6f));
    REQUIRE_THAT(mat5.Get(2, 0), Catch::Matchers::WithinRel(-3.0F, 1e-6f));
    REQUIRE_THAT(mat5.Get(2, 1), Catch::Matchers::WithinRel(-6.0F, 1e-6f));
    REQUIRE_THAT(mat5.Get(2, 2), Catch::Matchers::WithinRel(-3.0F, 1e-6f));
  }

  SECTION("Invert") {
    mat3 = mat1.Invert();
    REQUIRE_THAT(mat3.Get(0, 0), Catch::Matchers::WithinRel(-2.0F, 1e-6f));
    REQUIRE_THAT(mat3.Get(0, 1), Catch::Matchers::WithinRel(1.0F, 1e-6f));
    REQUIRE_THAT(mat3.Get(1, 0), Catch::Matchers::WithinRel(1.5F, 1e-6f));
    REQUIRE_THAT(mat3.Get(1, 1), Catch::Matchers::WithinRel(-0.5F, 1e-6f));
  };

  SECTION("Transpose") {
    // transpose a square matrix
    mat3 = mat1.Transpose();

    REQUIRE(mat3.Get(0, 0) == 1);
    REQUIRE(mat3.Get(0, 1) == 3);
    REQUIRE(mat3.Get(1, 0) == 2);
    REQUIRE(mat3.Get(1, 1) == 4);

    // transpose a non-square matrix
    Matrix<2, 3> mat4{1, 2, 3, 4, 5, 6};
    Matrix<3, 2> mat5{};

    mat5 = mat4.Transpose();

    REQUIRE(mat5.Get(0, 0) == 1);
    REQUIRE(mat5.Get(0, 1) == 4);
    REQUIRE(mat5.Get(1, 0) == 2);
    REQUIRE(mat5.Get(1, 1) == 5);
    REQUIRE(mat5.Get(2, 0) == 3);
    REQUIRE(mat5.Get(2, 1) == 6);
  }

  SECTION("GET ROW") {
    Matrix<1, 2> mat1Rows{};
    mat1.GetRow(0, mat1Rows);
    REQUIRE(mat1Rows.Get(0, 0) == 1);
    REQUIRE(mat1Rows.Get(0, 1) == 2);

    mat1.GetRow(1, mat1Rows);
    REQUIRE(mat1Rows.Get(0, 0) == 3);
    REQUIRE(mat1Rows.Get(0, 1) == 4);
  }

  SECTION("GET COLUMN") {
    Matrix<2, 1> mat1Columns{};
    mat1.GetColumn(0, mat1Columns);
    REQUIRE(mat1Columns.Get(0, 0) == 1);
    REQUIRE(mat1Columns.Get(1, 0) == 3);

    mat1.GetColumn(1, mat1Columns);
    REQUIRE(mat1Columns.Get(0, 0) == 2);
    REQUIRE(mat1Columns.Get(1, 0) == 4);
  }

  SECTION("Get Sub-Matrices") {
    Matrix<3, 3> mat4{1, 2, 3, 4, 5, 6, 7, 8, 9};

    Matrix<2, 2> mat5 = mat4.SubMatrix<2, 2, 0, 0>();

    REQUIRE(mat5.Get(0, 0) == 1);
    REQUIRE(mat5.Get(0, 1) == 2);
    REQUIRE(mat5.Get(1, 0) == 4);
    REQUIRE(mat5.Get(1, 1) == 5);

    mat5 = mat4.SubMatrix<2, 2, 1, 1>();
    REQUIRE(mat5.Get(0, 0) == 5);
    REQUIRE(mat5.Get(0, 1) == 6);
    REQUIRE(mat5.Get(1, 0) == 8);
    REQUIRE(mat5.Get(1, 1) == 9);

    Matrix<3, 1> mat6 = mat4.SubMatrix<3, 1, 0, 0>();
    REQUIRE(mat6.Get(0, 0) == 1);
    REQUIRE(mat6.Get(1, 0) == 4);
    REQUIRE(mat6.Get(2, 0) == 7);

    Matrix<1, 3> mat7 = mat4.SubMatrix<1, 3, 0, 0>();
    REQUIRE(mat7.Get(0, 0) == 1);
    REQUIRE(mat7.Get(0, 1) == 2);
    REQUIRE(mat7.Get(0, 2) == 3);
  }

  SECTION("Set Sub-Matrices") {
    Matrix<3, 3> startMatrix{1, 2, 3, 4, 5, 6, 7, 8, 9};
    Matrix<3, 3> mat4 = startMatrix;

    Matrix<2, 2> mat5{10, 11, 12, 13};
    mat4.SetSubMatrix(0, 0, mat5);
    REQUIRE(mat4.Get(0, 0) == 10);
    REQUIRE(mat4.Get(0, 1) == 11);
    REQUIRE(mat4.Get(1, 0) == 12);
    REQUIRE(mat4.Get(1, 1) == 13);

    mat4 = startMatrix;
    mat4.SetSubMatrix(1, 1, mat5);
    REQUIRE(mat4.Get(1, 1) == 10);
    REQUIRE(mat4.Get(1, 2) == 11);
    REQUIRE(mat4.Get(2, 1) == 12);
    REQUIRE(mat4.Get(2, 2) == 13);

    Matrix<3, 1> mat6{10, 11, 12};
    mat4.SetSubMatrix(0, 0, mat6);
    REQUIRE(mat4.Get(0, 0) == 10);
    REQUIRE(mat4.Get(1, 0) == 11);
    REQUIRE(mat4.Get(2, 0) == 12);

    Matrix<1, 3> mat7{10, 11, 12};
    mat4.SetSubMatrix(0, 0, mat7);
    REQUIRE(mat4.Get(0, 0) == 10);
    REQUIRE(mat4.Get(0, 1) == 11);
    REQUIRE(mat4.Get(0, 2) == 12);
  }
}

TEST_CASE("Identity Matrix", "Matrix") {
  SECTION("Square Matrix") {
    Matrix<5, 5> matrix = Matrix<5, 5>::Identity();
    uint32_t oneColumnIndex{0};
    for (uint32_t row = 0; row < 5; row++) {
      for (uint32_t column = 0; column < 5; column++) {
        float value = matrix[row][column];
        if (oneColumnIndex == column) {
          REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
        } else {
          REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
        }
      }
      oneColumnIndex++;
    }
  }

  SECTION("Wide Matrix") {
    Matrix<2, 5> matrix = Matrix<2, 5>::Identity();

    uint32_t oneColumnIndex{0};
    for (uint32_t row = 0; row < 2; row++) {
      for (uint32_t column = 0; column < 5; column++) {
        float value = matrix[row][column];
        if (oneColumnIndex == column && row < 3) {
          REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
        } else {
          REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
        }
      }
      oneColumnIndex++;
    }
  }

  SECTION("Tall Matrix") {
    Matrix<5, 2> matrix = Matrix<5, 2>::Identity();
    uint32_t oneColumnIndex{0};
    for (uint32_t row = 0; row < 5; row++) {
      for (uint32_t column = 0; column < 2; column++) {
        float value = matrix[row][column];
        if (oneColumnIndex == column) {
          REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
        } else {
          REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
        }
      }
      oneColumnIndex++;
    }
  }
}

// TODO: Add test for scalar division
TEST_CASE("Euclidean Norm", "Matrix") {

  SECTION("2x2 Normalize") {
    Matrix<2, 2> mat1{1, 2, 3, 4};
    Matrix<2, 2> mat2{};

    mat2 = mat1 / mat1.EuclideanNorm();

    float sqrt_30{static_cast<float>(sqrt(30.0f))};

    REQUIRE(mat2.Get(0, 0) == 1 / sqrt_30);
    REQUIRE(mat2.Get(0, 1) == 2 / sqrt_30);
    REQUIRE(mat2.Get(1, 0) == 3 / sqrt_30);
    REQUIRE(mat2.Get(1, 1) == 4 / sqrt_30);

    REQUIRE_THAT(matrixSum(mat2), Catch::Matchers::WithinRel(1.0f, 1e-6f));
  }

  SECTION("2x1 (Vector) Normalize") {
    Matrix<2, 1> mat1{-0.878877044, 2.92092276};
    Matrix<2, 1> mat2{};
    mat2 = mat1 / mat1.EuclideanNorm();

    REQUIRE_THAT(mat2.Get(0, 0),
                 Catch::Matchers::WithinRel(-0.288129855179f, 1e-6f));
    REQUIRE_THAT(mat2.Get(1, 0),
                 Catch::Matchers::WithinRel(0.957591346325f, 1e-6f));

    float sum = matrixSum(mat2);
    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));
  }

  SECTION("Normalized vectors sum to 1") {
    Matrix<9, 1> mat1{1, 2, 3, 4, 5, 6, 7, 8, 9};
    Matrix<9, 1> mat2;
    mat2 = mat1 / mat1.EuclideanNorm();
    float sum = matrixSum(mat2);
    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));

    Matrix<2, 3> mat3{1, 2, 3, 4, 5, 6};
    Matrix<2, 3> mat4{};
    mat4 = mat3 / mat3.EuclideanNorm();
    sum = matrixSum(mat4);
    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));
  }
}

TEST_CASE("QR Decompositions", "Matrix") {
  SECTION("2x2 QRDecomposition") {
    Matrix<2, 2> A{1.0f, 2.0f, 3.0f, 4.0f};
    Matrix<2, 2> Q{}, R{};
    A.QRDecomposition(Q, R);

    // Check that Q * R ≈ A
    Matrix<2, 2> QR{};
    Q.Mult(R, QR);
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 2; ++j) {
        REQUIRE_THAT(QR[i][j], Catch::Matchers::WithinRel(A[i][j], 1e-4f));
      }
    }

    // Check that Q is orthonormal: Qᵀ * Q ≈ I
    Matrix<2, 2> Qt = Q.Transpose();
    Matrix<2, 2> QtQ{};
    Qt.Mult(Q, QtQ);
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 2; ++j) {
        if (i == j)
          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinRel(1.0f, 1e-4f));
        else
          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
      }
    }

    // Optional: R should be upper triangular
    REQUIRE(std::fabs(R[1][0]) < 1e-4f);

    // check that all Q values are correct
    REQUIRE_THAT(Q[0][0], Catch::Matchers::WithinRel(0.3162f, 1e-4f));
    REQUIRE_THAT(Q[0][1], Catch::Matchers::WithinRel(0.94868f, 1e-4f));
    REQUIRE_THAT(Q[1][0], Catch::Matchers::WithinRel(0.94868f, 1e-4f));
    REQUIRE_THAT(Q[1][1], Catch::Matchers::WithinRel(-0.3162f, 1e-4f));

    // check that all R values are correct
    REQUIRE_THAT(R[0][0], Catch::Matchers::WithinRel(3.16228f, 1e-4f));
    REQUIRE_THAT(R[0][1], Catch::Matchers::WithinRel(4.42719f, 1e-4f));
    REQUIRE_THAT(R[1][0], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
    REQUIRE_THAT(R[1][1], Catch::Matchers::WithinRel(0.63246f, 1e-4f));
  }

  SECTION("3x3 QRDecomposition") {
    // this symmetrix tridiagonal matrix is well behaved for testing
    Matrix<3, 3> A{1, 2, 3, 4, 5, 6, 7, 8, 9};

    Matrix<3, 3> Q{}, R{};
    A.QRDecomposition(Q, R);

    // Check that Q * R ≈ A
    Matrix<3, 3> QR{};
    QR = Q * R;
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        REQUIRE_THAT(QR[i][j], Catch::Matchers::WithinRel(A[i][j], 1e-4f));
      }
    }

    // Check that Qᵀ * Q ≈ I
    // Since the rank of this matrix is 2, only the top left 2x2 sub-matrix will
    // equal I.
    Matrix<3, 3> Qt = Q.Transpose();
    Matrix<3, 3> QtQ{};
    QtQ = Qt * Q;
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 2; ++j) {
        if (i == j)
          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinRel(1.0f, 1e-4f));
        else
          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
      }
    }

    // Optional: Check R is upper triangular
    for (int i = 1; i < 3; ++i) {
      for (int j = 0; j < i; ++j) {
        REQUIRE(std::fabs(R[i][j]) < 1e-4f);
      }
    }
  }

  SECTION("4x2 QRDecomposition") {
    // A simple 4x2 matrix
    Matrix<4, 2> A{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f};

    Matrix<4, 2> Q{};
    Matrix<2, 2> R{};
    A.QRDecomposition(Q, R);

    // Check that Q * R ≈ A
    Matrix<4, 2> QR{};
    Q.Mult(R, QR);
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 2; ++j) {
        REQUIRE_THAT(QR[i][j], Catch::Matchers::WithinRel(A[i][j], 1e-4f));
      }
    }

    // Check that Qᵀ * Q ≈ I₂
    Matrix<2, 4> Qt = Q.Transpose();
    Matrix<2, 2> QtQ{};
    Qt.Mult(Q, QtQ);
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 2; ++j) {
        if (i == j)
          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinRel(1.0f, 1e-4f));
        else
          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
      }
    }

    // Check R is upper triangular (i > j ⇒ R[i][j] ≈ 0)
    for (int i = 1; i < 2; ++i) {
      for (int j = 0; j < i; ++j) {
        REQUIRE(std::fabs(R[i][j]) < 1e-4f);
      }
    }
  }
}

TEST_CASE("Eigenvalues and Vectors", "Matrix") {
  SECTION("2x2 Eigen") {
    Matrix<2, 2> A{1.0f, 2.0f, 3.0f, 4.0f};
    Matrix<2, 2> vectors{};
    Matrix<2, 1> values{};

    A.EigenQR(vectors, values, 1000000, 1e-20f);

    REQUIRE_THAT(vectors[0][0], Catch::Matchers::WithinRel(0.41597f, 1e-4f));
    REQUIRE_THAT(vectors[1][0], Catch::Matchers::WithinRel(0.90938f, 1e-4f));
    REQUIRE_THAT(values[0][0], Catch::Matchers::WithinRel(5.372282f, 1e-4f));
    REQUIRE_THAT(values[1][0], Catch::Matchers::WithinRel(-0.372281f, 1e-4f));
  }

  SECTION("3x3 Rank Defficient Eigen") {
    SKIP("Skipping this because QR decomposition isn't ready for it");
    // this symmetrix tridiagonal matrix is well behaved for testing
    Matrix<3, 3> A{1, 2, 3, 4, 5, 6, 7, 8, 9};

    Matrix<3, 3> vectors{};
    Matrix<3, 1> values{};
    A.EigenQR(vectors, values, 1000000, 1e-8f);

    std::string strBuf1 = "";
    vectors.ToString(strBuf1);
    std::cout << "Vectors:\n" << strBuf1 << std::endl;
    strBuf1 = "";
    values.ToString(strBuf1);
    std::cout << "Values:\n" << strBuf1 << std::endl;

    REQUIRE_THAT(vectors[0][0], Catch::Matchers::WithinRel(0.23197f, 1e-4f));
    REQUIRE_THAT(vectors[1][0], Catch::Matchers::WithinRel(0.525322f, 1e-4f));
    REQUIRE_THAT(vectors[2][0], Catch::Matchers::WithinRel(0.81867f, 1e-4f));
    REQUIRE_THAT(values[0][0], Catch::Matchers::WithinRel(-1.11684f, 1e-4f));
    REQUIRE_THAT(values[1][0], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
    REQUIRE_THAT(values[2][0], Catch::Matchers::WithinRel(16.1168f, 1e-4f));
  }
}

// ============================================================================
// SVD Tests — Reference values computed via scipy.linalg.svd (Python)
// ============================================================================

/**
 * @brief Helper: compute Frobenius norm of a matrix.
 */
template <uint8_t rows, uint8_t columns>
static float frobeniusNorm(const Matrix<rows, columns> &M) {
  float sum = 0;
  for (uint8_t i = 0; i < rows; i++) {
    for (uint8_t j = 0; j < columns; j++) {
      float v = M.Get(i, j);
      sum += v * v;
    }
  }
  return sqrtf(sum);
}

/**
 * @brief Helper: compute reconstruction error ||A - UΣVᵀ||_F.
 *
 * Verifies the fundamental SVD identity A = U × diag(σ) × Vᵀ.
 * For non-square matrices, only the first min(rows,cols) singular values
 * contribute to the reconstruction.
 */
template <uint8_t rows, uint8_t columns>
static float svdReconstructionError(const Matrix<rows, columns> &A,
                                    const Matrix<rows, columns> &U,
                                    const Matrix<columns, 1> &sigma,
                                    const Matrix<columns, columns> &Vt) {
  // Compute U × diag(σ): only first min(rows,cols) columns of U are used
  constexpr uint8_t k = (rows < columns) ? rows : columns;
  Matrix<rows, columns> USigma{0};
  for (uint8_t i = 0; i < rows; i++) {
    for (uint8_t j = 0; j < k; j++) {
      USigma[i][j] = U.Get(i, j) * sigma.Get(j, 0);
    }
  }

  // Compute (UΣ) × Vᵀ: only first k rows of Vt are used
  Matrix<rows, columns> UVt{0};
  for (uint8_t i = 0; i < rows; i++) {
    for (uint8_t j = 0; j < columns; j++) {
      float sum = 0;
      for (uint8_t p = 0; p < k; p++) {
        sum += USigma[i][p] * Vt.Get(p, j);
      }
      UVt[i][j] = sum;
    }
  }

  // Compute ||A - UVᵀ||_F
  Matrix<rows, columns> diff{0};
  A.Sub(UVt, diff);
  return frobeniusNorm(diff);
}

/**
 * @brief Helper: check orthogonality of the first k columns of M.
 * Verifies M[:,0:k]ᵀ × M[:,0:k] ≈ I_k.
 */
template <uint8_t rows, uint8_t columns>
static float orthogonalityError(const Matrix<rows, columns> &M) {
  constexpr uint8_t k = (rows < columns) ? rows : columns;

  // Compute Mᵀ × M (should be I_k in top-left)
  Matrix<columns, rows> Mt = M.Transpose();
  Matrix<columns, columns> MtM{0};
  Mt.Mult(M, MtM);

  float err = 0;
  for (uint8_t i = 0; i < k; i++) {
    for (uint8_t j = 0; j < k; j++) {
      float expected = (i == j) ? 1.0f : 0.0f;
      err += (MtM.Get(i, j) - expected) * (MtM.Get(i, j) - expected);
    }
  }
  return sqrtf(err);
}

/**
 * @brief Helper: check that singular values are sorted in descending order.
 */
template <uint8_t maxCols>
static bool isSortedDescending(const Matrix<maxCols, 1> &sigma, uint8_t count) {
  for (uint8_t i = 0; i < count - 1; i++) {
    if (sigma.Get(i + 1, 0) > sigma.Get(i, 0) + 1e-6f) {
      return false;
    }
  }
  return true;
}

TEST_CASE("SVD: Simple 2x2 Matrix", "Matrix") {
  // Reference: scipy.linalg.svd([[1,2],[3,4]])
  // σ = [5.4649857042, 0.3659661906]
  Matrix<2, 2> A{1.0f, 2.0f, 3.0f, 4.0f};
  Matrix<2, 2> U{}, Vt{};
  Matrix<2, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  // Verify singular values (verified with Python scipy.linalg.svd)
  REQUIRE_THAT(sigma.Get(0, 0),
               Catch::Matchers::WithinRel(5.4649857042f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0),
               Catch::Matchers::WithinRel(0.3659661906f, 1e-4f));

  // Verify descending order
  REQUIRE(isSortedDescending(sigma, 2));

  // Verify U is orthogonal: UᵀU ≈ I
  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-4f));

  // Verify Vt is orthogonal: VtVᵀ ≈ I
  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-4f));

  // Verify reconstruction: A ≈ U Σ Vᵀ
  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
}

TEST_CASE("SVD: Symmetric Positive Definite 2x2", "Matrix") {
  // Reference: scipy.linalg.svd([[5,3],[3,5]])
  // σ = [8.0, 2.0] (eigenvalues since symmetric PD)
  Matrix<2, 2> A{5.0f, 3.0f, 3.0f, 5.0f};
  Matrix<2, 2> U{}, Vt{};
  Matrix<2, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(8.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(2.0f, 1e-4f));

  // For symmetric PD matrices, U ≈ V (up to sign)
  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
}

TEST_CASE("SVD: Full-Rank 3x3 Matrix", "Matrix") {
  // Reference: scipy.linalg.svd([[1,2,3],[4,5,6],[7,8,10]])
  // σ = [17.4125051668, 0.8751613501, 0.1968665211]
  Matrix<3, 3> A{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 10.0f};
  Matrix<3, 3> U{}, Vt{};
  Matrix<3, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0),
               Catch::Matchers::WithinRel(17.4125051668f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0),
               Catch::Matchers::WithinRel(0.8751613501f, 1e-4f));
  REQUIRE_THAT(sigma.Get(2, 0),
               Catch::Matchers::WithinRel(0.1968665211f, 1e-4f));

  REQUIRE(isSortedDescending(sigma, 3));
  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-4f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
}

TEST_CASE("SVD: Rank-Deficient 3x3 Matrix", "Matrix") {
  // Reference: scipy.linalg.svd([[1,2,3],[4,5,6],[7,8,9]])
  // σ = [16.8481033526, 1.0683695146, ~0] (rank 2)
  Matrix<3, 3> A{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f};
  Matrix<3, 3> U{}, Vt{};
  Matrix<3, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0),
               Catch::Matchers::WithinRel(16.8481033526f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0),
               Catch::Matchers::WithinRel(1.0683695146f, 1e-4f));
  // Third singular value should be ~0 (rank deficiency)
  REQUIRE(sigma.Get(2, 0) < 1e-3f);

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
}

TEST_CASE("SVD: Diagonal 3x3 Matrix", "Matrix") {
  // For a diagonal matrix, σ = diagonal entries, U = V = I
  Matrix<3, 3> A{10.0f, 0.0f, 0.0f, 5.0f, 0.0f, 0.0f, 0.0f, 0.0f, 2.0f};
  Matrix<3, 3> U{}, Vt{};
  Matrix<3, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(10.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(5.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(2.0f, 1e-4f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
}

TEST_CASE("SVD: Tall Matrix (4×3)", "Matrix") {
  // Reference: scipy.linalg.svd with full_matrices=False
  // σ = [25.4624074360, 1.2906616758, ~0] (rank 2)
  Matrix<4, 3> A{1.0f, 2.0f, 3.0f, 4.0f,  5.0f,  6.0f,
                 7.0f, 8.0f, 9.0f, 10.0f, 11.0f, 12.0f};
  Matrix<4, 3> U{};
  Matrix<3, 3> Vt{}; // Vt is always n×n
  Matrix<3, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0),
               Catch::Matchers::WithinRel(25.4624074360f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0),
               Catch::Matchers::WithinRel(1.2906616758f, 1e-4f));
  REQUIRE(sigma.Get(2, 0) < 1e-3f);

  // U should be 4×3 with orthonormal columns
  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-3f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
}

TEST_CASE("SVD: Wide Matrix (3×5)", "Matrix") {
  // Reference: scipy.linalg.svd with full_matrices=False
  // σ = [35.1272233336, 2.4653966969, ~0] (rank 2)
  Matrix<3, 5> A{1.0f, 2.0f,  3.0f,  4.0f,  5.0f,  6.0f,  7.0f, 8.0f,
                 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f};
  Matrix<3, 5> U{};
  Matrix<5, 5> Vt{}; // Vt is always n×n
  Matrix<5, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0),
               Catch::Matchers::WithinRel(35.1272233336f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0),
               Catch::Matchers::WithinRel(2.4653966969f, 1e-4f));
  REQUIRE(sigma.Get(2, 0) < 1e-3f);

  // Vt should be 5×5 with orthonormal rows (first k)
  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-3f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
}

TEST_CASE("SVD: 5×5 Symmetric Tridiagonal", "Matrix") {
  // Reference: scipy.linalg.svd for discrete Laplacian-like matrix
  // σ = [3.7320508076, 3.0, 2.0, 1.0, 0.2679491924]
  Matrix<5, 5> A{2.0f, -1.0f, 0.0f,  0.0f, 0.0f,  -1.0f, 2.0f, -1.0f, 0.0f,
                 0.0f, 0.0f,  -1.0f, 2.0f, -1.0f, 0.0f,  0.0f, 0.0f,  -1.0f,
                 2.0f, -1.0f, 0.0f,  0.0f, 0.0f,  -1.0f, 2.0f};
  Matrix<5, 5> U{}, Vt{};
  Matrix<5, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0),
               Catch::Matchers::WithinRel(3.7320508076f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(3.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(2.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(3, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(4, 0),
               Catch::Matchers::WithinRel(0.2679491924f, 1e-4f));

  REQUIRE(isSortedDescending(sigma, 5));
  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-3f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
}

TEST_CASE("SVD: Non-Square with Negative Values (2×3)", "Matrix") {
  // Reference: scipy.linalg.svd([[0.5,-0.3,0.8],[-0.2,0.7,0.1]])
  // σ = [1.0384009867, 0.6646227432]
  Matrix<2, 3> A{0.5f, -0.3f, 0.8f, -0.2f, 0.7f, 0.1f};
  Matrix<2, 3> U{};
  Matrix<3, 3> Vt{}; // Vt is always n×n
  Matrix<3, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0),
               Catch::Matchers::WithinRel(1.0384009867f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0),
               Catch::Matchers::WithinRel(0.6646227432f, 1e-4f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
}

TEST_CASE("SVD: Near-Singular 2×2 Matrix", "Matrix") {
  // Condition number ≈ 1e6 — tests numerical stability
  // Reference: scipy.linalg.svd([[1,0],[0,1e-6]])
  // σ = [1.0, 1e-6]
  Matrix<2, 2> A{1.0f, 0.0f, 0.0f, 1e-6f};
  Matrix<2, 2> U{}, Vt{};
  Matrix<2, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(1e-6f, 1e-2f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
}

TEST_CASE("SVD: Orthogonal Matrix (3×3)", "Matrix") {
  // For an orthogonal matrix, all singular values should be 1.
  // Rotation matrix about z-axis by 45°
  float c = sqrtf(0.5f); // cos(45°)
  float s = sqrtf(0.5f); // sin(45°)
  Matrix<3, 3> A{c, -s, 0.0f, s, c, 0.0f, 0.0f, 0.0f, 1.0f};
  Matrix<3, 3> U{}, Vt{};
  Matrix<3, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  // All singular values should be 1 for an orthogonal matrix
  REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
}

TEST_CASE("SVD: Identity Matrix", "Matrix") {
  // For I, σ = [1, 1, 1], U = V = I
  Matrix<3, 3> A{1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
  Matrix<3, 3> U{}, Vt{};
  Matrix<3, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
  REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
}

TEST_CASE("SVD: Zero Matrix", "Matrix") {
  // All singular values should be zero
  Matrix<3, 3> A{0.0f};
  Matrix<3, 3> U{}, Vt{};
  Matrix<3, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  REQUIRE(sigma.Get(0, 0) < 1e-6f);
  REQUIRE(sigma.Get(1, 0) < 1e-6f);
  REQUIRE(sigma.Get(2, 0) < 1e-6f);

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
}

TEST_CASE("SVD: 2×1 Column Vector", "Matrix") {
  // For a column vector v, σ = ||v||, U = v/||v|| (with padding)
  Matrix<2, 1> A{3.0f, 4.0f};
  Matrix<2, 1> U{};
  Matrix<1, 1> Vt{}; // Vt is always n×n
  Matrix<1, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  // σ should be the Euclidean norm: ||[3,4]|| = 5
  REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(5.0f, 1e-4f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
}

TEST_CASE("SVD: 1×2 Row Vector", "Matrix") {
  // For a row vector vᵀ, σ = ||v||, Vt = v/||v|| (with padding)
  Matrix<1, 2> A{3.0f, 4.0f};
  Matrix<1, 2> U{};
  Matrix<2, 2> Vt{}; // Vt is always n×n
  Matrix<2, 1> sigma{};

  SVD::SVD(A, U, sigma, Vt);

  // σ should be the Euclidean norm: ||[3,4]|| = 5
  REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(5.0f, 1e-4f));

  float reconErr = svdReconstructionError(A, U, sigma, Vt);
  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
}