// 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: Frobenius norm of a 5×5 matrix
// ============================================================================
static float frobeniusNorm5(const Matrix<5, 5> &M) {
  float sum = 0.0f;
  for (uint8_t i = 0; i < 5; i++) {
    for (uint8_t j = 0; j < 5; j++) {
      float v = M.Get(i, j);
      sum += v * v;
    }
  }
  return sqrtf(sum);
}

// ============================================================================
// Helper: Check if a matrix is orthogonal (Mᵀ·M ≈ I)
// ============================================================================
static bool isOrthogonal5(const Matrix<5, 5> &M, float tol = 1e-6f) {
  Matrix<5, 5> Mt = M.Transpose();
  Matrix<5, 5> MtM{0};
  Mt.Mult(M, MtM);

  for (uint8_t i = 0; i < 5; i++) {
    for (uint8_t j = 0; j < 5; j++) {
      float expected = (i == j) ? 1.0f : 0.0f;
      if (fabsf(MtM.Get(i, j) - expected) > tol) {
        return false;
      }
    }
  }
  return true;
}

// ============================================================================
// TEST 1: ComputeHouseholder
// ============================================================================
TEST_CASE("SVD Building Block: ComputeHouseholder", "[Matrix][SVD]") {
  // Test case: [3, 4] should give alpha = -5 (norm), v normalized
  // Reference: scipy.linalg.householder([3, 4]) → v ≈ [0.894427191,
  // 0.447213596], α = -5
  {
    float x[] = {3.0f, 4.0f};
    float v[5] = {0};
    float alpha = 0;

    float norm = SVD::ComputeHouseholder(x, 2, v, alpha);

    // Norm should be 5.0
    REQUIRE_THAT(norm, Catch::Matchers::WithinRel(5.0f, 1e-6f));

    // Alpha should be -5 (negative norm)
    REQUIRE_THAT(alpha, Catch::Matchers::WithinRel(-5.0f, 1e-6f));

    // v should be normalized: ||v|| ≈ 1
    float vNorm = sqrtf(v[0] * v[0] + v[1] * v[1]);
    REQUIRE_THAT(vNorm, Catch::Matchers::WithinRel(1.0f, 1e-6f));

    // Verify H·x = [alpha, 0]: (I - 2vvᵀ)·x should give [-5, 0]
    float hx0 = x[0] - 2.0f * v[0] * (v[0] * x[0] + v[1] * x[1]);
    float hx1 = x[1] - 2.0f * v[1] * (v[0] * x[0] + v[1] * x[1]);
    REQUIRE_THAT(hx0, Catch::Matchers::WithinRel(alpha, 1e-6f));
    REQUIRE_THAT(hx1, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  // Test case: [1, 3]
  // Reference: norm = √10 ≈ 3.16228, alpha = -√10
  {
    float x[] = {1.0f, 3.0f};
    float v[5] = {0};
    float alpha = 0;

    float norm = SVD::ComputeHouseholder(x, 2, v, alpha);

    REQUIRE_THAT(norm, Catch::Matchers::WithinRel(sqrtf(10.0f), 1e-6f));
    REQUIRE_THAT(alpha, Catch::Matchers::WithinRel(-sqrtf(10.0f), 1e-6f));

    // Verify H·x = [alpha, 0]
    float dot = v[0] * x[0] + v[1] * x[1];
    float hx0 = x[0] - 2.0f * v[0] * dot;
    float hx1 = x[1] - 2.0f * v[1] * dot;
    REQUIRE_THAT(hx0, Catch::Matchers::WithinRel(alpha, 1e-6f));
    REQUIRE_THAT(hx1, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  // Test case: [1, 2, 3] (3D)
  // Reference: norm = √14 ≈ 3.74166
  {
    float x[] = {1.0f, 2.0f, 3.0f};
    float v[5] = {0};
    float alpha = 0;

    float norm = SVD::ComputeHouseholder(x, 3, v, alpha);

    REQUIRE_THAT(norm, Catch::Matchers::WithinRel(sqrtf(14.0f), 1e-6f));
    REQUIRE_THAT(alpha, Catch::Matchers::WithinRel(-sqrtf(14.0f), 1e-6f));

    // Verify v is normalized
    float vNorm = sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
    REQUIRE_THAT(vNorm, Catch::Matchers::WithinRel(1.0f, 1e-6f));

    // Verify H·x = [alpha, 0, 0]
    float dot = v[0] * x[0] + v[1] * x[1] + v[2] * x[2];
    for (uint8_t i = 0; i < 3; i++) {
      float hx_i = x[i] - 2.0f * v[i] * dot;
      if (i == 0) {
        REQUIRE_THAT(hx_i, Catch::Matchers::WithinRel(alpha, 1e-6f));
      } else {
        REQUIRE_THAT(hx_i, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
      }
    }
  }

  // Test case: [0, 0, 1] (already has leading zeros)
  {
    float x[] = {0.0f, 0.0f, 1.0f};
    float v[5] = {0};
    float alpha = 0;

    float norm = SVD::ComputeHouseholder(x, 3, v, alpha);

    REQUIRE_THAT(norm, Catch::Matchers::WithinRel(1.0f, 1e-6f));
    REQUIRE_THAT(alpha, Catch::Matchers::WithinRel(-1.0f, 1e-6f));

    // Verify H·x = [-1, 0, 0]
    float dot = v[0] * x[0] + v[1] * x[1] + v[2] * x[2];
    float hx0 = x[0] - 2.0f * v[0] * dot;
    float hx1 = x[1] - 2.0f * v[1] * dot;
    float hx2 = x[2] - 2.0f * v[2] * dot;
    REQUIRE_THAT(hx0, Catch::Matchers::WithinRel(alpha, 1e-6f));
    REQUIRE_THAT(hx1, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
    REQUIRE_THAT(hx2, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  // Test case: [5, -3, 2, 1] (4D)
  {
    float x[] = {5.0f, -3.0f, 2.0f, 1.0f};
    float v[5] = {0};
    float alpha = 0;

    float norm = SVD::ComputeHouseholder(x, 4, v, alpha);

    REQUIRE_THAT(norm, Catch::Matchers::WithinRel(sqrtf(39.0f), 1e-6f));
    REQUIRE_THAT(alpha, Catch::Matchers::WithinRel(-sqrtf(39.0f), 1e-6f));

    // Verify H·x = [alpha, 0, 0, 0]
    float dot = v[0] * x[0] + v[1] * x[1] + v[2] * x[2] + v[3] * x[3];
    for (uint8_t i = 0; i < 4; i++) {
      float hx_i = x[i] - 2.0f * v[i] * dot;
      if (i == 0) {
        REQUIRE_THAT(hx_i, Catch::Matchers::WithinRel(alpha, 1e-6f));
      } else {
        REQUIRE_THAT(hx_i, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
      }
    }
  }

  // Test case: zero vector
  {
    float x[] = {0.0f, 0.0f};
    float v[5] = {0};
    float alpha = 0;

    float norm = SVD::ComputeHouseholder(x, 2, v, alpha);

    REQUIRE_THAT(norm, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
    REQUIRE(alpha == 0.0f);
  }
}

// ============================================================================
// TEST 2: ApplyHouseholderLeft
// ============================================================================
TEST_CASE("SVD Building Block: ApplyHouseholderLeft", "[Matrix][SVD]") {
  // Test: Apply Householder to zero out column 0, rows 1:2 of a 3×3 matrix
  // Input: [[1, 2, 3], [4, 5, 6], [7, 8, 9]]
  // After applying HH on col 0 (rows 1:2): A[2,0] should be ~0
  {
    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0,    0, 4.0f, 5.0f, 6.0f, 0,
                   0,    7.0f, 8.0f, 9.0f, 0, 0,    0,    0,    0,
                   0,    0,    0,    0,    0, 0,    0};

    // Compute Householder for column 0, rows 1:2 → vector [4, 7]
    float x[] = {4.0f, 7.0f};
    float v[5] = {0};
    float alpha = 0;
    SVD::ComputeHouseholder(x, 2, v, alpha);

    // Apply from left
    SVD::ApplyHouseholderLeft(W, v, 1, 2);

    // A[2,0] should be ~0
    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));

    // Verify orthogonality of the transformation: W = H·W_original
    Matrix<5, 5> W_orig{1.0f, 2.0f, 3.0f, 0,    0, 4.0f, 5.0f, 6.0f, 0,
                        0,    7.0f, 8.0f, 9.0f, 0, 0,    0,    0,    0,
                        0,    0,    0,    0,    0, 0,    0};

    // Compute H_left explicitly: I - 2*v*vᵀ (on rows 1:2)
    Matrix<5, 5> H_left{0};
    for (uint8_t i = 0; i < 5; i++) {
      H_left[i][i] = 1.0f;
    }
    // Apply -2*v*vᵀ to the sub-block
    float vv = v[0] * v[0] + v[1] * v[1];
    for (uint8_t i = 1; i <= 2; i++) {
      for (uint8_t j = 1; j <= 2; j++) {
        H_left[i][j] -= 2.0f * v[i - 1] * v[j - 1] / vv;
      }
    }

    // Verify: W ≈ H_left · W_orig
    Matrix<5, 5> HLeftW{0};
    H_left.Mult(W_orig, HLeftW);
    float err = frobeniusNorm5(W - HLeftW);
    REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-4f));

    // Verify H_left is orthogonal
    REQUIRE(isOrthogonal5(H_left));
  }

  // Test: Apply to a larger block (4 rows)
  {
    Matrix<5, 5> W{1.0f, 2.0f, 0,    0, 0, 3.0f, 4.0f, 0,    0,
                   0,    5.0f, 6.0f, 0, 0, 0,    7.0f, 8.0f, 0,
                   0,    0,    0,    0, 0, 0,    0};

    // Householder on [3, 5, 7] (rows 1:3)
    float x[] = {3.0f, 5.0f, 7.0f};
    float v[5] = {0};
    float alpha = 0;
    SVD::ComputeHouseholder(x, 3, v, alpha);

    SVD::ApplyHouseholderLeft(W, v, 1, 3);

    // A[2,0] and A[3,0] should be ~0
    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
    REQUIRE_THAT(W.Get(3, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }
}

// ============================================================================
// TEST 3: ApplyHouseholderRight
// ============================================================================
TEST_CASE("SVD Building Block: ApplyHouseholderRight", "[Matrix][SVD]") {
  // Test: Apply Householder to zero out row 0, cols 1:2 of a 3×3 matrix
  // Input: [[1, 2, 3], [4, 5, 6], [7, 8, 9]]
  // After applying HH on row 0 (cols 1:2): A[0,2] should be ~0
  {
    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0,    0, 4.0f, 5.0f, 6.0f, 0,
                   0,    7.0f, 8.0f, 9.0f, 0, 0,    0,    0,    0,
                   0,    0,    0,    0,    0, 0,    0};

    // Householder for row 0, cols 1:2 → vector [2, 3]
    float x[] = {2.0f, 3.0f};
    float v[5] = {0};
    float alpha = 0;
    SVD::ComputeHouseholder(x, 2, v, alpha);

    // Apply from right
    SVD::ApplyHouseholderRight(W, v, 1, 2);

    // A[0,2] should be ~0
    REQUIRE_THAT(W.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-6f));

    // Verify W ≈ W_orig · H_right
    Matrix<5, 5> W_orig{1.0f, 2.0f, 3.0f, 0,    0, 4.0f, 5.0f, 6.0f, 0,
                        0,    7.0f, 8.0f, 9.0f, 0, 0,    0,    0,    0,
                        0,    0,    0,    0,    0, 0,    0};

    // Compute H_right = I - 2*v*vᵀ (on cols 1:2)
    Matrix<5, 5> H_right{0};
    for (uint8_t i = 0; i < 5; i++) {
      H_right[i][i] = 1.0f;
    }
    float vv = v[0] * v[0] + v[1] * v[1];
    for (uint8_t i = 1; i <= 2; i++) {
      for (uint8_t j = 1; j <= 2; j++) {
        H_right[i][j] -= 2.0f * v[i - 1] * v[j - 1] / vv;
      }
    }

    Matrix<5, 5> WOrigH{0};
    W_orig.Mult(H_right, WOrigH);
    float err = frobeniusNorm5(W - WOrigH);
    REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-4f));

    // Verify H_right is orthogonal
    REQUIRE(isOrthogonal5(H_right));
  }

  // Test: Apply to wider block (4 cols)
  {
    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 4.0f, 0, 5.0f, 6.0f, 7.0f, 8.0f,
                   0,    0,    0,    0,    0, 0,    0,    0,    0,
                   0,    0,    0,    0,    0, 0,    0};

    // Householder on [2, 3, 4] (cols 1:3)
    float x[] = {2.0f, 3.0f, 4.0f};
    float v[5] = {0};
    float alpha = 0;
    SVD::ComputeHouseholder(x, 3, v, alpha);

    SVD::ApplyHouseholderRight(W, v, 1, 3);

    // A[0,2] and A[0,3] should be ~0
    REQUIRE_THAT(W.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
    REQUIRE_THAT(W.Get(0, 3), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }
}

// ============================================================================
// TEST 4: ComputeGivens
// ============================================================================
TEST_CASE("SVD Building Block: ComputeGivens", "[Matrix][SVD]") {
  // Test case: [3, 4] → c = 0.6, s = 0.8 (3-4-5 triangle)
  {
    float c, s;
    SVD::ComputeGivens(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));

    // Verify: [c s; -s c] · [3; 4] = [5; 0]
    float r = c * 3.0f + s * 4.0f;
    float z = -s * 3.0f + c * 4.0f;
    REQUIRE_THAT(r, Catch::Matchers::WithinRel(5.0f, 1e-6f));
    REQUIRE_THAT(z, Catch::Matchers::WithinAbs(0.0f, 1e-6f));

    // Verify c² + s² = 1
    REQUIRE_THAT(c * c + s * s, Catch::Matchers::WithinRel(1.0f, 1e-6f));
  }

  // Test case: [1, 0] → c = 1, s = 0
  {
    float c, s;
    SVD::ComputeGivens(1.0f, 0.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(1.0f, 1e-6f));
    REQUIRE_THAT(s, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  // Test case: [0, 5] → c = 0, s = 1
  {
    float c, s;
    SVD::ComputeGivens(0.0f, 5.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
    REQUIRE_THAT(s, Catch::Matchers::WithinRel(1.0f, 1e-6f));

    // Verify: [c s; -s c] · [0; 5] = [5; 0]
    float r = c * 0.0f + s * 5.0f;
    float z = -s * 0.0f + c * 5.0f;
    REQUIRE_THAT(r, Catch::Matchers::WithinRel(5.0f, 1e-6f));
    REQUIRE_THAT(z, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  // Test case: [-3, -4] → c = -0.6, s = -0.8
  {
    float c, s;
    SVD::ComputeGivens(-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));

    // Verify: [c s; -s c] · [-3; -4] = [5; 0]
    float r = c * (-3.0f) + s * (-4.0f);
    float z = -s * (-3.0f) + c * (-4.0f);
    REQUIRE_THAT(r, Catch::Matchers::WithinRel(5.0f, 1e-6f));
    REQUIRE_THAT(z, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  // Test case: [1, -1] → c = 1/√2, s = -1/√2 (45°)
  {
    float c, s;
    SVD::ComputeGivens(1.0f, -1.0f, c, s);
    float invSqrt2 = 1.0f / sqrtf(2.0f);
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(invSqrt2, 1e-6f));
    REQUIRE_THAT(s, Catch::Matchers::WithinRel(-invSqrt2, 1e-6f));

    // Verify: [c s; -s c] · [1; -1] = [√2; 0]
    float r = c * 1.0f + s * (-1.0f);
    float z = -s * 1.0f + c * (-1.0f);
    REQUIRE_THAT(r, Catch::Matchers::WithinRel(sqrtf(2.0f), 1e-6f));
    REQUIRE_THAT(z, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  // Test case: [0, 0] → c = 1, s = 0 (identity)
  {
    float c, s;
    SVD::ComputeGivens(0.0f, 0.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(1.0f, 1e-6f));
    REQUIRE_THAT(s, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }

  // Test case: [7, 24] → c = 7/25, s = 24/25 (7-24-25 triangle)
  {
    float c, s;
    SVD::ComputeGivens(7.0f, 24.0f, c, s);
    REQUIRE_THAT(c, Catch::Matchers::WithinRel(7.0f / 25.0f, 1e-6f));
    REQUIRE_THAT(s, Catch::Matchers::WithinRel(24.0f / 25.0f, 1e-6f));

    float r = c * 7.0f + s * 24.0f;
    float z = -s * 7.0f + c * 24.0f;
    REQUIRE_THAT(r, Catch::Matchers::WithinRel(25.0f, 1e-6f));
    REQUIRE_THAT(z, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }
}

// ============================================================================
// TEST 5: ApplyGivensLeft
// ============================================================================
TEST_CASE("SVD Building Block: ApplyGivensLeft", "[Matrix][SVD]") {
  // Test: Apply Givens to zero out W[1,0] of a 2×2 matrix
  // Input: [[3, 4], [1, 2]]
  // Givens on rows 0,1 with x=W[0,0]=3, y=W[1,0]=1
  {
    Matrix<5, 5> W{3.0f, 4.0f, 0, 0, 0, 1.0f, 2.0f, 0, 0, 0, 0, 0, 0,
                   0,    0,    0, 0, 0, 0,    0,    0, 0, 0, 0, 0};

    float c, s;
    SVD::ComputeGivens(3.0f, 1.0f, c, s);

    SVD::ApplyGivensLeft(W, 0, 1, c, s, 0, 4);

    // W[1,0] should be ~0
    REQUIRE_THAT(W.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));

    // Verify W ≈ G · W_orig
    Matrix<5, 5> W_orig{3.0f, 4.0f, 0, 0, 0, 1.0f, 2.0f, 0, 0, 0, 0, 0, 0,
                        0,    0,    0, 0, 0, 0,    0,    0, 0, 0, 0, 0};

    // Givens rotation matrix (5×5)
    Matrix<5, 5> G{0};
    for (uint8_t i = 0; i < 5; i++) {
      G[i][i] = 1.0f;
    }
    G[0][0] = c;
    G[0][1] = s;
    G[1][0] = -s;
    G[1][1] = c;

    Matrix<5, 5> GW{0};
    G.Mult(W_orig, GW);
    float err = frobeniusNorm5(W - GW);
    REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-6f));

    // Verify G is orthogonal
    REQUIRE(isOrthogonal5(G));
  }

  // Test: Apply to larger range of columns
  {
    Matrix<5, 5> W{3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 1.0f, 2.0f, 3.0f, 4.0f,
                   5.0f, 0,    0,    0,    0,    0,    0,    0,    0,
                   0,    0,    0,    0,    0,    0,    0};

    float c, s;
    SVD::ComputeGivens(3.0f, 1.0f, c, s);

    SVD::ApplyGivensLeft(W, 0, 1, c, s, 0, 4);

    REQUIRE_THAT(W.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }
}

// ============================================================================
// TEST 6: ApplyGivensRight
// ============================================================================
TEST_CASE("SVD Building Block: ApplyGivensRight", "[Matrix][SVD]") {
  // Test: Apply Givens to zero out W[0,1] of a 2×2 matrix
  // Input: [[3, 4], [1, 2]]
  // Givens on cols 0,1 with x=W[0,0]=3, y=W[0,1]=4
  {
    Matrix<5, 5> W{3.0f, 4.0f, 0, 0, 0, 1.0f, 2.0f, 0, 0, 0, 0, 0, 0,
                   0,    0,    0, 0, 0, 0,    0,    0, 0, 0, 0, 0};

    float c, s;
    SVD::ComputeGivens(3.0f, 4.0f, c, s);

    SVD::ApplyGivensRight(W, 0, 1, c, s, 0, 4);

    // W[0,1] should be ~0
    REQUIRE_THAT(W.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f));

    // Verify W ≈ W_orig · G
    Matrix<5, 5> W_orig{3.0f, 4.0f, 0, 0, 0, 1.0f, 2.0f, 0, 0, 0, 0, 0, 0,
                        0,    0,    0, 0, 0, 0,    0,    0, 0, 0, 0, 0};

    // Givens rotation matrix (5×5)
    Matrix<5, 5> G{0};
    for (uint8_t i = 0; i < 5; i++) {
      G[i][i] = 1.0f;
    }
    G[0][0] = c;
    G[0][1] = -s;
    G[1][0] = s;
    G[1][1] = c;

    Matrix<5, 5> WG{0};
    W_orig.Mult(G, WG);
    float err = frobeniusNorm5(W - WG);
    REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-6f));

    // Verify G is orthogonal
    REQUIRE(isOrthogonal5(G));
  }

  // Test: Apply to larger range of rows
  {
    Matrix<5, 5> W{3.0f, 4.0f, 0, 0, 0, 1.0f, 2.0f, 0, 0, 0, 5.0f, 6.0f, 0,
                   0,    0,    0, 0, 0, 0,    0,    0, 0, 0, 0,    0};

    float c, s;
    SVD::ComputeGivens(3.0f, 4.0f, c, s);

    SVD::ApplyGivensRight(W, 0, 1, c, s, 0, 2);

    REQUIRE_THAT(W.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }
}

// ============================================================================
// TEST 7: Full Bidiagonalization (composing Householder steps)
// ============================================================================
TEST_CASE("SVD Building Block: Householder Bidiagonalization",
          "[Matrix][SVD]") {
  // Test: Bidiagonalize a 3×3 matrix and verify reconstruction
  // Input: [[1, 2, 3], [4, 5, 6], [7, 8, 10]]
  {
    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0,     0, 4.0f, 5.0f, 6.0f, 0,
                   0,    7.0f, 8.0f, 10.0f, 0, 0,    0,    0,    0,
                   0,    0,    0,    0,     0, 0,    0};

    // Step 1: Left HH on column 0, rows 1:2 → zero out W[2,0]
    {
      float x[] = {4.0f, 7.0f};
      float v[5] = {0};
      float alpha = 0;
      SVD::ComputeHouseholder(x, 2, v, alpha);
      SVD::ApplyHouseholderLeft(W, v, 1, 2);
    }

    // Step 2: Right HH on row 0, cols 1:2 → zero out W[0,2]
    {
      float x[] = {W.Get(0, 1), W.Get(0, 2)};
      float v[5] = {0};
      float alpha = 0;
      SVD::ComputeHouseholder(x, 2, v, alpha);
      SVD::ApplyHouseholderRight(W, v, 1, 2);
    }

    // Step 3: Left HH on column 1, rows 2:2 → nothing to do (single element)

    // Verify bidiagonal structure: for 3x3, zero elements are A[2][0] (below
    // subdiag in col 0) and A[0][2] (above superdiag in row 0) A[2][1] is the
    // subdiagonal element of col 1 — valid in bidiagonal form
    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
    REQUIRE_THAT(W.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
  }

  // Test: Bidiagonalize a 4×3 matrix
  {
    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0,    0, 4.0f, 5.0f,  6.0f,  0,
                   0,    7.0f, 8.0f, 9.0f, 0, 0,    10.0f, 11.0f, 12.0f,
                   0,    0,    0,    0,    0, 0,    0};

    // Step 1: Left HH on col 0, rows 1:3 → zero out W[2,0], W[3,0]
    {
      float x[] = {4.0f, 7.0f, 10.0f};
      float v[5] = {0};
      float alpha = 0;
      SVD::ComputeHouseholder(x, 3, v, alpha);
      SVD::ApplyHouseholderLeft(W, v, 1, 3);
    }

    // Step 2: Right HH on row 0, cols 1:2 → zero out W[0,2]
    {
      float x[] = {W.Get(0, 1), W.Get(0, 2)};
      float v[5] = {0};
      float alpha = 0;
      SVD::ComputeHouseholder(x, 2, v, alpha);
      SVD::ApplyHouseholderRight(W, v, 1, 2);
    }

    // Step 3: Left HH on col 1, rows 2:3 → zero out W[3,1]
    {
      float x[] = {W.Get(2, 1), W.Get(3, 1)};
      float v[5] = {0};
      float alpha = 0;
      SVD::ComputeHouseholder(x, 2, v, alpha);
      SVD::ApplyHouseholderLeft(W, v, 2, 3);
    }

    // Verify bidiagonal structure
    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(W.Get(3, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(W.Get(3, 1), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
  }

  // Test: Diagonal matrix (no transformations needed)
  {
    Matrix<5, 5> W{10.0f, 0, 0, 0, 0, 0, 5.0f, 0, 0, 0, 0, 0, 2.0f,
                   0,     0, 0, 0, 0, 0, 0,    0, 0, 0, 0, 0};

    // Householder on zero vector should be identity
    float x[] = {0.0f, 0.0f};
    float v[5] = {0};
    float alpha = 0;
    SVD::ComputeHouseholder(x, 2, v, alpha);

    // Applying identity should not change anything
    Matrix<5, 5> W_copy{10.0f, 0, 0, 0, 0, 0, 5.0f, 0, 0, 0, 0, 0, 2.0f,
                        0,     0, 0, 0, 0, 0, 0,    0, 0, 0, 0, 0};
    SVD::ApplyHouseholderLeft(W_copy, v, 1, 2);

    REQUIRE_THAT(frobeniusNorm5(W - W_copy),
                 Catch::Matchers::WithinAbs(0.0f, 1e-6f));
  }
}

// ============================================================================
// //
// ============================================================================
// TEST 8: Givens QR step on bidiagonal matrix
// ===========================================================================
TEST_CASE("SVD Building Block: Givens QR Step on Bidiagonal", "[Matrix][SVD]") {
  // Test: Apply left Givens to zero subdiagonal of a bidiagonal matrix,
  // then apply right Givens with restricted row range to restore bidiagonal
  // form.
  //
  // Input: 3x3 bidiagonal [[1, 2, 0], [3, -4, 5], [0, 6, -7]]
  // Step 1: Left Givens on rows 0,1 with x=W[0][0]=1, y=W[1][0]=3 -> zero
  // W[1][0] Step 2: Right Givens on cols 1,2 with x=W[0][1], y=W[0][2] -> zero
  // W[0][2]
  //         Only applied to row 0 (to not reintroduce subdiagonal non-zeros)
  {
    Matrix<5, 5> W{1.0f, 2.0f, 0.0f, 0,     0, 3.0f, -4.0f, 5.0f, 0,
                   0,    0.0f, 6.0f, -7.0f, 0, 0,    0,     0,    0,
                   0,    0,    0,    0,     0, 0,    0};

    float c, s;
    SVD::ComputeGivens(W.Get(0, 0), W.Get(1, 0), c, s);

    // Apply from left to zero subdiagonal at W[1][0]
    SVD::ApplyGivensLeft(W, 0, 1, c, s, 0, 4);

    REQUIRE_THAT(W.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));

    // After left Givens, W[0][2] may have become non-zero (fill-in from row 0)
    // Apply right Givens to cols 1,2 with x=W[0][1], y=W[0][2] -> zero W[0][2]
    // Only apply to rows 0 (to preserve bidiagonal structure below row 0)
    float c2, s2;
    SVD::ComputeGivens(W.Get(0, 1), W.Get(0, 2), c2, s2);
    SVD::ApplyGivensRight(W, 1, 2, c2, s2, 0, 0);

    // Should be bidiagonal: W[1][0] ~ 0 (from left Givens), W[0][2] ~ 0 (from
    // right Givens)
    REQUIRE_THAT(W.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(W.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
  }

  // Test: Verify that a full QR step (left + right Givens) preserves the
  // bidiagonal structure when applied correctly with proper row ranges.
  {
    Matrix<5, 5> W{2.0f, 3.0f, 0, 0, 0, -1.0f, 4.0f, 5.0f, 0, 0, 0, 6.0f, -7.0f,
                   0,    0,    0, 0, 0, 0,     0,    0,    0, 0, 0, 0};

    // Left Givens on col 0 (rows 0,1)
    float c, s;
    SVD::ComputeGivens(W.Get(0, 0), W.Get(1, 0), c, s);
    SVD::ApplyGivensLeft(W, 0, 1, c, s, 0, 4);

    REQUIRE_THAT(W.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));

    // Right Givens on row 0 (cols 1,2) - only affect row 0
    float c2, s2;
    SVD::ComputeGivens(W.Get(0, 1), W.Get(0, 2), c2, s2);
    SVD::ApplyGivensRight(W, 1, 2, c2, s2, 0, 0);

    // Bidiagonal structure preserved
    REQUIRE_THAT(W.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
    REQUIRE_THAT(W.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
  }
}

// ============================================================================TEST
// 9: Orthogonality preservation of Householder transformations
// ============================================================================
TEST_CASE("SVD Building Block: Householder preserves orthogonality",
          "[Matrix][SVD]") {
  // Starting with an orthogonal matrix, applying Householder should preserve it
  {
    // Identity matrix is orthogonal
    Matrix<5, 5> M{0};
    for (uint8_t i = 0; i < 5; i++) {
      M[i][i] = 1.0f;
    }

    // Householder on first 3 elements of column 0
    float x[] = {1.0f, 0.0f, 0.0f};
    float v[5] = {0};
    float alpha = 0;
    SVD::ComputeHouseholder(x, 3, v, alpha);

    // Apply from left
    Matrix<5, 5> M_left = M;
    SVD::ApplyHouseholderLeft(M_left, v, 0, 2);

    // M_left should still be orthogonal
    REQUIRE(isOrthogonal5(M_left));

    // Apply from right
    Matrix<5, 5> M_right = M;
    SVD::ApplyHouseholderRight(M_right, v, 0, 2);

    REQUIRE(isOrthogonal5(M_right));
  }

  // Random orthogonal matrix (rotation)
  {
    float c = sqrtf(0.5f);
    float s = sqrtf(0.5f);
    Matrix<5, 5> M{0};
    M[0][0] = c;
    M[0][1] = -s;
    M[1][0] = s;
    M[1][1] = c;
    for (uint8_t i = 2; i < 5; i++) {
      M[i][i] = 1.0f;
    }

    REQUIRE(isOrthogonal5(M));

    // Apply Householder on rows 0,1
    float x[] = {c, s};
    float v[5] = {0};
    float alpha = 0;
    SVD::ComputeHouseholder(x, 2, v, alpha);

    Matrix<5, 5> M_test = M;
    SVD::ApplyHouseholderLeft(M_test, v, 0, 1);

    REQUIRE(isOrthogonal5(M_test));
  }
}

// ============================================================================
// TEST 10: Orthogonality preservation of Givens transformations
// ============================================================================
TEST_CASE("SVD Building Block: Givens preserves orthogonality",
          "[Matrix][SVD]") {
  // Starting with an orthogonal matrix, applying Givens should preserve it
  {
    Matrix<5, 5> M{0};
    for (uint8_t i = 0; i < 5; i++) {
      M[i][i] = 1.0f;
    }

    float c, s;
    SVD::ComputeGivens(3.0f, 4.0f, c, s);

    // Apply from left
    Matrix<5, 5> M_left = M;
    SVD::ApplyGivensLeft(M_left, 0, 1, c, s, 0, 4);

    REQUIRE(isOrthogonal5(M_left));

    // Apply from right
    Matrix<5, 5> M_right = M;
    SVD::ApplyGivensRight(M_right, 0, 1, c, s, 0, 4);

    REQUIRE(isOrthogonal5(M_right));
  }
}