// 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 "QR.hpp"

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

// ============================================================================
// Helpers
// ============================================================================

/**
 * @brief Frobenius norm of an N x N matrix.
 */
template <uint8_t N>
static float frob(const Matrix<N, N> &M) {
  float sum = 0.0f;
  for (uint8_t i = 0; i < N; i++)
    for (uint8_t j = 0; j < N; j++) {
      float v = M.Get(i, j);
      sum += v * v;
    }
  return sqrtf(sum);
}

/**
 * @brief Check M is orthogonal (M^T M ~ I).
 */
template <uint8_t N>
static bool isOrthogonal(const Matrix<N, N> &M, float tol = 1e-5f) {
  Matrix<N, N> Mt = M.Transpose();
  Matrix<N, N> MtM{};
  Mt.Mult(M, MtM);
  for (uint8_t i = 0; i < N; i++)
    for (uint8_t j = 0; j < N; j++) {
      float expected = (i == j) ? 1.0f : 0.0f;
      if (fabsf(MtM.Get(i, j) - expected) > tol)
        return false;
    }
  return true;
}

/**
 * @brief 3x3 trace.
 */
static float trace3(const Matrix<3, 3> &A) {
  return A.Get(0, 0) + A.Get(1, 1) + A.Get(2, 2);
}

/**
 * @brief 3x3 sum of principal 2x2 minors (2nd elementary invariant).
 */
static float e2_3x3(const Matrix<3, 3> &A) {
  return A.Get(0, 0) * A.Get(1, 1) - A.Get(0, 1) * A.Get(0, 1) +
         A.Get(0, 0) * A.Get(2, 2) - A.Get(0, 2) * A.Get(0, 2) +
         A.Get(1, 1) * A.Get(2, 2) - A.Get(1, 2) * A.Get(1, 2);
}

/**
 * @brief 3x3 determinant.
 */
static float det3(const Matrix<3, 3> &A) {
  return A.Get(0, 0) *
             (A.Get(1, 1) * A.Get(2, 2) - A.Get(1, 2) * A.Get(2, 1)) -
         A.Get(0, 1) *
             (A.Get(1, 0) * A.Get(2, 2) - A.Get(1, 2) * A.Get(2, 0)) +
         A.Get(0, 2) *
             (A.Get(1, 0) * A.Get(2, 1) - A.Get(1, 1) * A.Get(2, 0));
}

/**
 * @brief Sign-invariant comparison of |actual| against refAbs.
 */
static bool matchesAbs(float actual, float refAbs, float relTol = 1e-5f,
                       float absTol = 1e-6f) {
  float a = fabsf(actual);
  if (refAbs < 1e-3f)
    return a < absTol + relTol;
  return fabsf(a - refAbs) <= relTol * refAbs;
}

