1 files changed, 37 insertions, 2 deletions

diff --git a/hadamard_mcmc.hpp b/hadamard_mcmc.hpp
index bb948de..41d7a48 100644
--- a/hadamard_mcmc.hpp
+++ b/hadamard_mcmc.hpp
@@ -40,6 +40,34 @@ public:
 
     return E;
   }
+
+  void orthogonalize() {
+    for (unsigned i = 0; i < d; i++) {
+      for (unsigned j = 0; j < i; j++) {
+        double ij = 0;
+        for (unsigned k = 0; k < d; k++) {
+          ij += (*this)(i, k) * (*this)(j, k);
+        }
+
+        ij /= d;
+
+        for (unsigned k = 0; k < d; k++) {
+          (*this)(i, k) -= ij * (*this)(j, k);
+        }
+      }
+
+      double ii = 0;
+      for (unsigned j = 0; j < d; j++) {
+        ii += pow((*this)(i, j), 2);
+      }
+
+      ii = sqrt(ii / d);
+
+      for (unsigned j = 0; j < d; j++) {
+        (*this)(i, j) = (*this)(i, j) / ii;
+      }
+    }
+  }
 };
 
 class Givens {
@@ -187,13 +215,20 @@ public:
     }
   }
 
-  void run(unsigned N, bool dry = false) {
+  double run(unsigned N, bool dry = false, unsigned nOrth = 1000) {
+    double totalRatio = 0;
+
     for (unsigned i = 0; i < N; i++) {
-      this->sweep(dry);
+      totalRatio += this->sweep(dry);
       if (!dry) {
         A.after_sweep(θ0, E, M);
       }
+      if (i % nOrth == 0) {
+        M.orthogonalize();
+      }
     }
+
+    return totalRatio / N;
   }
 };