From a47125a5c94fc75125547d64c693e7384ed9b70d Mon Sep 17 00:00:00 2001
From: Regina Kleinhappel <regina@treefish.org>
Date: Mon, 5 Jan 2015 01:52:28 +0100
Subject: [PATCH] ...

---
 u1casc-ordinary/obs_diagcorr.hpp | 50 ++++++++++++++++++++++++++++----
 1 file changed, 44 insertions(+), 6 deletions(-)

diff --git a/u1casc-ordinary/obs_diagcorr.hpp b/u1casc-ordinary/obs_diagcorr.hpp
index 57b835b..f51df7c 100644
--- a/u1casc-ordinary/obs_diagcorr.hpp
+++ b/u1casc-ordinary/obs_diagcorr.hpp
@@ -16,6 +16,9 @@
 #include <gsl/gsl_complex_math.h>
 #include <gsl/gsl_math.h>
 #include <gsl/gsl_eigen.h>
+#include <gsl/gsl_linalg.h>
+#include <gsl/gsl_cblas.h>
+#include <gsl/gsl_blas.h>
 
 using namespace std;
 
@@ -44,6 +47,8 @@ private:
   gsl_vector_complex **measurements_disco;
   
   static void cdiag (gsl_vector *v, gsl_matrix_complex *m);
+
+  static void invertcm (gsl_matrix_complex *invm, const gsl_matrix_complex *m);
 };
 
 obs_diagcorr::obs_diagcorr(o815 *_O815) : o815::obs("diagcorr", 
@@ -92,23 +97,44 @@ void obs_diagcorr::_meas(bool loadedobs, const int& nthmeas) {
   }
 };
 
+void obs_diagcorr::invertcm (gsl_matrix_complex *invm, const gsl_matrix_complex *m)
+{
+  int s;
+  gsl_permutation * perm = gsl_permutation_alloc (m->size1);
+  
+  gsl_matrix_complex *mtmp = gsl_matrix_complex_alloc(m->size1,m->size2);
+  gsl_matrix_complex_memcpy(mtmp, m);
+
+  gsl_linalg_complex_LU_decomp (mtmp, perm, &s);
+  gsl_linalg_complex_LU_invert (mtmp, perm, invm);
+
+  gsl_matrix_complex_free(mtmp);
+}
+
 void obs_diagcorr::cdiag (gsl_vector *v, gsl_matrix_complex *m)
 {
-  gsl_eigen_herm_workspace *wspace = gsl_eigen_herm_alloc(100);
-  gsl_eigen_herm(m, v, wspace);
-  gsl_eigen_herm_free(wspace);
+  gsl_matrix_complex *evec = gsl_matrix_complex_alloc(4,4);  
+  gsl_eigen_hermv_workspace *wspace = gsl_eigen_hermv_alloc(100);
+
+  gsl_eigen_hermv(m, v, evec, wspace);
+  gsl_eigen_hermv_sort(v, evec, GSL_EIGEN_SORT_VAL_ASC);
+  
+  gsl_eigen_hermv_free(wspace);
+  gsl_matrix_complex_free(evec);
 }
 
 void obs_diagcorr::_finish() {
   gsl_matrix_complex *totalval = gsl_matrix_complex_alloc(4,4);
   gsl_matrix_complex *tmpmatrix = gsl_matrix_complex_alloc(4,4);
+  gsl_matrix_complex *tmpmatrix2 = gsl_matrix_complex_alloc(4,4);
   gsl_vector *tmpvec = gsl_vector_alloc(4);
   gsl_vector_complex *tmpvecc = gsl_vector_complex_alloc(4);
   gsl_vector *tmpvec2 = gsl_vector_alloc(4);
   gsl_vector *jackres = gsl_vector_alloc(4);
   gsl_vector *jackerrornorm = gsl_vector_alloc(4);
   gsl_vector_complex *totaldisco = gsl_vector_complex_alloc(4);
-
+  gsl_matrix_complex *invC0 = NULL;
+  
   gsl_vector_complex_set_zero(totaldisco);
   for (int imeas=0; imeas<O815->comargs.nmeas; imeas++)
     gsl_vector_complex_add( totaldisco, measurements_disco[imeas] );
@@ -134,7 +160,15 @@ void obs_diagcorr::_finish() {
 						 )
 				);
       }
-    cdiag(jackres, tmpmatrix);
+
+    if (invC0 == NULL) {
+      invC0 = gsl_matrix_complex_alloc(4,4);
+      invertcm(invC0, tmpmatrix); 
+    }
+
+    gsl_blas_zgemm (CblasNoTrans, CblasNoTrans, gsl_complex_rect(1.0,0.0),
+		    invC0, tmpmatrix, gsl_complex_rect(0.0,0.0), tmpmatrix2);
+    cdiag(jackres, tmpmatrix2);
 
     for (int imeas=0; imeas<O815->comargs.nmeas; imeas++) {
       gsl_matrix_complex_memcpy (tmpmatrix, totalval);
@@ -154,7 +188,11 @@ void obs_diagcorr::_finish() {
 						   )
 				  );
 	}
-      cdiag(tmpvec, tmpmatrix);
+
+      gsl_blas_zgemm (CblasNoTrans, CblasNoTrans, gsl_complex_rect(1.0,0.0),
+		      invC0, tmpmatrix, gsl_complex_rect(0.0,0.0), tmpmatrix2);
+      
+      cdiag(tmpvec, tmpmatrix2);
       gsl_vector_sub(tmpvec, jackres);
       gsl_vector_memcpy(tmpvec2, tmpvec);
       gsl_vector_mul(tmpvec, tmpvec2);
-- 
2.39.5