Small refactor of SVD implimentation

src/SVD.hpp

@@ -78,6 +78,50 @@ static void ApplyHouseholderLeft(Matrix<5, 5> &W, const float *v,
 static void ApplyHouseholderRight(Matrix<5, 5> &W, const float *v,
                                   uint8_t startCol, uint8_t endCol);
 
+/**
+ * @brief Extract singular values from bidiagonal matrix diagonal and sort.
+ *
+ * Extracts absolute values of diagonal elements of W as singular values,
+ * then sorts them in descending order while reordering columns of QL
+ * and QR to maintain consistency.
+ *
+ * @param W Input: bidiagonal matrix (5×5 working array)
+ * @param sigma Output: sorted singular values (5×1 column vector, only first p used)
+ * @param p Number of singular values (min(rows, columns))
+ * @param QL Input/output: left transformation matrix (modified during sort)
+ * @param QR Input/output: right transformation matrix (modified during sort)
+ */
+static void ExtractAndSortSingularValues(Matrix<5, 5> &W,
+                                          Matrix<5, 1> &sigma,
+                                          uint8_t p,
+                                          Matrix<5, 5> &QL,
+                                          Matrix<5, 5> &QR);
+
+/**
+ * @brief Assemble final U and Vt matrices from QL/QR.
+ *
+ * Computes the final left singular vectors (U) and right singular vectors
+ * transposed (Vt) from the accumulated Householder transformations.
+ *
+ * For non-transpose case: U = QL[:,0:p], Vt = QR[:,0:p]ᵀ
+ * For transpose case:    U = QR[:,0:p]ᵀ, Vt = QL[:,0:p]ᵀ
+ *
+ * @param m Number of rows in original matrix
+ * @param n Number of columns in original matrix
+ * @param p Rank = min(m, n)
+ * @param transposeNeeded True if we computed SVD(Aᵀ) instead of SVD(A)
+ * @param QL Left Householder accumulation (5×5)
+ * @param QR Right Householder accumulation (5×5)
+ * @param U Output: left singular vectors (m×n matrix, only first p columns used)
+ * @param Vt Output: right singular vectors transposed (n×n matrix, only first p rows used)
+ */
+static void AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
+                           bool transposeNeeded,
+                           const Matrix<5, 5> &QL,
+                           const Matrix<5, 5> &QR,
+                           Matrix<5, 5> &U,
+                           Matrix<5, 5> &Vt);
+
 /**
  * @brief Compute a Givens rotation that zeros out y.
  *