// ============================================================================
// TEST 1: GivensRotation
// ============================================================================
TEST_CASE("QR Building Block: GivensRotation", "[Matrix][QR]") {
  // R = [[c, s], [-s, c]] must satisfy R * (a, b)^T = (r, 0)^T.

  {
    // Reference: hypot(2, 1) = sqrt(5) = 2.236067977
    float c = 0, s = 0;
    QR::GivensRotation(2.0f, 1.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(0.894427191f, 1e-6f));
    REQUIRE_THAT(s, Catch::Matchers::WithinRel(0.447213595f, 1e-6f));
    REQUIRE_THAT(c * 2.0f + s * 1.0f,
                 Catch::Matchers::WithinRel(2.236067977f, 1e-6f));
    REQUIRE_THAT(-s * 2.0f + c * 1.0f, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  {
    // Reference: hypot(3, 4) = 5 exactly
    float c = 0, s = 0;
    QR::GivensRotation(3.0f, 4.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(0.6f, 1e-6f));
    REQUIRE_THAT(s, Catch::Matchers::WithinRel(0.8f, 1e-6f));
    REQUIRE_THAT(c * 3.0f + s * 4.0f, Catch::Matchers::WithinRel(5.0f, 1e-6f));
    REQUIRE_THAT(-s * 3.0f + c * 4.0f, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  {
    // Pure second component: c = 0, s = 1
    float c = 1, s = 1;
    QR::GivensRotation(0.0f, 5.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinAbs(0.0f, 1e-7f));
    REQUIRE_THAT(s, Catch::Matchers::WithinRel(1.0f, 1e-6f));
  }

  {
    // Zero vector: identity rotation
    float c = 0, s = 0;
    QR::GivensRotation(0.0f, 0.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(1.0f, 1e-7f));
    REQUIRE_THAT(s, Catch::Matchers::WithinAbs(0.0f, 1e-7f));
  }

  {
    // Negative first component preserves the sign of c
    float c = 0, s = 0;
    QR::GivensRotation(-2.0f, 1.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(-0.894427191f, 1e-6f));
    REQUIRE_THAT(s, Catch::Matchers::WithinRel(0.447213595f, 1e-6f));
    REQUIRE_THAT(-s * -2.0f + c * 1.0f, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }
}

// ============================================================================
// TEST 2: ApplyRotationBothSides (similarity A <- G A G^T)
// ============================================================================
TEST_CASE("QR Building Block: ApplyRotationBothSides", "[Matrix][QR]") {
  // Reference (numpy, float64): A = [[2,1,0],[1,3,1],[0,1,4]], i = 0,
  // Givens(2,1) -> G A G^T =
  // [[ 3.0,         1.0,           0.447213595],
  //  [ 1.0,         2.0,           0.894427191],
  //  [ 0.447213595, 0.894427191,   4.0]]
  // (Note: G A G^T with G zeroing (2,1) sends the A[0][1] coupling into the
  // (0,2) corner, NOT into the subdiagonal -- the subdiagonal-zeroing happens
  // in the QR chase context where the bulge column has the right shape.)
  {
    Matrix<3, 3> A{2, 1, 0, 1, 3, 1, 0, 1, 4};
    float c = 0.894427191f, s = 0.447213595f;

    QR::ApplyRotationBothSides(A, 0, c, s);

    REQUIRE_THAT(A.Get(0, 0), Catch::Matchers::WithinRel(3.0f, 1e-5f));
    REQUIRE_THAT(A.Get(0, 1), Catch::Matchers::WithinRel(1.0f, 1e-5f));
    REQUIRE_THAT(A.Get(0, 2),
                 Catch::Matchers::WithinRel(0.447213595f, 1e-5f));
    REQUIRE_THAT(A.Get(1, 1), Catch::Matchers::WithinRel(2.0f, 1e-5f));
    REQUIRE_THAT(A.Get(1, 2),
                 Catch::Matchers::WithinRel(0.894427191f, 1e-5f));
    REQUIRE_THAT(A.Get(2, 2), Catch::Matchers::WithinRel(4.0f, 1e-5f));

    // Symmetry must be preserved exactly in both triangles
    for (uint8_t i = 0; i < 3; i++)
      for (uint8_t j = 0; j < 3; j++)
        REQUIRE(A.Get(i, j) == A.Get(j, i));
  }

  // Same check at i = 1.
  // Reference (numpy, float64): B = [[5,0,1],[0,6,2],[1,2,7]], i = 1,
  // Givens(6,2) -> G B G^T =
  // [[ 5.0,          0.316227766,  0.948683298],
  //  [ 0.316227766,   7.3,          1.9],
  //  [ 0.948683298,   1.9,          5.7]]
  {
    Matrix<3, 3> B{5, 0, 1, 0, 6, 2, 1, 2, 7};
    float c = 0.948683298f, s = 0.316227766f;

    QR::ApplyRotationBothSides(B, 1, c, s);

    REQUIRE_THAT(B.Get(0, 0), Catch::Matchers::WithinRel(5.0f, 1e-5f));
    REQUIRE_THAT(B.Get(0, 1),
                 Catch::Matchers::WithinRel(0.316227766f, 1e-5f));
    REQUIRE_THAT(B.Get(0, 2),
                 Catch::Matchers::WithinRel(0.948683298f, 1e-5f));
    REQUIRE_THAT(B.Get(1, 1), Catch::Matchers::WithinRel(7.3f, 1e-5f));
    REQUIRE_THAT(B.Get(1, 2), Catch::Matchers::WithinRel(1.9f, 1e-5f));
    REQUIRE_THAT(B.Get(2, 2), Catch::Matchers::WithinRel(5.7f, 1e-5f));

    for (uint8_t i = 0; i < 3; i++)
      for (uint8_t j = 0; j < 3; j++)
        REQUIRE(B.Get(i, j) == B.Get(j, i));
  }

  // Identity rotation leaves the matrix unchanged
  {
    Matrix<3, 3> C{1, 2, 3, 2, 4, 5, 3, 5, 6};
    QR::ApplyRotationBothSides(C, 1, 1.0f, 0.0f);
    REQUIRE(C.Get(0, 0) == 1.0f);
    REQUIRE(C.Get(0, 1) == 2.0f);
    REQUIRE(C.Get(0, 2) == 3.0f);
    REQUIRE(C.Get(1, 1) == 4.0f);
    REQUIRE(C.Get(1, 2) == 5.0f);
    REQUIRE(C.Get(2, 2) == 6.0f);
  }

  // Spectrum invariants (trace, Frobenius norm) are preserved. (c, s)
  // must be a unit vector for G A G^T to be a similarity transform.
  {
    Matrix<3, 3> D{1, 2, 3, 2, 5, 8, 3, 8, 9};
    float tr = trace3(D);
    float fn = frob(D);
    float c = 0.6f, s = 0.8f;
    QR::ApplyRotationBothSides(D, 0, c, s);
    REQUIRE_THAT(trace3(D), Catch::Matchers::WithinRel(tr, 1e-5f));
    REQUIRE_THAT(frob(D), Catch::Matchers::WithinRel(fn, 1e-5f));
  }
}
// ============================================================================
// TEST 3: ApplyRotationToVectors (V <- V G^T)
// ============================================================================
TEST_CASE("QR Building Block: ApplyRotationToVectors", "[Matrix][QR]") {
  // V = I, i = 0, Givens(2,1): V <- I * G^T with G^T = [[c, -s], [s, c]] =
  // [[ c,  -s,  0],
  //  [ s,   c,  0],
  //  [ 0,   0,  1]]
  {
    Matrix<3, 3> V{0};
    V[0][0] = 1;
    V[1][1] = 1;
    V[2][2] = 1;
    float c = 0.894427191f, s = 0.447213595f;

    QR::ApplyRotationToVectors(V, 0, c, s);

    REQUIRE_THAT(V.Get(0, 0), Catch::Matchers::WithinRel(0.894427191f, 1e-6f));
    REQUIRE_THAT(V.Get(0, 1), Catch::Matchers::WithinRel(-0.447213595f, 1e-6f));
    REQUIRE_THAT(V.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
    REQUIRE_THAT(V.Get(1, 0), Catch::Matchers::WithinRel(0.447213595f, 1e-6f));
    REQUIRE_THAT(V.Get(1, 1), Catch::Matchers::WithinRel(0.894427191f, 1e-6f));
    REQUIRE_THAT(V.Get(1, 2), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
    REQUIRE_THAT(V.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
    REQUIRE_THAT(V.Get(2, 1), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
    REQUIRE_THAT(V.Get(2, 2), Catch::Matchers::WithinRel(1.0f, 1e-7f));

    // Product of rotations must stay orthogonal
    REQUIRE(isOrthogonal(V));
  }

  // Two successive rotations accumulate (V <- V G1^T G2^T)
  // Reference (numpy, float64):
  // [[ 0.894427191, -0.424264069,  0.141421356],
  //  [ 0.447213595,  0.848528137, -0.282842712],
  //  [ 0.0,          0.316227766,  0.948683298]]
  {
    Matrix<3, 3> V{0};
    V[0][0] = 1;
    V[1][1] = 1;
    V[2][2] = 1;
    QR::ApplyRotationToVectors(V, 0, 0.894427191f, 0.447213595f);
    QR::ApplyRotationToVectors(V, 1, 0.948683298f, 0.316227766f);
    REQUIRE(isOrthogonal(V));
    // Column 0 was only touched by the first rotation
    REQUIRE_THAT(V.Get(0, 0), Catch::Matchers::WithinRel(0.894427191f, 1e-5f));
    REQUIRE_THAT(V.Get(1, 0), Catch::Matchers::WithinRel(0.447213595f, 1e-5f));
    REQUIRE_THAT(V.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
    REQUIRE_THAT(V.Get(0, 1), Catch::Matchers::WithinRel(-0.424264069f, 1e-5f));
    REQUIRE_THAT(V.Get(0, 2), Catch::Matchers::WithinRel(0.141421356f, 1e-5f));
    REQUIRE_THAT(V.Get(1, 2), Catch::Matchers::WithinRel(-0.282842712f, 1e-5f));
    REQUIRE_THAT(V.Get(2, 1), Catch::Matchers::WithinRel(0.316227766f, 1e-5f));
    REQUIRE_THAT(V.Get(2, 2), Catch::Matchers::WithinRel(0.948683298f, 1e-5f));
  }
}

// ============================================================================
// TEST 4: WilkinsonShift
// ============================================================================
TEST_CASE("QR Building Block: WilkinsonShift", "[Matrix][QR]") {
  // mu = (a+d)/2 - sign(a-d) * sqrt(((a-d)/2)^2 + b^2)
  // Reference: eigenvalues of [[2,1],[1,4]] are 1.5858, 4.4142; closest
  // to d = 4 is 4.414213562.
  REQUIRE_THAT(QR::WilkinsonShift(2.0f, 1.0f, 4.0f),
               Catch::Matchers::WithinRel(4.414213562f, 1e-6f));

  // [[5,2],[2,1]]: eigenvalues 0.1716, 5.8284; closest to d = 1 is 0.171572875
  REQUIRE_THAT(QR::WilkinsonShift(5.0f, 2.0f, 1.0f),
               Catch::Matchers::WithinRel(0.171572875f, 1e-5f));

  // Zero off-diagonal: returns d itself (sign(0) = +1 picks d, not a)
  REQUIRE_THAT(QR::WilkinsonShift(3.0f, 0.0f, 7.0f),
               Catch::Matchers::WithinRel(7.0f, 1e-7f));
  REQUIRE_THAT(QR::WilkinsonShift(7.0f, 0.0f, 3.0f),
               Catch::Matchers::WithinRel(3.0f, 1e-7f));

  // a == d: shift is the larger-magnitude off-diagonal combination
  // [[1,3],[3,1]]: eigenvalues -2, 4; closest to d = 1 is -2
  REQUIRE_THAT(QR::WilkinsonShift(1.0f, 3.0f, 1.0f),
               Catch::Matchers::WithinRel(-2.0f, 1e-6f));
}

// ============================================================================
// TEST 5: Solve2x2Eigen
// ============================================================================
TEST_CASE("QR Building Block: Solve2x2Eigen", "[Matrix][QR]") {
  // Symmetric block [[2,1],[1,3]]:
  // eigenvalues 1.381966011, 3.618033989;
  // eigenvector of 3.618033989 is +/- (0.525731112, 0.850650808)
  {
    Matrix<2, 2> A{2, 1, 1, 3};
    float lHi = 0, lLo = 0, c = 0, s = 0;
    QR::Solve2x2Eigen(A, 0, lHi, lLo, c, s);

    REQUIRE_THAT(lHi, Catch::Matchers::WithinRel(3.618033989f, 1e-6f));
    REQUIRE_THAT(lLo, Catch::Matchers::WithinRel(1.381966011f, 1e-6f));
    REQUIRE(matchesAbs(c, 0.525731112f));
    REQUIRE(matchesAbs(s, 0.850650808f));

    // Residual: A * vHi = lHi * vHi with vHi = (c, s)
    REQUIRE_THAT(c * 2.0f + s * 1.0f,
                 Catch::Matchers::WithinRel(lHi * c, 1e-5f));
    REQUIRE_THAT(c * 1.0f + s * 3.0f,
                 Catch::Matchers::WithinRel(lHi * s, 1e-5f));
    // Second eigenvector vLo = (-s, c)
    REQUIRE_THAT(-s * 2.0f + c * 1.0f,
                 Catch::Matchers::WithinRel(lLo * -s, 1e-5f));
    REQUIRE_THAT(-s * 1.0f + c * 3.0f,
                 Catch::Matchers::WithinRel(lLo * c, 1e-5f));
  }

  // Nonsymmetric block [[1,2],[3,4]] (used by the N == 2 entry point):
  // eigenvalues 5.372281323, -0.372281323;
  // eigenvector of 5.372281323 is +/- (0.415973558, 0.909376709)
  {
    Matrix<2, 2> A{1, 2, 3, 4};
    float lHi = 0, lLo = 0, c = 0, s = 0;
    QR::Solve2x2Eigen(A, 0, lHi, lLo, c, s);

    REQUIRE_THAT(lHi, Catch::Matchers::WithinRel(5.372281323f, 1e-6f));
    REQUIRE_THAT(lLo, Catch::Matchers::WithinRel(-0.372281323f, 1e-6f));
    REQUIRE(matchesAbs(c, 0.415973558f));
    REQUIRE(matchesAbs(s, 0.909376709f));

    // Both-row residual with vHi = (c, s): A v = l v
    REQUIRE_THAT(c * 1.0f + s * 2.0f,
                 Catch::Matchers::WithinRel(lHi * c, 1e-5f));
    REQUIRE_THAT(c * 3.0f + s * 4.0f,
                 Catch::Matchers::WithinRel(lHi * s, 1e-5f));
  }

  // Diagonal blocks: eigenvectors are coordinate vectors
  {
    Matrix<2, 2> A{5, 0, 0, 2};
    float lHi = 0, lLo = 0, c = 0, s = 0;
    QR::Solve2x2Eigen(A, 0, lHi, lLo, c, s);
    REQUIRE_THAT(lHi, Catch::Matchers::WithinRel(5.0f, 1e-7f));
    REQUIRE_THAT(lLo, Catch::Matchers::WithinRel(2.0f, 1e-7f));
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(1.0f, 1e-7f));
    REQUIRE_THAT(s, Catch::Matchers::WithinAbs(0.0f, 1e-7f));

    A = Matrix<2, 2>{2, 0, 0, 5};
    QR::Solve2x2Eigen(A, 0, lHi, lLo, c, s);
    REQUIRE_THAT(lHi, Catch::Matchers::WithinRel(5.0f, 1e-7f));
    REQUIRE_THAT(lLo, Catch::Matchers::WithinRel(2.0f, 1e-7f));
    REQUIRE_THAT(c, Catch::Matchers::WithinAbs(0.0f, 1e-7f));
    REQUIRE_THAT(s, Catch::Matchers::WithinRel(1.0f, 1e-7f));
  }
}
// ============================================================================
// TEST 6: Deflate
// ============================================================================
TEST_CASE("QR Building Block: Deflate", "[Matrix][QR]") {
  // subdiag[0] = 1e-9 <= 1e-6 * (|2| + |3|) = 5e-6  -> deflated
  // subdiag[1] = 0.5  > 1e-6 * (|3| + |4|) = 7e-6  -> kept
  {
    Matrix<3, 3> A{2, 1e-9f, 0, 1e-9f, 3, 0.5f, 0, 0.5f, 4};
    QR::Deflate(A, 0, 2, 1e-6f);

    REQUIRE(A.Get(1, 0) == 0.0f);
    REQUIRE(A.Get(0, 1) == 0.0f);
    REQUIRE_THAT(A.Get(2, 1), Catch::Matchers::WithinRel(0.5f, 1e-7f));
    REQUIRE_THAT(A.Get(1, 2), Catch::Matchers::WithinRel(0.5f, 1e-7f));
    // Diagonals untouched
    REQUIRE_THAT(A.Get(0, 0), Catch::Matchers::WithinRel(2.0f, 1e-7f));
    REQUIRE_THAT(A.Get(1, 1), Catch::Matchers::WithinRel(3.0f, 1e-7f));
    REQUIRE_THAT(A.Get(2, 2), Catch::Matchers::WithinRel(4.0f, 1e-7f));
  }

  // Nothing deflated when all subdiagonals are well above tolerance
  {
    Matrix<3, 3> A{2, 0.1f, 0, 0.1f, 3, 0.2f, 0, 0.2f, 4};
    QR::Deflate(A, 0, 2, 1e-6f);
    REQUIRE_THAT(A.Get(1, 0), Catch::Matchers::WithinRel(0.1f, 1e-7f));
    REQUIRE_THAT(A.Get(2, 1), Catch::Matchers::WithinRel(0.2f, 1e-7f));
  }
}

// ============================================================================
// TEST 7: Tridiagonalize
// ============================================================================
TEST_CASE("QR Building Block: Tridiagonalize", "[Matrix][QR]") {
  // 4x4 symmetric with a full (0,3) corner coupling
  {
    Matrix<4, 4> A{2, 1, 0, 1, 1, 3, 1, 0, 0, 1, 4, 1, 1, 0, 1, 5};
    Matrix<4, 4> Aorig = A;
    Matrix<4, 4> U{0};

    QR::Tridiagonalize(A, U);

    // Off-tridiagonal entries must be zero up to float32 roundoff (the
    // Givens zeroing cancels only in exact arithmetic; residuals are
    // ~1e-7 for O(1) entries).
    REQUIRE_THAT(A.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(A.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(A.Get(0, 3), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(A.Get(3, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(A.Get(1, 3), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(A.Get(3, 1), Catch::Matchers::WithinAbs(0.0f, 1e-5f));

    // Symmetry preserved exactly
    for (uint8_t i = 0; i < 4; i++)
      for (uint8_t j = 0; j < 4; j++)
        REQUIRE(A.Get(i, j) == A.Get(j, i));

    // U must be orthogonal
    REQUIRE(isOrthogonal(U));

    // Reconstruction: U * A_tri * U^T == Aorig (absolute check for
    // originally-zero entries: WithinRel has no absolute fallback there)
    Matrix<4, 4> UAt{};
    U.Mult(A, UAt);
    Matrix<4, 4> UAtU{};
    UAt.Mult(U.Transpose(), UAtU);
    for (uint8_t i = 0; i < 4; i++)
      for (uint8_t j = 0; j < 4; j++) {
        float actual = UAtU.Get(i, j);
        float expected = Aorig.Get(i, j);
        if (fabsf(expected) < 1e-3f)
          REQUIRE_THAT(actual, Catch::Matchers::WithinAbs(0.0f, 1e-5f));
        else
          REQUIRE_THAT(actual,
                       Catch::Matchers::WithinRel(expected, 1e-5f));
      }

    // Spectrum invariants match the original
    {
      float tr0 = Aorig.Get(0, 0) + Aorig.Get(1, 1) + Aorig.Get(2, 2) +
                  Aorig.Get(3, 3);
      float tr1 = A.Get(0, 0) + A.Get(1, 1) + A.Get(2, 2) + A.Get(3, 3);
      REQUIRE_THAT(tr1, Catch::Matchers::WithinRel(tr0, 1e-6f));
      REQUIRE_THAT(frob(A), Catch::Matchers::WithinRel(frob(Aorig), 1e-6f));
    }

    // Eigenvalues of the tridiagonal match the original (scipy reference):
    // 6.0, 4.0, 3.0, 1.0
    {
      Matrix<4, 1> vals{};
      Matrix<4, 4> vecs{};
      QR::EigenQR(A, vecs, vals, 10000, 1e-6f);
      REQUIRE_THAT(vals[0][0], Catch::Matchers::WithinRel(6.0f, 1e-4f));
      REQUIRE_THAT(vals[1][0], Catch::Matchers::WithinRel(4.0f, 1e-4f));
      REQUIRE_THAT(vals[2][0], Catch::Matchers::WithinRel(3.0f, 1e-4f));
      REQUIRE_THAT(vals[3][0], Catch::Matchers::WithinRel(1.0f, 1e-4f));
    }
  }

  // 5x5 symmetric
  {
    Matrix<5, 5> A{3, 1, 0, 0, 1, 1, 4, 1, 0, 0, 0, 1, 5, 1, 0, 0, 0, 1, 6, 1,
                   1, 0, 0, 1, 7};
    Matrix<5, 5> Aorig = A;
    Matrix<5, 5> U{0};

    QR::Tridiagonalize(A, U);

    // All |i - j| >= 2 entries zero up to float32 roundoff
    for (uint8_t i = 0; i < 5; i++)
      for (uint8_t j = 0; j < 5; j++)
        if (i > j + 1 || j > i + 1)
          REQUIRE_THAT(A.Get(i, j), Catch::Matchers::WithinAbs(0.0f, 1e-5f));

    REQUIRE(isOrthogonal(U));

    Matrix<5, 5> UAt{};
    U.Mult(A, UAt);
    Matrix<5, 5> UAtU{};
    UAt.Mult(U.Transpose(), UAtU);
    for (uint8_t i = 0; i < 5; i++)
      for (uint8_t j = 0; j < 5; j++) {
        float actual = UAtU.Get(i, j);
        float expected = Aorig.Get(i, j);
        if (fabsf(expected) < 1e-3f)
          REQUIRE_THAT(actual, Catch::Matchers::WithinAbs(0.0f, 1e-5f));
        else
          REQUIRE_THAT(actual,
                       Catch::Matchers::WithinRel(expected, 1e-5f));
      }
  }

  // Already tridiagonal: U must come out as the identity
  {
    Matrix<3, 3> A{1, 2, 0, 2, 5, 2, 0, 2, 9};
    Matrix<3, 3> U{0};
    QR::Tridiagonalize(A, U);
    for (uint8_t i = 0; i < 3; i++)
      for (uint8_t j = 0; j < 3; j++) {
        float expected = (i == j) ? 1.0f : 0.0f;
        REQUIRE_THAT(U.Get(i, j), Catch::Matchers::WithinAbs(expected, 1e-7f));
      }
  }
}
// ============================================================================
// TEST 8: One full shifted QR step (integration of the blocks)
// ============================================================================
TEST_CASE("QR Building Block: Full Shifted QR Step", "[Matrix][QR]") {
  // One Wilkinson-shifted QR step on the whole 3x3 block is a similarity
  // transform, so all spectrum invariants (trace, sum of principal 2x2
  // minors, determinant) must be preserved.
  //
  // A = [[1,2,3],[2,5,8],[3,8,9]]:  tr = 15, e2 = -18, det = -4
  {
    Matrix<3, 3> A{1, 2, 3, 2, 5, 8, 3, 8, 9};
    float tr0 = trace3(A);   // 15
    float e20 = e2_3x3(A);   // -18
    float det0 = det3(A);    // -4

    // mu from the trailing 2x2 [[5,8],[8,9]]: eigenvalues
    // -1.246211251, 15.246211251; closest to d = 9 is 15.246211251 (Wilkinson)
    float mu = QR::WilkinsonShift(A.Get(1, 1), A.Get(2, 1), A.Get(2, 2));
    REQUIRE_THAT(mu, Catch::Matchers::WithinRel(15.246211251f, 1e-5f));

    for (uint8_t i = 0; i < 3; i++)
      A[i][i] -= mu;

    // Bulge chase: rotations on (0,1) then (1,2)
    float c = 0, s = 0;
    QR::GivensRotation(A.Get(0, 0), A.Get(1, 0), c, s);
    QR::ApplyRotationBothSides(A, 0, c, s);
    QR::GivensRotation(A.Get(1, 1), A.Get(2, 1), c, s);
    QR::ApplyRotationBothSides(A, 1, c, s);

    for (uint8_t i = 0; i < 3; i++)
      A[i][i] += mu;

    // Symmetry preserved
    for (uint8_t i = 0; i < 3; i++)
      for (uint8_t j = 0; j < 3; j++)
        REQUIRE(A.Get(i, j) == A.Get(j, i));

    // Spectrum invariants preserved
    REQUIRE_THAT(trace3(A), Catch::Matchers::WithinRel(tr0, 1e-5f));
    REQUIRE_THAT(e2_3x3(A), Catch::Matchers::WithinRel(e20, 1e-5f));
    REQUIRE_THAT(det3(A), Catch::Matchers::WithinRel(det0, 1e-5f));
  }

  // For TRIDIAGONAL input a single step keeps the tridiagonal structure
  {
    Matrix<3, 3> T{1, 2, 0, 2, 5, 2, 0, 2, 9};
    float mu = QR::WilkinsonShift(T.Get(1, 1), T.Get(2, 1), T.Get(2, 2));
    for (uint8_t i = 0; i < 3; i++)
      T[i][i] -= mu;
    float c = 0, s = 0;
    QR::GivensRotation(T.Get(0, 0), T.Get(1, 0), c, s);
    QR::ApplyRotationBothSides(T, 0, c, s);
    QR::GivensRotation(T.Get(1, 1), T.Get(2, 1), c, s);
    QR::ApplyRotationBothSides(T, 1, c, s);
    for (uint8_t i = 0; i < 3; i++)
      T[i][i] += mu;

    // Corners must vanish up to float32 roundoff: tridiagonal form
    // maintained. The cancellation is exact in exact arithmetic (the
    // corner is s1*a - c1*b times a factor, and Givens gives s1*a = c1*b),
    // so the residual is pure rounding, ~1e-6 for O(1) entries.
    REQUIRE_THAT(T.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(T.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
  }
}