diff --git a/src/jacobi.cpp b/src/jacobi.cpp
index 00fd9eb..d597793 100644
--- a/src/jacobi.cpp
+++ b/src/jacobi.cpp
@@ -6,7 +6,9 @@
  *  @bug The eigenvalues fail the test. When comparing them to arma::eigsym
  *  there is one value that is way off when testing with a 6x6 matrix.
  */
+#include <algorithm>
 #include <cmath>
+#include <utility>
 
 #include "utils.hpp"
 #include "jacobi.hpp"
@@ -75,11 +77,19 @@ void jacobi_eigensolver(const arma::mat& A,
 
     } while (max_offdiag >= eps && ++iterations < maxiter);
 
-    // The diagonal elements of A_m are the eigenvalues
-    eigenvalues = arma::diagvec(A_m);
-    eigenvectors = R;
+    // Get the diagonal of A
+    arma::vec res = arma::diagvec(A_m);
+    // Get the indeces of res in sorted order
+    arma::uvec sorted_indeces = arma::sort_index(res);
+
+    eigenvalues.resize(res.n_elem);
+    eigenvectors.resize(res.n_elem, res.n_elem);
+    
+    // Set the eigenvalues and their corresponding vectors in order
+    for (int i=0; i < res.n_elem; i++) {
+        eigenvalues[i] = res[sorted_indeces[i]]; 
+        eigenvectors.insert_cols(i, R.col(sorted_indeces[i]));
+    }
 
     converged = max_offdiag < eps;
-    A_m.print();
-    R.print();
 }