Changeset 4f1369

Timestamp:

Apr 23, 2008, 1:45:37 PM (17 years ago)

Author:

Frederik Heber <heber@…>

Children:

77f1ae

Parents:

9bdd86

Message:

Wannier-file split up: ComputeMWLF() into smaller functions

This split up was done in the SVN repository and tested there and is working. It was impossible to piece it up into smaller chunks in order to maintain "bisectability", hence the last wannier.* from the SVN rep. was used. However, there was one bug in ParseWannierFile: There is the define "type" and there was a enum variable also of name type which caused errors on reading perturbed densities in the resulting shielding values. Removing the variable completely fixed this.
Note: With regards to the shielding values of c2h4 at 24Ht the resulting values are still absolutely the same as with revision initial_commit

Location:

pcp/src

Files:

• wannier.c (modified) (16 diffs)
• wannier.h (modified) (1 diff)

pcp/src/wannier.c

@@ -12 +12 @@
 
   File: wannier.c
-  $Id: wannier.c,v 1.63 2007-02-13 14:15:29 foo Exp $
+  $Id: wannier.c,v 1.7 2007-10-12 15:50:38 heber Exp $
 */
 
@@ -30 +30 @@
 #include "density.h"
 #include "errors.h"
+#include "gramsch.h"
 #include "helpers.h"
 #include "init.h"
@@ -35 +36 @@
 #include "mymath.h"
 #include "output.h"
+#include "perturbed.h"
 #include "wannier.h"
 
 
 #define max_operators NDIM*2 //!< number of chosen self-adjoint operators when evaluating the spread
+#define type Occupied
 
 
@@ -57 +60 @@
  * \note taken from [Golub, Matrix computations, 1989, p451]
  */
-void music(int *top, int *bot, int m)
+void MerryGoRoundIndices(int *top, int *bot, int m)
 {
   int *old_top, *old_bot;
@@ -96 +99 @@
   fprintf(stderr,"\n");*/
   // and finito
-  Free(old_top, "bla");
-  Free(old_bot, "bla");
+  Free(old_top, "MerryGoRoundIndices: old_top");
+  Free(old_bot, "MerryGoRoundIndices: old_bot");
 }
 
@@ -114 +117 @@
  * \param tagR1 MPI tag for receiving right column
  */ 
-void shiftcolumns(MPI_Comm comm, double **Aloc, int Num, int max_rounds, int k, int tagS0, int tagS1, int tagR0, int tagR1) {
+void MerryGoRoundColumns(MPI_Comm comm, double **Aloc, int Num, int max_rounds, int k, int tagS0, int tagS1, int tagR0, int tagR1) {
   //double *A_locS1, *A_locS2;  // local columns of A[k]
   //double *A_locR1, *A_locR2;  // local columns of A[k]
@@ -213 +216 @@
     MPI_Wait(&requestR1, &status);
   //fprintf(stderr,"...done\n");
+}
+
+/** By testing of greatest common divisor of the matrix rows (\a AllocNum) finds suitable parallel cpu group.
+ * \param *P Problem at hand
+ * \param AllocNum number of rows in matrix
+ * \return address of MPI communicator
+ */
+#ifdef HAVE_INLINE
+inline MPI_Comm * DetermineParallelGroupbyGCD (struct Problem *P, int AllocNum)
+#else
+MPI_Comm * DetermineParallelGroupbyGCD (struct Problem *P, int AllocNum)
+#endif
+{
+  MPI_Comm *comm = &P->Par.comm_ST;
+  
+  //if (P->Call.out[ReadOut]) fprintf(stderr,"(%i) Comparing groups - AllocNum %i --- All %i\t Psi %i\t PsiT %i\n",P->Par.me, AllocNum, P->Par.Max_me_comm_ST, P->Par.Max_me_comm_ST_Psi, P->Par.Max_my_color_comm_ST_Psi);
+  //if (P->Call.out[ReadOut]) fprintf(stderr,"(%i) Jacobi diagonalization is done parallely by ", P->Par.me);
+  if (AllocNum % (P->Par.Max_me_comm_ST*2) == 0) {  // all parallel
+    comm = &P->Par.comm_ST;
+    //if (P->Call.out[ReadOut]) fprintf(stderr,"all\n");           
+  } else if (P->Par.Max_me_comm_ST_Psi >= P->Par.Max_my_color_comm_ST_Psi) { // always the bigger group comes first  
+    if (AllocNum % (P->Par.Max_me_comm_ST_Psi*2) == 0) {  // coefficients parallel
+      comm = &P->Par.comm_ST_Psi;
+      //if (P->Call.out[ReadOut]) fprintf(stderr,"Psi\n");       
+    } else if (AllocNum % (P->Par.Max_my_color_comm_ST_Psi*2) == 0) {  // Psis parallel
+      comm = &P->Par.comm_ST_PsiT;
+      //if (P->Call.out[ReadOut]) fprintf(stderr,"PsiT\n");       
+    }
+  } else {
+    if (AllocNum % (P->Par.Max_my_color_comm_ST_Psi*2) == 0) {  // Psis parallel 
+      comm = &P->Par.comm_ST_PsiT;
+      //if (P->Call.out[ReadOut]) fprintf(stderr,"PsiT\n");       
+    } else if (AllocNum % (P->Par.Max_me_comm_ST_Psi*2) == 0) {  // coefficients parallel
+      comm = &P->Par.comm_ST_Psi;
+      //if (P->Call.out[ReadOut]) fprintf(stderr,"Psi\n");       
+    }
+  }
+  return comm;
+}
+
+/** Allocates and fills Lookup table for sin/cos values at each grid node.
+ * \param ***cos_table pointer to two-dimensional lookup table for cosine values
+ * \param ***sin_table pointer to two-dimensional lookup table for sine values
+ * \param *N array with number of nodes per \a NDIM axis
+ */
+void CreateSinCosLookupTable(double ***cos_table, double ***sin_table, int *N)
+{
+  int i, j;
+  double argument;
+  double **cos_lookup, **sin_lookup;
+  
+  // create lookup table for sin/cos values
+  cos_lookup = (double **) Malloc(sizeof(double *)*NDIM, "ComputeMLWF: *cos_lookup");
+  sin_lookup = (double **) Malloc(sizeof(double *)*NDIM, "ComputeMLWF: *sin_lookup");  
+  for (i=0;i<NDIM;i++) {
+    // allocate memory
+    cos_lookup[i] = (double *) Malloc(sizeof(double)*N[i], "ComputeMLWF: cos_lookup");
+    sin_lookup[i] = (double *) Malloc(sizeof(double)*N[i], "ComputeMLWF: sin_lookup");
+    
+    // reset arrays
+    SetArrayToDouble0(cos_lookup[i],N[i]);
+    SetArrayToDouble0(sin_lookup[i],N[i]);
+    
+    // create lookup values
+    for (j=0;j<N[i];j++) {
+      argument = 2*PI/(double)N[i]*(double)j; 
+      cos_lookup[i][j] = cos(argument);
+      sin_lookup[i][j] = sin(argument);
+    }
+  }
+  *cos_table = cos_lookup;
+  *sin_table = sin_lookup;
+}
+
+/** Frees memory allocated during CreateSinCosLookupTable().
+ * \param ***cos_lookup pointer to two-dimensional lookup table for cosine values
+ * \param ***sin_lookup pointer to two-dimensional lookup table for sine values
+ */
+void FreeSinCosLookupTable(double **cos_lookup, double **sin_lookup)
+{
+  int i;
+  // free lookups
+  for (i=0;i<NDIM;i++) {
+    Free(cos_lookup[i], "FreeSinCosLookupTable: cos_lookup[i]");
+    Free(sin_lookup[i], "FreeSinCosLookupTable: sin_lookup[i]");
+  }
+  Free(cos_lookup, "FreeSinCosLookupTable: cos_lookup");
+  Free(sin_lookup, "FreeSinCosLookupTable: sin_lookup");
+}
+
+/** Fills the entries of the six variance matrices.
+ * These matrices are parallely diagonalized during Wannier Localization. They are calculated from the
+ * wave function and by diagonalization one obtains the unitary transformation with which the Psis are
+ * treated afterwards.
+ * \param *P Problem at hand
+ * \param AllocNum number of rows/columns
+ * \param **A pointer to variance matrices
+ * \sa ComputeMLWF() - master function. 
+ */
+void FillHigherOrderRealMomentsMatrices(struct Problem *P, int AllocNum, gsl_matrix **A)
+{
+  struct Lattice *Lat = &P->Lat;
+  struct RunStruct *R = &P->R;
+  struct Psis *Psi = &Lat->Psi;
+  struct LatticeLevel *Lev0 = R->Lev0;
+  struct LatticeLevel *LevS = R->LevS;
+  struct Density *Dens0 = Lev0->Dens;
+  struct OnePsiElement *OnePsiA, *OnePsiB, *LOnePsiB;
+  struct fft_plan_3d *plan = Lat->plan; 
+  fftw_complex *PsiC = Dens0->DensityCArray[ActualPsiDensity];
+  fftw_real *PsiCR = (fftw_real *)PsiC;
+  fftw_complex *work = Dens0->DensityCArray[Temp2Density]; 
+  fftw_real **HGcR = &Dens0->DensityArray[HGDensity];  // use HGDensity, 4x Gap..Density, TempDensity as a storage array
+  fftw_complex **HGcRC = (fftw_complex**)HGcR;
+  fftw_complex **HGcR2C = &Dens0->DensityCArray[HGcDensity];  // use HGcDensity, 4x Gap..Density, TempDensity as an array
+  fftw_real **HGcR2 = (fftw_real**)HGcR2C;
+  int ElementSize = (sizeof(fftw_complex) / sizeof(double)), RecvSource;
+  MPI_Status status;
+  fftw_complex *LPsiDatA=NULL, *LPsiDatB=NULL;
+  int n[NDIM],n0,i0,iS, Index;
+  int Num = Psi->NoOfPsis;  // is number of occupied plus unoccupied states for rows
+  int N0 = LevS->Plan0.plan->local_nx;
+  int *N = LevS->Plan0.plan->N;
+  const int NUpx = LevS->NUp[0];
+  const int NUpy = LevS->NUp[1];
+  const int NUpz = LevS->NUp[2];
+  double argument, PsiAtNode;
+  int e,g,i,j,k,l,m,p,u;
+  double a_ij = 0, b_ij = 0, A_ij = 0, B_ij = 0;
+  double **cos_lookup =  NULL,**sin_lookup = NULL; 
+
+  if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 2\n",P->Par.me);
+
+  debug(P, "Creating Lookup Table");
+  CreateSinCosLookupTable(&cos_lookup, &sin_lookup, N);
+
+  debug(P, "Calculating each entry of variance matrices");
+  l=-1; // to access U matrix element (0..Num-1)
+  // fill the matrices
+  for (i=0; i < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; i++) {  // go through all wave functions
+    OnePsiA = &Psi->AllPsiStatus[i];    // grab OnePsiA
+    if (OnePsiA->PsiType == type) {   // drop all but occupied ones
+      l++;  // increase l if it is non-extra wave function
+      if (OnePsiA->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // local?
+        LPsiDatA=LevS->LPsi->LocalPsi[OnePsiA->MyLocalNo];
+      else
+        LPsiDatA = NULL;  // otherwise processes won't enter second loop, though they're supposed to send coefficients!
+      
+      //fprintf(stderr,"(%i),(%i,%i): fft'd, A[..] and B, back-fft'd acting on \\phi_A\n",P->Par.me,l,0);
+      if (LPsiDatA != NULL) {
+        CalculateOneDensityR(Lat, LevS, Dens0, LPsiDatA, Dens0->DensityArray[ActualDensity], R->FactorDensityR, 1);
+        // note: factor is not used when storing result in DensityCArray[ActualPsiDensity] in CalculateOneDensityR()!
+        for (n0=0;n0<N0;n0++)  
+          for (n[1]=0;n[1]<N[1];n[1]++) 
+            for (n[2]=0;n[2]<N[2];n[2]++) {
+              i0 = n[2]*NUpz+N[2]*NUpz*(n[1]*NUpy+N[1]*NUpy*n0*NUpx);
+              iS = n[2]+N[2]*(n[1]+N[1]*n0);
+              n[0] = n0 + LevS->Plan0.plan->start_nx;
+              for (k=0;k<max_operators;k+=2) {
+                e = k/2;
+                argument = 2.*PI/(double)(N[e])*(double)(n[e]);
+                PsiAtNode = PsiCR[i0] /LevS->MaxN;
+                // check lookup
+                if (!l) // perform check on first wave function only
+                  if ((fabs(cos(argument) - cos_lookup[e][n[e]]) > MYEPSILON) || (fabs(sin(argument) - sin_lookup[e][n[e]]) > MYEPSILON)) {
+                    Error(SomeError, "Lookup table does not match real value!");
+                  }
+                HGcR[k][iS] = cos_lookup[e][n[e]] * PsiAtNode; /* Matrix Vector Mult */
+                HGcR2[k][iS] = cos_lookup[e][n[e]] * HGcR[k][iS]; /* Matrix Vector Mult */
+                HGcR[k+1][iS] = sin_lookup[e][n[e]] * PsiAtNode; /* Matrix Vector Mult */
+                HGcR2[k+1][iS] = sin_lookup[e][n[e]] * HGcR[k+1][iS]; /* Matrix Vector Mult */
+              }
+            }
+        for (u=0;u<max_operators;u++) {
+          fft_3d_real_to_complex(plan, LevS->LevelNo, FFTNF1, HGcRC[u], work);
+          fft_3d_real_to_complex(plan, LevS->LevelNo, FFTNF1, HGcR2C[u], work);
+        }
+      }  
+      m = -1; // to access U matrix element (0..Num-1)
+      for (j=0; j < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; j++) {  // go through all wave functions
+        OnePsiB = &Psi->AllPsiStatus[j];    // grab OnePsiB
+        if (OnePsiB->PsiType == type) {   // drop all but occupied ones
+          m++;  // increase m if it is non-extra wave function
+          if (OnePsiB->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // local?
+             LOnePsiB = &Psi->LocalPsiStatus[OnePsiB->MyLocalNo];
+          else 
+             LOnePsiB = NULL;
+          if (LOnePsiB == NULL) {   // if it's not local ... receive it from respective process into TempPsi
+            RecvSource = OnePsiB->my_color_comm_ST_Psi;
+            MPI_Recv( LevS->LPsi->TempPsi, LevS->MaxG*ElementSize, MPI_DOUBLE, RecvSource, WannierTag2, P->Par.comm_ST_PsiT, &status );
+            LPsiDatB=LevS->LPsi->TempPsi;
+          } else {                  // .. otherwise send it to all other processes (Max_me... - 1)
+            for (p=0;p<P->Par.Max_me_comm_ST_PsiT;p++)
+              if (p != OnePsiB->my_color_comm_ST_Psi) 
+                MPI_Send( LevS->LPsi->LocalPsi[OnePsiB->MyLocalNo], LevS->MaxG*ElementSize, MPI_DOUBLE, p, WannierTag2, P->Par.comm_ST_PsiT);
+            LPsiDatB=LevS->LPsi->LocalPsi[OnePsiB->MyLocalNo];
+          } // LPsiDatB is now set to the coefficients of OnePsi either stored or MPI_Received
+
+          for (u=0;u<max_operators;u++) {
+            a_ij = 0;
+            b_ij = 0;
+            if (LPsiDatA != NULL) { // calculate, reduce and send to all ...
+              //fprintf(stderr,"(%i),(%i,%i): A[%i]: multiplying with \\phi_B\n",P->Par.me, l,m,u);
+              g=0;
+              if (LevS->GArray[0].GSq == 0.0) {
+                Index = LevS->GArray[g].Index;
+                a_ij = (LPsiDatB[0].re*HGcRC[u][Index].re + LPsiDatB[0].im*HGcRC[u][Index].im);
+                b_ij = (LPsiDatB[0].re*HGcR2C[u][Index].re + LPsiDatB[0].im*HGcR2C[u][Index].im);
+                g++;
+              }
+              for (; g < LevS->MaxG; g++) {
+                Index = LevS->GArray[g].Index;
+                a_ij += 2*(LPsiDatB[g].re*HGcRC[u][Index].re + LPsiDatB[g].im*HGcRC[u][Index].im);
+                b_ij += 2*(LPsiDatB[g].re*HGcR2C[u][Index].re + LPsiDatB[g].im*HGcR2C[u][Index].im);
+              } // due to the symmetry the resulting matrix element is real and symmetric in (i,j) ! (complex multiplication simplifies ...)
+              // sum up elements from all coefficients sharing processes
+              MPI_Allreduce ( &a_ij, &A_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_Psi);
+              MPI_Allreduce ( &b_ij, &B_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_Psi);
+              a_ij = A_ij;
+              b_ij = B_ij;
+              // send element to all Psi-sharing who don't have l local (MPI_Send is a lot slower than AllReduce!)
+              MPI_Allreduce ( &a_ij, &A_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_PsiT);
+              MPI_Allreduce ( &b_ij, &B_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_PsiT);
+            } else { // receive ...
+              MPI_Allreduce ( &a_ij, &A_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_PsiT);
+              MPI_Allreduce ( &b_ij, &B_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_PsiT);              
+            }
+            // ... and store
+            //fprintf(stderr,"(%i),(%i,%i): A[%i]: setting component (local: %lg, total: %lg)\n",P->Par.me, l,m,u,a_ij,A_ij);
+            //fprintf(stderr,"(%i),(%i,%i): B: adding upon component (local: %lg, total: %lg)\n",P->Par.me, l,m,b_ij,B_ij);
+            gsl_matrix_set(A[u], l, m, A_ij);
+            gsl_matrix_set(A[max_operators], l, m, B_ij + gsl_matrix_get(A[max_operators],l,m));
+          }
+        }
+      }
+    }
+  }
+  // reset extra entries
+  for (u=0;u<=max_operators;u++) {
+    for (i=Num;i<AllocNum;i++)
+      for (j=0;j<AllocNum;j++)
+        gsl_matrix_set(A[u], i,j, 0.);
+    for (i=Num;i<AllocNum;i++)
+      for (j=0;j<AllocNum;j++)
+        gsl_matrix_set(A[u], j,i, 0.);
+  }
+  FreeSinCosLookupTable(cos_lookup, sin_lookup);
+
+/*// print A matrices for debug
+  if (P->Par.me == 0) 
+    for (u=0;u<max_operators+1;u++) {
+     fprintf(stderr, "A[%i] = \n",u);
+      for (i=0;i<Num;i++) {
+        for (j=0;j<Num;j++)
+          fprintf(stderr, "%e\t",gsl_matrix_get(A[u],i,j));
+        fprintf(stderr, "\n");
+      }
+    }  
+*/
+}
+
+/** Calculates reciprocal second order moments of each PsiType#Occupied orbital.
+ * First order is zero due to wave function being real (symmetry condition).
+ * \param *P Problem at hand
+ */
+void CalculateSecondOrderReciprocalMoment(struct Problem *P)
+{
+  struct Lattice *Lat = &P->Lat;
+  struct RunStruct *R = &P->R;
+  struct FileData *F = &P->Files;
+  struct Psis *Psi = &Lat->Psi;
+  struct LatticeLevel *LevS = R->LevS;
+  double result, Result; 
+  fftw_complex *LPsiDatA=NULL;
+  struct OnePsiElement *OnePsiA;
+  int i,j,l,g;
+  char spin[12], suffix[18];
+
+  switch (Lat->Psi.PsiST) {
+  case SpinDouble:
+    strcpy(suffix,".recispread.csv");    
+    strcpy(spin,"SpinDouble");
+    break;
+  case SpinUp:
+    strcpy(suffix,".recispread_up.csv");    
+    strcpy(spin,"SpinUp");
+    break;
+  case SpinDown:
+    strcpy(suffix,".recispread_down.csv");    
+    strcpy(spin,"SpinDown");
+    break;
+  }
+  if(P->Par.me_comm_ST == 0) {
+    if (P->Call.out[NormalOut]) fprintf(stderr,"(%i) Calculating reciprocal moments ...\n",P->Par.me);
+    if (R->LevSNo == Lat->MaxLevel-1) // open freshly if first level 
+      OpenFile(P, &F->ReciSpreadFile, suffix, "w", P->Call.out[ReadOut]); // only open on starting level
+    else if (F->ReciSpreadFile == NULL) // re-op,18en if not first level and not opened yet (or closed from ParseWannierFile)
+      OpenFile(P, &F->ReciSpreadFile, suffix, "a", P->Call.out[ReadOut]); // only open on starting level
+    if (F->ReciSpreadFile == NULL) {
+      Error(SomeError,"ComputeMLWF: Error opening Reciprocal spread File!\n");
+    } else {   
+      fprintf(F->ReciSpreadFile,"===== Reciprocal Spreads of type %s ==========================================================================\n", spin);
+    }
+  }
+
+  // integrate second order moment
+  for (l=0; l < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; l++) {  // go through all wave functions
+    OnePsiA = &Psi->AllPsiStatus[l];    // grab OnePsiA
+    if (OnePsiA->PsiType == type) {   // drop all but occupied ones
+      if (OnePsiA->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // local?
+        LPsiDatA=LevS->LPsi->LocalPsi[OnePsiA->MyLocalNo];
+      else
+        LPsiDatA = NULL;
+      
+      if (LPsiDatA != NULL) {
+        if (P->Par.me_comm_ST == 0) 
+          fprintf(F->ReciSpreadFile,"Psi%d_Lev%d\t", Psi->AllPsiStatus[l].MyGlobalNo, R->LevSNo); 
+        for (i=0;i<NDIM;i++) {
+          for (j=0;j<NDIM;j++) {
+            result = 0.;
+            g = 0;
+            if (LevS->GArray[0].GSq == 0.0) {
+              result += LevS->GArray[g].G[i]*LevS->GArray[g].G[j]*(LPsiDatA[0].re * LPsiDatA[0].re);
+              g++;
+            }
+            for (;g<LevS->MaxG;g++)
+              result += LevS->GArray[g].G[i]*LevS->GArray[g].G[j]*2.*(LPsiDatA[g].re * LPsiDatA[g].re + LPsiDatA[g].im * LPsiDatA[g].im);
+            //result *= Lat->Volume/LevS->MaxG;
+            MPI_Allreduce ( &result, &Result, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_Psi);
+            if (P->Par.me_comm_ST == 0) 
+              fprintf(F->ReciSpreadFile,"%2.5lg  ", Result);
+          }
+        }
+        if (P->Par.me_comm_ST == 0) 
+          fprintf(F->ReciSpreadFile,"\n");
+      }
+    }
+  }
+  if(P->Par.me_comm_ST == 0) { 
+    fprintf(F->ReciSpreadFile,"====================================================================================================================\n\n");
+    fflush(F->ReciSpreadFile);
+  }
+}
+
+/** Given the unitary matrix the transformation is performed on the Psis.
+ * \param *P Problem at hand
+ * \param *U gsl matrix containing the transformation matrix
+ * \param Num dimension parameter for the matrix, i.e. number of wave functions
+ */
+void UnitaryTransformationOnWavefunctions(struct Problem *P, gsl_matrix *U, int Num)
+{
+  struct Lattice *Lat = &P->Lat;
+  struct RunStruct *R = &P->R;
+  struct Psis *Psi = &Lat->Psi;
+  struct LatticeLevel *LevS = R->LevS;
+  MPI_Status status;
+  struct OnePsiElement *OnePsiB, *OnePsiA, *LOnePsiB;
+  int ElementSize = (sizeof(fftw_complex) / sizeof(double)), RecvSource;
+  fftw_complex *LPsiDatA=NULL, *LPsiDatB=NULL;
+  int g,i,j,l,k,m,p;
+
+  //if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 6: Transformation of all wave functions according to U\n",P->Par.me); 
+  
+  Num = Psi->TypeStartIndex[type+1] - Psi->TypeStartIndex[type]; // recalc Num as we can only work with local Psis from here
+  fftw_complex **coeffs_buffer = Malloc(sizeof(fftw_complex *)*Num, "ComputeMLWF: **coeffs_buffer");
+
+  for (l=0;l<Num;l++) // allocate for each local wave function
+    coeffs_buffer[l] = LevS->LPsi->OldLocalPsi[l];
+  
+  //if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 6: Transformation ...\n",P->Par.me);
+  l=-1; // to access U matrix element (0..Num-1)
+  k=-1; // to access the above swap coeffs_buffer (0..LocalNo-1)
+  for (i=0; i < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; i++) {  // go through all wave functions
+    OnePsiA = &Psi->AllPsiStatus[i];    // grab OnePsiA
+    if (OnePsiA->PsiType == type) {   // drop all but occupied ones
+      l++;  // increase l if it is occupied wave function
+      if (OnePsiA->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) { // local?
+        k++; // increase k only if it is a local, non-extra orbital wave function
+        LPsiDatA = (fftw_complex *) coeffs_buffer[k];    // new coeffs first go to copy buffer, old ones must not be overwritten yet
+        SetArrayToDouble0((double *)LPsiDatA, 2*LevS->MaxG);  // zero buffer part
+      } else
+        LPsiDatA = NULL;  // otherwise processes won't enter second loop, though they're supposed to send coefficients!
+
+      m = -1; // to access U matrix element (0..Num-1)
+      for (j=0; j < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; j++) {  // go through all wave functions
+        OnePsiB = &Psi->AllPsiStatus[j];    // grab OnePsiB
+        if (OnePsiB->PsiType == type) {   // drop all but occupied ones
+          m++;  // increase m if it is occupied wave function
+          if (OnePsiB->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // local?
+             LOnePsiB = &Psi->LocalPsiStatus[OnePsiB->MyLocalNo];
+          else 
+             LOnePsiB = NULL;
+          if (LOnePsiB == NULL) {   // if it's not local ... receive it from respective process into TempPsi
+            RecvSource = OnePsiB->my_color_comm_ST_Psi;
+            MPI_Recv( LevS->LPsi->TempPsi, LevS->MaxG*ElementSize, MPI_DOUBLE, RecvSource, WannierTag2, P->Par.comm_ST_PsiT, &status );
+            LPsiDatB=LevS->LPsi->TempPsi;
+          } else {                  // .. otherwise send it to all other processes (Max_me... - 1)
+            for (p=0;p<P->Par.Max_me_comm_ST_PsiT;p++)
+              if (p != OnePsiB->my_color_comm_ST_Psi) 
+                MPI_Send( LevS->LPsi->LocalPsi[OnePsiB->MyLocalNo], LevS->MaxG*ElementSize, MPI_DOUBLE, p, WannierTag2, P->Par.comm_ST_PsiT);
+            LPsiDatB=LevS->LPsi->LocalPsi[OnePsiB->MyLocalNo];
+          } // LPsiDatB is now set to the coefficients of OnePsi either stored or MPI_Received
+
+          if (LPsiDatA != NULL) {
+            double tmp = gsl_matrix_get(U,l,m);
+            g=0;
+            if (LevS->GArray[0].GSq == 0.0) {
+              LPsiDatA[g].re += LPsiDatB[g].re * tmp;
+              LPsiDatA[g].im += LPsiDatB[g].im * tmp;
+              g++;
+            }
+            for (; g < LevS->MaxG; g++) {
+              LPsiDatA[g].re += LPsiDatB[g].re * tmp;
+              LPsiDatA[g].im += LPsiDatB[g].im * tmp;
+            }
+          }
+        }
+      }
+    }
+  }
+
+  //if(P->Call.out[StepLeaderOut]) fprintf(stderr,"(%i) STEP 6: Swapping buffer mem\n",P->Par.me);
+  // now, as all wave functions are updated, swap the buffer
+  l = -1;
+  for (k=0;k<Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT;k++) { // go through each local occupied wave function
+    if (Psi->AllPsiStatus[k].PsiType == type && Psi->AllPsiStatus[k].my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) {
+      l++;
+      //if(P->Call.out[StepLeaderOut]) fprintf(stderr,"(%i) (k:%i,l:%i) LocalNo = (%i,%i)\t AllPsiNo = (%i,%i)\n", P->Par.me, k,l,Psi->LocalPsiStatus[l].MyLocalNo, Psi->LocalPsiStatus[l].MyGlobalNo, Psi->AllPsiStatus[k].MyLocalNo, Psi->AllPsiStatus[k].MyGlobalNo);
+      LPsiDatA = (fftw_complex *)coeffs_buffer[l];
+      LPsiDatB = LevS->LPsi->LocalPsi[l];
+      for (g=0;g<LevS->MaxG;g++) {
+        LPsiDatB[g].re = LPsiDatA[g].re;
+        LPsiDatB[g].im = LPsiDatA[g].im;
+      }
+      // recalculating non-local form factors which are coefficient dependent!
+      CalculateNonLocalEnergyNoRT(P, Psi->LocalPsiStatus[l].MyLocalNo);
+    }
+  } 
+  // and free allocated buffer memory
+  Free(coeffs_buffer, "UnitaryTransformationOnWavefunctions: coeffs_buffer");
+}
+
+/** Changes Wannier Centres according to RunStruct#CommonWannier.
+ * \param *P Problem at hand.
+ * \param Num number of Psis
+ * \param **WannierCentre 2D array (NDIM, \a Num) with wannier centres
+ * \param *WannierSpread array with wannier spread per wave function
+ */
+void ChangeWannierCentres(struct Problem *P, int Num, double **WannierCentre, double *WannierSpread)
+{
+  struct RunStruct *R = &P->R;
+  struct Lattice *Lat = &P->Lat;
+  struct LatticeLevel *LevS = R->LevS;
+  int *marker, **group;
+  int partner[Num];
+  int i,j,l,k;
+  int totalflag, flag;
+  double q[NDIM], center[NDIM];
+  double Spread;
+  int *N = LevS->Plan0.plan->N;
+  
+  switch (R->CommonWannier) {
+   case 4:
+    debug(P,"Shifting each Wannier centers to cell center");
+    for (j=0;j<NDIM;j++)  // center point in [0,1]^3
+      center[j] = 0.5;
+    RMat33Vec3(q,Lat->RealBasis, center); // transform to real coordinates
+    for (i=0; i < Num; i++) {  // go through all occupied wave functions
+      for (j=0;j<NDIM;j++)  // put into Wannier centres
+        WannierCentre[i][j] = q[j]; 
+    }    
+    break;
+   case 3:
+    debug(P,"Shifting Wannier centers individually to nearest grid point");
+    for (i=0;i < Num; i++) {  // go through all wave functions
+      RMat33Vec3(q, Lat->ReciBasis, WannierCentre[i]);
+      for (j=0;j<NDIM;j++) {  // Recibasis is not true inverse but times 2.*PI
+        q[j] *= (double)N[j]/(2.*PI);
+        
+        //fprintf(stderr,"(%i) N[%i]: %i\t tmp %e\t floor %e\t ceil %e\n",P->Par.me, j, N[j], tmp, floor(tmp), ceil(tmp));
+        if (fabs((double)floor(q[j]) - q[j]) < fabs((double)ceil(q[j]) - q[j]))
+          q[j] = floor(q[j])/(double)N[j];
+        else
+          q[j] = ceil(q[j])/(double)N[j];
+      }
+      RMat33Vec3(WannierCentre[i], Lat->RealBasis, q);
+    }
+    break;
+   case 2:
+    debug(P,"Combining individual orbitals according to spread.");
+    //fprintf(stderr,"(%i) Finding multiple bindings and Reweighting Wannier centres\n",P->Par.me);
+    debug(P,"finding partners");
+    marker = (int*) Malloc(sizeof(int)*(Num+1),"ComputeMLWF: marker");
+    group = (int**) Malloc(sizeof(int *)*Num,"ComputeMLWF: group");
+    for (l=0;l<Num;l++) {
+      group[l] = (int*) Malloc(sizeof(int)*(Num+1),"ComputeMLWF: group[l]");  // each must group must have one more as end marker
+      for (k=0;k<=Num;k++)
+        group[l][k] = -1; // reset partner group
+    }
+    for (k=0;k<Num;k++)
+      partner[k] = 0;
+    debug(P,"mem allocated");
+    // go for each orbital through every other, check distance against the sum of both spreads
+    // if smaller add to group of this orbital
+    for (l=0;l<Num;l++) {
+      j=0;  // index for partner group
+      for (k=0;k<Num;k++) {  // check this against l
+        Spread = 0.;
+        for (i=0;i<NDIM;i++) {
+          //fprintf(stderr,"(%i) Spread += (%e - %e)^2 \n", P->Par.me, WannierCentre[l][i], WannierCentre[k][i]); 
+          Spread += (WannierCentre[l][i] - WannierCentre[k][i])*(WannierCentre[l][i] - WannierCentre[k][i]);
+        }
+        Spread = sqrt(Spread);  // distance in Spread
+        //fprintf(stderr,"(%i) %i to %i: distance %e, SpreadSum = %e + %e = %e \n", P->Par.me, l, k, Spread, WannierSpread[l], WannierSpread[k], WannierSpread[l]+WannierSpread[k]); 
+        if (Spread < 1.5*(WannierSpread[l]+WannierSpread[k])) {// if distance smaller than sum of spread
+          group[l][j++] = k;  // add k to group of l
+          partner[l]++;
+          //fprintf(stderr,"(%i) %i added as %i-th member to %i's group.\n", P->Par.me, k, j, l);
+        } 
+      }
+    }
+    
+    // consistency, for each orbital check if this orbital is also in the group of each referred orbital
+    debug(P,"checking consistency");
+    totalflag = 1;
+    for (l=0;l<Num;l++) // checking l's group
+      for (k=0;k<Num;k++) { // k is partner index
+        if (group[l][k] != -1) {  // if current index k is a partner
+          flag = 0;
+          for(j=0;j<Num;j++) {  // go through each entry in l partner's partner group if l exists
+            if ((group[ group[l][k] ][j] == l))
+              flag = 1;
+          }
+          //if (flag == 0) fprintf(stderr, "(%i) in %i's group %i is referred as a partner, but not the other way round!\n", P->Par.me, l, group[l][k]);
+          if (totalflag == 1) totalflag = flag;
+        }
+      }
+    // for each orbital group (marker group) weight each center to a total and put this into the local WannierCentres
+    debug(P,"weight and calculate new centers for partner groups");
+    for (l=0;l<=Num;l++)
+      marker[l] = 1;
+    if (totalflag) {
+      for (l=0;l<Num;l++) { // go through each orbital
+        if (marker[l] != 0) { // if it hasn't been reweighted
+          marker[l] = 0;
+          for (i=0;i<NDIM;i++) 
+            q[i] = 0.;
+          j = 0;
+          while (group[l][j] != -1) {
+            marker[group[l][j]] = 0;
+            for (i=0;i<NDIM;i++) {
+              //fprintf(stderr,"(%i) Adding to %i's group, %i entry of %i: %e\n", P->Par.me, l, i, group[l][j], WannierCentre[ group[l][j] ][i]);
+              q[i] += WannierCentre[ group[l][j] ][i];
+            } 
+            j++;
+          }
+          //fprintf(stderr,"(%i) %i's group: (%e,%e,%e)/%i = (%e,%e,%e)\n", P->Par.me, l, q[0], q[1], q[2], j, q[0]/(double)j, q[1]/(double)j, q[2]/(double)j);
+          for (i=0;i<NDIM;i++) {// weight by number of elements in partner group
+            q[i] /= (double)(j);
+          }
+  
+          // put WannierCentre into own and all partners'      
+          for (i=0;i<NDIM;i++)
+            WannierCentre[l][i] = q[i];
+          j = 0;  
+          while (group[l][j] != -1) {
+            for (i=0;i<NDIM;i++)
+              WannierCentre[group[l][j]][i] = q[i];
+            j++;
+          }
+        }
+      }
+    }
+    if (P->Call.out[StepLeaderOut]) {
+      fprintf(stderr,"Summary:\n");
+      fprintf(stderr,"========\n");
+      for (i=0;i<Num;i++)
+        fprintf(stderr,"%i belongs to a %i-ple binding.\n",i,partner[i]);
+    } 
+    debug(P,"done");
+  
+    Free(marker, "ChangeWannierCentres: marker");
+    for (l=0;l<Num;l++)
+      Free(group[l], "ChangeWannierCentres: group[l]");
+    Free(group, "ChangeWannierCentres: group");
+    break;
+   case 1:
+    debug(P,"Individual orbitals are changed to center of all.");
+    for (i=0;i<NDIM;i++)  // zero center of weight
+      q[i] = 0.;
+    for (k=0;k<Num;k++)
+      for (i=0;i<NDIM;i++) {  // sum up all orbitals each component
+        q[i] += WannierCentre[k][i];
+      }
+    for (i=0;i<NDIM;i++)  // divide by number
+      q[i] /= Num;
+    for (k=0;k<Num;k++)
+      for (i=0;i<NDIM;i++) {  // put into this function's array
+        WannierCentre[k][i] = q[i];
+      }
+    break;
+   case 0:
+   default:
+   break;
+  }
+}
+
+/** From the entries of the variance matrices the spread is calculated.
+ * WannierCentres are evaluated according to Resta operator: \f$\langle X \rangle = \frac{L}{2\pi} \sum_j {\cal I} \ln{\langle \Psi_j | \exp{(i \frac{2\pi}{L} X)} | \Psi_j \rangle}\f$
+ * WannierSpread is: \f$ \sum_j \langle \Psi_j | r^2 | \Psi_j \rangle - \langle \Psi_j | r | psi_j \rangle^2\f$
+ * \param *P Problem at hand
+ * \param *A variance matrices
+ * \param old_spread first term of wannier spread
+ * \param spread second term of wannier spread
+ * \param **WannierCentre 2D array (NDIM, \a Num) with wannier centres
+ * \param *WannierSpread array with wannier spread per wave function
+ */
+void ComputeWannierCentresfromVarianceMatrices(struct Problem *P, gsl_matrix **A, double *spread, double *old_spread, double **WannierCentre, double *WannierSpread)
+{
+  struct Lattice *Lat = &P->Lat;
+  struct Psis *Psi = &Lat->Psi;
+  struct OnePsiElement *OnePsiA;
+  int i,j,k,l;
+  double tmp, q[NDIM];
+  
+  *old_spread = 0;
+  *spread = 0;
+
+  // the spread for x,y,z resides in the respective diagonal element of A_.. for each orbital
+  i=-1;
+  for (l=0; l < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; l++) {  // go through all wave functions
+    OnePsiA = &Psi->AllPsiStatus[l];    // grab OnePsiA
+    if (OnePsiA->PsiType == type) {   // drop all but occupied ones
+      i++;  // increase l if it is occupied wave function
+      //fprintf(stderr,"(%i) Wannier for %i\n", P->Par.me, i);
+      
+      // calculate Wannier Centre
+      for (j=0;j<NDIM;j++) {
+        q[j] = 1./(2.*PI) * GSL_IMAG( gsl_complex_log( gsl_complex_rect(gsl_matrix_get(A[j*2],i,i),gsl_matrix_get(A[j*2+1],i,i))));
+        if (q[j] < 0) // change wrap around of above operator to smooth 0...Lat->RealBasisSQ
+          q[j] += 1.;
+      }
+      RMat33Vec3(WannierCentre[i], Lat->RealBasis, q);
+      
+      // store orbital spread and centre in file
+      tmp = - pow(gsl_matrix_get(A[0],i,i),2) - pow(gsl_matrix_get(A[1],i,i),2)
+            - pow(gsl_matrix_get(A[2],i,i),2) - pow(gsl_matrix_get(A[3],i,i),2)
+            - pow(gsl_matrix_get(A[4],i,i),2) - pow(gsl_matrix_get(A[5],i,i),2);          
+      WannierSpread[i] = gsl_matrix_get(A[max_operators],i,i) + tmp; 
+      //fprintf(stderr,"(%i) WannierSpread[%i] = %e\n", P->Par.me, i, WannierSpread[i]);
+      //if (P->Par.me == 0) fprintf(F->SpreadFile,"Orbital %d:\t Wannier center (x,y,z)=(%lg,%lg,%lg)\t Spread sigma^2 = %lg - %lg = %lg\n",
+        //Psi->AllPsiStatus[i].MyGlobalNo, WannierCentre[i][0], WannierCentre[i][1], WannierCentre[i][2], gsl_matrix_get(A[max_operators],i,i), -tmp, WannierSpread[i]); 
+      //if (P->Par.me == 0) fprintf(F->SpreadFile,"%e\t%e\t%e\n",
+        //WannierCentre[i][0], WannierCentre[i][1], WannierCentre[i][2]);
+      
+      // gather all spreads
+      *old_spread += gsl_matrix_get(A[max_operators],i,i); // tr(U^H B U)
+      for (k=0;k<max_operators;k++)
+        *spread += pow(gsl_matrix_get(A[k],i,i),2);
+    }
+  }
+}
+
+/** Prints gsl_matrix nicely to screen.
+ * \param *P Problem at hand
+ * \param *U gsl matrix
+ * \param Num number of rows/columns
+ * \param *msg name of matrix, prepended before entries
+ */
+void PrintGSLMatrix(struct Problem *P, gsl_matrix *U, int Num, const char *msg)
+{
+  int k,l;
+  fprintf(stderr,"(%i) %s = \n",P->Par.me, msg);
+  for (k=0;k<Num;k++) {    
+    for (l=0;l<Num;l++)
+      fprintf(stderr,"%e\t",gsl_matrix_get(U,l,k));
+    fprintf(stderr,"\n");
+  }
+}
+
+/** Allocates memory for the  DiagonalizationData structure
+ * \param *P Problem at hand
+ * \param DiagData pointer to structure
+ * \param Num number of rows and columns in matrix
+ * \param NumMatrices number of matrices to be simultaneously diagonalized
+ * \param extra number of extra matrices to be passively also diagonalized
+ */
+void InitDiagonalization(struct Problem *P, struct DiagonalizationData *DiagData, int Num, int NumMatrices, int extra)
+{
+  int i;
+  
+  // store integer values
+  //if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 1\n",P->Par.me);
+  DiagData->Num = Num;
+  DiagData->AllocNum = ceil((double)Num / 2. ) *2;
+  DiagData->NumMatrices = NumMatrices;
+  DiagData->extra = extra;
+  
+  // determine communicator and our rank and its size
+  DiagData->comm = DetermineParallelGroupbyGCD(P,DiagData->AllocNum);
+  MPI_Comm_size (*(DiagData->comm), &(DiagData->ProcNum));
+  MPI_Comm_rank (*(DiagData->comm), &(DiagData->ProcRank));
+  
+  // allocate memory
+  DiagData->U = gsl_matrix_calloc(DiagData->AllocNum,DiagData->AllocNum);
+  gsl_matrix_set_identity(DiagData->U);
+  
+  DiagData->A = (gsl_matrix **) Malloc((DiagData->NumMatrices+DiagData->extra) * sizeof(gsl_matrix *), "InitDiagonalization: *A"); 
+  for (i=0;i<(DiagData->NumMatrices+DiagData->extra);i++) {
+    DiagData->A[i] = gsl_matrix_calloc(DiagData->AllocNum,DiagData->AllocNum);
+    gsl_matrix_set_zero(DiagData->A[i]);
+  }
+
+  // init merry-go-round array for diagonilzation
+  DiagData->top = (int *) Malloc(sizeof(int)*DiagData->AllocNum/2, "InitDiagonalization: top");
+  DiagData->bot = (int *) Malloc(sizeof(int)*DiagData->AllocNum/2, "InitDiagonalization: bot");
+  for (i=0;i<DiagData->AllocNum/2;i++) {
+    DiagData->top[i] = 2*i;
+    DiagData->bot[i] = 2*i+1;      
+  }
+/*    // print starting values of index generation tables top and bot
+      fprintf(stderr,"top\t");
+      for (k=0;k<AllocNum/2;k++)
+        fprintf(stderr,"%i\t", top[k]);
+      fprintf(stderr,"\n");
+      fprintf(stderr,"bot\t");
+      for (k=0;k<AllocNum/2;k++)
+        fprintf(stderr,"%i\t", bot[k]);
+      fprintf(stderr,"\n");*/
+}
+
+/** Frees allocated memory for the DiagonalizationData structure.
+ * \param DiagData pointer to structure
+ */
+void FreeDiagonalization(struct DiagonalizationData *DiagData)
+{
+  int i;
+  
+  Free(DiagData->top, "FreeDiagonalization: DiagData->top");
+  Free(DiagData->bot, "FreeDiagonalization: DiagData->bot");
+  gsl_matrix_free(DiagData->U);
+  for (i=0;i<(DiagData->NumMatrices+DiagData->extra);i++)
+    gsl_matrix_free(DiagData->A[i]);   
+  Free(DiagData->A, "FreeDiagonalization: DiagData->A");
+}
+
+/** Orthogonalizing PsiType#Occupied wave functions for MD.
+ * Orthogonalizes wave functions by finding a unitary transformation such that the density remains unchanged.
+ * To this end, the overlap matrix \f$\langle \Psi_i | \Psi_j \rangle\f$ is created and diagonalised by Jacobi
+ * transformation (product of rotation matrices). The found transformation matrix is applied to all Psis.
+ * \param *P Problem at hand
+ * \note [Payne] states that GramSchmidt is not well-suited. However, this Jacobi diagonalisation is not well
+ * suited for our CG minimisation either. If one wave functions is changed in course of the minimsation
+ * procedure, in a Gram-Schmidt-Orthogonalisation only this wave function is changed (although it remains under
+ * debate whether subsequent Psis are still orthogonal to this changed wave function!), in a Jacobi-Orthogonali-
+ * stion however at least two if not all wave functions are changed. Thus inhibits our fast way of updating the
+ * density after a minimisation step -- UpdateDensityCalculation(). Thus, this routine can only be used for a
+ * final orthogonalisation of all Psis, after the CG minimisation.
+ */
+void OrthogonalizePsis(struct Problem *P)
+{
+  struct Lattice *Lat = &P->Lat;
+  struct Psis *Psi = &Lat->Psi;
+  struct LatticeLevel *LevS = P->R.LevS;
+  int Num = Psi->NoOfPsis;
+  int i,j,l;
+  struct DiagonalizationData DiagData;
+  double PsiSP;
+
+  InitDiagonalization(P, &DiagData, Num, 1, 0);
+    
+  // Calculate Overlap matrix
+  for (l=Psi->TypeStartIndex[Occupied]; l<Psi->TypeStartIndex[UnOccupied]; l++)
+    CalculateOverlap(P, l, Occupied);  
+  for (i=0;i<Num;i++) // fill gsl matrix with overlap values
+    for (j=0;j<Num;j++)
+      gsl_matrix_set(DiagData.A[0],i,j,Psi->Overlap[i][j]);
+  //if (P->Call.out[ReadOut]) PrintGSLMatrix(P,DiagData.A[0],Num,"<Psi_i|Psi_J> (before)");
+  
+  //if (P->Call.out[ReadOut]) PrintGSLMatrix(P,DiagData.U,Num,"Transformation (before)");
+
+  // diagonalize overlap matrix
+  Diagonalize(P, &DiagData);
+
+  //if (P->Call.out[ReadOut]) PrintGSLMatrix(P,DiagData.U,Num,"Transformation (after)");
+
+  // apply found unitary transformation to wave functions
+  UnitaryTransformationOnWavefunctions(P, DiagData.U, DiagData.AllocNum);
+
+  // update GramSch stati
+  for (i=Psi->TypeStartIndex[Occupied];i<Psi->TypeStartIndex[UnOccupied];i++) {
+    GramSchNormalize(P,LevS,LevS->LPsi->LocalPsi[i], 0.);   // calculates square product if 0. is given...
+    PsiSP = GramSchGetNorm2(P,LevS,LevS->LPsi->LocalPsi[i]);
+    //if (P->Par.me_comm_ST_Psi == 0) fprintf(stderr,"(%i) PsiSP[%i] = %lg\n", P->Par.me, i, PsiSP);
+    Psi->LocalPsiStatus[i].PsiGramSchStatus = (int)IsOrthonormal;
+  }
+  UpdateGramSchAllPsiStatus(P,Psi);
+/*
+  for (l=Psi->TypeStartIndex[Occupied]; l<Psi->TypeStartIndex[UnOccupied]; l++)
+    CalculateOverlap(P, l, Occupied);  
+  for (i=0;i<Num;i++) // fill gsl matrix with overlap values
+    for (j=0;j<Num;j++)
+      gsl_matrix_set(DiagData.A[0],i,j,Psi->Overlap[i][j]);
+  if (P->Call.out[ReadOut]) PrintGSLMatrix(P,DiagData.A[0],Num,"<Psi_i|Psi_J> (after)");
+  */
+  // free memory
+  FreeDiagonalization(&DiagData);
+}
+
+/** Orthogonalizing PsiType#Occupied wave functions and their time derivatives for MD.
+ * Ensures that two orthonormality condition are met for Psis: 
+ * \f$\langle \Psi_i | \Psi_j \rangle = \delta_{ij}\f$
+ * \f$\langle \dot{\Psi_i} | \dot{\Psi_j} \rangle = \delta_{ij}\f$
+ * \param *P Problem at hand
+ * \note [Payne] states that GramSchmidt is not well-suited, and this stronger
+ *       condition is needed for numerical stability
+ * \todo create and fill 1stoverlap with finite difference between Psi, OldPsi with R->Deltat
+ */
+void StrongOrthogonalizePsis(struct Problem *P)
+{
+  struct Lattice *Lat = &P->Lat;
+  struct Psis *Psi = &Lat->Psi;
+  int Num = Psi->NoOfPsis;
+  int i,j,l;
+  struct DiagonalizationData DiagData;
+
+  InitDiagonalization(P, &DiagData, Num, 2, 0);
+    
+  // Calculate Overlap matrix
+  for (l=Psi->TypeStartIndex[Occupied]; l<Psi->TypeStartIndex[UnOccupied]; l++) {
+    CalculateOverlap(P, l, Occupied);  
+    //Calculate1stOverlap(P, l, Occupied);
+  }  
+  for (i=0;i<Num;i++) // fill gsl matrix with overlap values
+    for (j=0;j<Num;j++) {
+      gsl_matrix_set(DiagData.A[0],i,j,Psi->Overlap[i][j]);
+      //gsl_matrix_set(DiagData.A[1],i,j,Psi->1stOverlap[i][j]);
+    }
+      
+  // diagonalize overlap matrix
+  Diagonalize(P, &DiagData);
+  
+  // apply found unitary transformation to wave functions
+  UnitaryTransformationOnWavefunctions(P, DiagData.U, DiagData.AllocNum);
+  
+  // free memory
+  FreeDiagonalization(&DiagData);
+}
+
+/** Uses either serial or parallel diagonalization.
+ * \param *P Problem at hand
+ * \param *DiagData pointer to structure DiagonalizationData containing necessary information for diagonalization
+ * \sa ParallelDiagonalization(), SerialDiagonalization()
+ */
+void Diagonalize(struct Problem *P, struct DiagonalizationData *DiagData)
+{
+  if (DiagData->Num != 1) { // one- or multi-process case?
+    if (((DiagData->AllocNum % 2) == 0) && (DiagData->ProcNum != 1) && ((DiagData->AllocNum / 2) % DiagData->ProcNum == 0)) {
+      /*
+      debug(P,"Testing with silly matrix");
+      ParallelDiagonalization(P, test, Utest, 1, 0, 4, Num, comm, ProcRank, ProcNum, top, bot);
+      if(P->Call.out[ReadOut]) // && P->Par.me == 0) 
+        PrintGSLMatrix(P, test[0], 4, "test[0] (diagonalized)");
+      if(P->Call.out[ReadOut]) // && P->Par.me == 0) 
+        PrintGSLMatrix(P, Utest, 4, "Utest (final)");
+        */  
+      debug(P,"ParallelDiagonalization");
+      ParallelDiagonalization(P, DiagData);
+    } else {/*
+      debug(P,"Testing with silly matrix");
+      SerialDiagonalization(P, test, Utest, 1, 0, 4, Num, top, bot);
+      if(P->Call.out[ReadOut]) // && P->Par.me == 0) 
+        PrintGSLMatrix(P, test[0], 4, "test[0] (diagonalized)");
+      if(P->Call.out[ReadOut]) // && P->Par.me == 0) 
+        PrintGSLMatrix(P, Utest, 4, "Utest (final)");
+         */ 
+      debug(P,"SerialDiagonalization");
+      SerialDiagonalization(P, DiagData);
+    }  
+    
+    //if(P->Call.out[ReadOut]) // && P->Par.me == 0) 
+      //PrintGSLMatrix(P, DiagData->U, DiagData->AllocNum, "U");
+  }
 }
 
@@ -256 +1142 @@
 void ComputeMLWF(struct Problem *P) {
   // variables and allocation
-  struct FileData *F = &P->Files;
   struct Lattice *Lat = &P->Lat;
-  struct RunStruct *R = &P->R;
   struct Psis *Psi = &Lat->Psi;
-  struct LatticeLevel *Lev0 = R->Lev0;
-  struct LatticeLevel *LevS = R->LevS;
-  struct Density *Dens0 = Lev0->Dens;
-  struct fft_plan_3d *plan = Lat->plan; 
-  fftw_complex *PsiC = Dens0->DensityCArray[ActualPsiDensity];
-  fftw_real *PsiCR = (fftw_real *)PsiC;
-  fftw_complex *work = Dens0->DensityCArray[Temp2Density]; 
-  fftw_real **HGcR = &Dens0->DensityArray[HGDensity];  // use HGDensity, 4x Gap..Density, TempDensity as a storage array
-  fftw_complex **HGcRC = (fftw_complex**)HGcR;
-  fftw_complex **HGcR2C = &Dens0->DensityCArray[HGcDensity];  // use HGcDensity, 4x Gap..Density, TempDensity as an array
-  fftw_real **HGcR2 = (fftw_real**)HGcR2C;
-  MPI_Status status;
-  struct OnePsiElement *OnePsiB, *OnePsiA, *LOnePsiB;
-  int ElementSize = (sizeof(fftw_complex) / sizeof(double)), RecvSource;
-  fftw_complex *LPsiDatA=NULL, *LPsiDatB=NULL;
-  int n[NDIM],n0,i0,iS, Index;
-  int N0;
-  int N[NDIM];
-  const int NUpx = LevS->NUp[0];
-  const int NUpy = LevS->NUp[1];
-  const int NUpz = LevS->NUp[2];
-  int e,i,j,k,l,m,u,p,g;
+  int i;
   int Num = Psi->NoOfPsis;  // is number of occupied plus unoccupied states for rows
+  double **WannierCentre;
+  double *WannierSpread;
+  double spread, spreadSQ;
+  struct DiagonalizationData DiagData;
+
+  if(P->Call.out[StepLeaderOut]) fprintf(stderr,"(%i) Beginning localization of orbitals ...\n",P->Par.me);
+
+  InitDiagonalization(P, &DiagData, Num, max_operators, 1);
+
+  // STEP 2: Calculate A[k]_ij = V/N \sum_{G1,G2} C^\ast_{l,G1} c_{m,G2} \sum_R A^{(k)}(R) exp(iR(G2-G1))
+  //debug (P,"Calculatung Variance matrices");
+  FillHigherOrderRealMomentsMatrices(P, DiagData.AllocNum, DiagData.A);
+
+  //debug (P,"Diagonalizing");
+  Diagonalize(P, &DiagData);
+
+  // STEP 6: apply transformation U to all wave functions \sum_i^Num U_ji | \phi_i \rangle = | \phi_j^\ast \rangle
+  //debug (P,"Performing Unitary transformation");
+  UnitaryTransformationOnWavefunctions(P, DiagData.U, Num);
+
+  if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 7: Compute centres and spread printout\n",P->Par.me);
+  WannierCentre = (double **) Malloc(sizeof(double *)*Num, "ComputeMLWF: *WannierCentre");
+  WannierSpread = (double *) Malloc(sizeof(double)*Num, "ComputeMLWF: WannierSpread"); 
+  for (i=0;i<Num;i++)
+    WannierCentre[i] = (double *) Malloc(sizeof(double)*NDIM, "ComputeMLWF: WannierCentre");
+
+  //debug (P,"Computing Wannier centres from variance matrix"); 
+  ComputeWannierCentresfromVarianceMatrices(P, DiagData.A, &spread, &spreadSQ, WannierCentre, WannierSpread);
+
+  // join Wannier orbital to groups with common centres under certain conditions
+  //debug (P,"Changing Wannier Centres according to CommonWannier");
+  ChangeWannierCentres(P, Num, WannierCentre, WannierSpread); 
+  
+  // write to file
+  //debug (P,"Writing spread file");
+  WriteWannierFile(P, spread, spreadSQ, WannierCentre, WannierSpread);  
+
+  debug(P,"Free'ing memory");
+  // free all remaining memory
+  FreeDiagonalization(&DiagData);
+  for (i=0;i<Num;i++)
+    Free(WannierCentre[i], "ComputeMLWF: WannierCentre[i]");
+  Free(WannierCentre, "ComputeMLWF: WannierCentre");
+  Free(WannierSpread, "ComputeMLWF: WannierSpread");
+}
+
+/** Solves directly for eigenvectors and eigenvalues of a real 2x2 matrix.
+ * The eigenvalues are determined by \f$\det{(A-\lambda \cdot I)}\f$, where \f$I\f$ is the unit matrix and
+ * \f$\lambda\f$ an eigenvalue. This leads to a pq-formula which is easily evaluated in the first part.
+ * 
+ * The eigenvectors are then obtained by solving \f$A-\lambda \cdot I)x = 0\f$ for a given eigenvalue
+ * \f$\lambda\f$. However, the eigenvector magnitudes are not specified, thus we the equation system is
+ * still lacking such an equation. We use this fact to set either coordinate arbitrarily to 1 and then
+ * derive the other by solving the equation system. Finally, the eigenvector is normalized.  
+ * \param *P Problem at hand
+ * \param *A matrix whose eigenvalues/-vectors are to be found
+ * \param *eval vector with eigenvalues
+ * \param *evec matrix with corresponding eigenvectors in columns
+ */
+#ifdef HAVE_INLINE 
+inline void EigensolverFor22Matrix(struct Problem *P, gsl_matrix *A, gsl_vector *eval, gsl_matrix *evec)
+#else
+void EigensolverFor22Matrix(struct Problem *P, gsl_matrix *A, gsl_vector *eval, gsl_matrix *evec)
+#endif
+{
+  double a11,a12,a21,a22;
+  double ev[2], summand1, summand2, norm;
+  int i;
+  // find eigenvalues
+  a11 = gsl_matrix_get(A,0,0);
+  a12 = gsl_matrix_get(A,0,1);
+  a21 = gsl_matrix_get(A,1,0);
+  a22 = gsl_matrix_get(A,1,1);
+  summand1 = (a11+a22)/2.;
+  summand2 = sqrt(summand1*summand1 + a12*a21 - a11*a22);
+  ev[0] = summand1 + summand2;
+  ev[1] = summand1 - summand2;
+  gsl_vector_set(eval, 0, ev[0]);
+  gsl_vector_set(eval, 1, ev[1]);
+  //fprintf (stderr,"(%i) ev1 %lg \t ev2 %lg\n", P->Par.me, ev1, ev2);
+  
+  // find eigenvectors
+  for(i=0;i<2;i++) {
+    if (fabs(ev[i]) < MYEPSILON) {
+      gsl_matrix_set(evec, 0,i, 0.);
+      gsl_matrix_set(evec, 1,i, 0.);
+    } else if (fabs(a22-ev[i]) > MYEPSILON) {
+      norm = sqrt(1*1 + (a21*a21/(a22-ev[i])/(a22-ev[i])));
+      gsl_matrix_set(evec, 0,i, 1./norm);
+      gsl_matrix_set(evec, 1,i, -(a21/(a22-ev[i]))/norm);
+      //fprintf (stderr,"(%i) evec %i (%lg,%lg)", P->Par.me, i, -(a12/(a11-ev[i]))/norm, 1./norm);
+    } else if (fabs(a12) > MYEPSILON) {
+      norm = sqrt(1*1 + (a11-ev[i])*(a11-ev[i])/(a12*a12));
+      gsl_matrix_set(evec, 0,i, 1./norm);
+      gsl_matrix_set(evec, 1,i, -((a11-ev[i])/a12)/norm);
+      //fprintf (stderr,"(%i) evec %i (%lg,%lg)", P->Par.me, i, -(a12/(a11-ev[i]))/norm, 1./norm);
+    } else {
+      if (fabs(a11-ev[i]) > MYEPSILON) {
+        norm = sqrt(1*1 + (a12*a12/(a11-ev[i])/(a11-ev[i])));
+        gsl_matrix_set(evec, 0,i, -(a12/(a11-ev[i]))/norm);
+        gsl_matrix_set(evec, 1,i, 1./norm);
+        //fprintf (stderr,"\t evec %i (%lg,%lg)\n", i, -(a12/(a11-ev[i]))/norm, 1./norm);
+      } else if (fabs(a21) > MYEPSILON) {
+        norm = sqrt(1*1 + (a22-ev[i])*(a22-ev[i])/(a21*a21));
+        gsl_matrix_set(evec, 0,i, -(a22-ev[i])/a21/norm);
+        gsl_matrix_set(evec, 1,i, 1./norm);
+        //fprintf (stderr,"\t evec %i (%lg,%lg)\n", i, -(a12/(a11-ev[i]))/norm, 1./norm);
+      } else {
+        //gsl_matrix_set(evec, 0,i, 0.);
+        //gsl_matrix_set(evec, 1,i, 1.);
+        fprintf (stderr,"\t evec %i undetermined\n", i);
+      }
+      //gsl_matrix_set(evec, 0,0, 1.);
+      //gsl_matrix_set(evec, 1,0, 0.);
+      //fprintf (stderr,"(%i) evec1 undetermined", P->Par.me);
+    }
+  }
+}
+
+/** Calculates sine and cosine values for multiple matrix diagonalization.
+ * \param *evec eigenvectors in columns
+ * \param *eval corresponding eigenvalues
+ * \param *c cosine to be returned
+ * \param *s sine to be returned
+ */
+#ifdef HAVE_INLINE
+inline void CalculateRotationAnglesFromEigenvalues(gsl_matrix *evec, gsl_vector *eval, double *c, double *s)
+#else
+void CalculateRotationAnglesFromEigenvalues(gsl_matrix *evec, gsl_vector *eval, double *c, double *s)
+#endif
+{
+  int index;
   double x,y,r;
-  double q[NDIM];
-  double *c,*s;
-  int index;
-  double spread = 0., old_spread=0., Spread=0.;
-  double WannierCentre[Num][NDIM];
-  double WannierSpread[Num];
-  double tmp,tmp2;
-  double a_ij = 0, b_ij = 0, A_ij = 0, B_ij = 0;
-  double **cos_lookup,**sin_lookup; 
-  gsl_matrix *G;
-  gsl_vector *h;
-  gsl_vector *eval;
-  gsl_matrix *evec;
-  gsl_eigen_symmv_workspace *w;
-  int ProcNum, ProcRank, set;
-  int it_steps; // iteration step counter
-  int *top, *bot;
-  int Lsend, Rsend, Lrecv, Rrecv; // where left(right) column is sent to or where it originates from
-  int left, right; // left or right neighbour for process
-  double **Aloc[max_operators+1], **Uloc; // local columns for one step of A[k]
-  double *Around[max_operators+1], *Uround; // all local columns for one round of A[k]
-  double *Atotal[max_operators+1], *Utotal; // all local columns for one round of A[k]
-  double a_i, a_j;
+  
+  index = gsl_vector_max_index (eval);  // get biggest eigenvalue
+  //fprintf(stderr,"\t1st: %lg\t2nd: %lg ---  biggest: %i\n", gsl_vector_get(eval, 0), gsl_vector_get(eval, 1), index);
+  x = gsl_matrix_get(evec, 0, index);
+  y = gsl_matrix_get(evec, 1, index);
+  if (x < 0) {  // ensure x>=0 so that rotation angles remain smaller Pi/4
+    y = -y;
+    x = -x;  
+  }
+  //z = gsl_matrix_get(evec, 2, index) * x/fabs(x);
+  //fprintf(stderr,"\tx %lg\ty %lg\n", x,y);
+  
+  //fprintf(stderr,"(%i),(%i,%i) STEP 3c\n",P->Par.me,i,j);
+  // STEP 3c: calculate R = [[c,s^\ast],[-s,c^\ast]]
+  r = sqrt(x*x + y*y); // + z*z);
+  if (fabs(r) > MYEPSILON) {
+    *c = sqrt((x + r) / (2.*r));
+    *s = y / sqrt(2.*r*(x+r)); //, -z / sqrt(2*r*(x+r)));
+  } else {
+    *c = 1.;
+    *s = 0.;
+  }
+}
+
+/*
+ * \param **A matrices (pointer array with \a NumMatrices entries) to be diagonalized
+ * \param *U transformation matrix set to unity matrix
+ * \param NumMatrices number of matrices to be diagonalized simultaneously
+ * \param extra number of additional matrices the rotation is applied to however which actively diagonalized (follow in \a **A)
+ * \param AllocNum number of rows/columns in matrices
+ * \param Num number of wave functions
+ * \param ProcRank index in group for this cpu
+ * \param ProcNum number of cpus in group
+ * \param *top array with top row indices (merry-go-round)
+ * \param *bot array with bottom row indices (merry-go-round) 
+ */
+
+/** Simultaneous diagonalization of matrices with multiple cpus.
+ * \param *P Problem at hand
+ * \param *DiagData pointer to structure DiagonalizationData containing necessary information for diagonalization
+ * \note this is slower given one cpu only than SerialDiagonalization()
+ */
+void ParallelDiagonalization(struct Problem *P, struct DiagonalizationData *DiagData)
+{
+  struct RunStruct *R =&P->R;
   int tagR0, tagR1, tagS0, tagS1;
   int iloc, jloc;
@@ -313 +1330 @@
   int start;
   int *rcounts, *rdispls;
-  int AllocNum = ceil((double)Num / 2. ) *2;
-  int totalflag, flag;
-  int *marker, **group;
-  int partner[Num];
-  int type = Occupied;
-  MPI_Comm *comm;
-  char spin[12], suffix[18];
-  
-  N0 = LevS->Plan0.plan->local_nx;
-  N[0] = LevS->Plan0.plan->N[0];
-  N[1] = LevS->Plan0.plan->N[1];
-  N[2] = LevS->Plan0.plan->N[2];
-  
-  comm = &P->Par.comm_ST;
-  
-  fprintf(stderr,"(%i) Comparing groups - AllocNum %i --- All %i\t Psi %i\t PsiT %i\n",P->Par.me, AllocNum, P->Par.Max_me_comm_ST, P->Par.Max_me_comm_ST_Psi, P->Par.Max_my_color_comm_ST_Psi);
-  if (AllocNum % (P->Par.Max_me_comm_ST*2) == 0) {  // all parallel
-    comm = &P->Par.comm_ST;
-    fprintf(stderr,"(%i) Jacobi is done parallely by all\n", P->Par.me);           
-  } else if (P->Par.Max_me_comm_ST_Psi >= P->Par.Max_my_color_comm_ST_Psi) { // always the bigger group comes first  
-    if (AllocNum % (P->Par.Max_me_comm_ST_Psi*2) == 0) {  // coefficients parallel
-      comm = &P->Par.comm_ST_Psi;
-      fprintf(stderr,"(%i) Jacobi is done parallely by Psi\n", P->Par.me);       
-    } else if (AllocNum % (P->Par.Max_my_color_comm_ST_Psi*2) == 0) {  // Psis parallel
-      comm = &P->Par.comm_ST_PsiT;
-      fprintf(stderr,"(%i) Jacobi is done parallely by PsiT\n", P->Par.me);       
-    }
+  double *c, *s;
+  int Lsend, Rsend, Lrecv, Rrecv; // where left(right) column is sent to or where it originates from
+  int left, right; // left or right neighbour for process
+  double spread = 0., old_spread=0., Spread=0.;
+  int i,j,k,l,m,u;
+  int set;
+  int it_steps; // iteration step counter
+  double **Aloc[DiagData->NumMatrices+1], **Uloc; // local columns for one step of A[k]
+  double *Around[DiagData->NumMatrices+1], *Uround; // all local columns for one round of A[k]
+  double *Atotal[DiagData->NumMatrices+1], *Utotal; // all local columns for one round of A[k]
+  double a_i, a_j;
+  gsl_matrix *G;
+  gsl_vector *h;
+  gsl_vector *eval;
+  gsl_matrix *evec;
+  //gsl_eigen_symmv_workspace *w;
+
+  max_rounds = (DiagData->AllocNum / 2)/DiagData->ProcNum;  // each process must perform multiple rotations per step of a set
+  if (P->Call.out[ReadOut]) fprintf(stderr,"(%i) start %i\tstep %i\tmax.rounds %i\n",P->Par.me, DiagData->ProcRank, DiagData->ProcNum, max_rounds);
+  
+  // allocate column vectors for interchange of columns
+  debug(P,"allocate column vectors for interchange of columns");
+  c = (double *) Malloc(sizeof(double)*max_rounds, "ComputeMLWF: c");
+  s = (double *) Malloc(sizeof(double)*max_rounds, "ComputeMLWF: s");
+  c_all = (double *) Malloc(sizeof(double)*DiagData->AllocNum/2, "ComputeMLWF: c_all");
+  s_all = (double *) Malloc(sizeof(double)*DiagData->AllocNum/2, "ComputeMLWF: s_all");
+  rcounts = (int *) Malloc(sizeof(int)*DiagData->ProcNum, "ComputeMLWF: rcounts");
+  rdispls = (int *) Malloc(sizeof(int)*DiagData->ProcNum, "ComputeMLWF: rdispls");
+
+  // allocate eigenvector stuff
+  debug(P,"allocate eigenvector stuff");
+  G = gsl_matrix_calloc (2,2);
+  h = gsl_vector_alloc (2);
+  eval = gsl_vector_alloc (2);
+  evec = gsl_matrix_alloc (2,2);
+  //w = gsl_eigen_symmv_alloc(2);
+  
+  // establish communication partners
+  debug(P,"establish communication partners");
+  if (DiagData->ProcRank == 0) {
+    tagS0 = WannierALTag;   // left p0 always remains left p0
   } else {
-    if (AllocNum % (P->Par.Max_my_color_comm_ST_Psi*2) == 0) {  // Psis parallel 
-      comm = &P->Par.comm_ST_PsiT;
-      fprintf(stderr,"(%i) Jacobi is done parallely by PsiT\n", P->Par.me);       
-    } else if (AllocNum % (P->Par.Max_me_comm_ST_Psi*2) == 0) {  // coefficients parallel
-      comm = &P->Par.comm_ST_Psi;
-      fprintf(stderr,"(%i) Jacobi is done parallely by Psi\n", P->Par.me);       
-    }
-  }
-
-  MPI_Comm_size (*comm, &ProcNum);
-  MPI_Comm_rank (*comm, &ProcRank);
-  
-  if(P->Call.out[StepLeaderOut]) fprintf(stderr,"(%i) Beginning localization of orbitals ...\n",P->Par.me);
-  
-  if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 2\n",P->Par.me);
-
-  // STEP 2: Calculate A[k]_ij = V/N \sum_{G1,G2} C^\ast_{l,G1} c_{m,G2} \sum_R A^{(k)}(R) exp(iR(G2-G1))
-  gsl_matrix *A[max_operators+1];   // one extra for B matrix
-  for (u=0;u<=max_operators;u++)
-    A[u] = gsl_matrix_calloc (AllocNum,AllocNum);  // allocate matrix    
-
-  // create lookup table for sin/cos values
-  cos_lookup = (double **) Malloc(sizeof(double *)*NDIM, "ComputeMLWF: *cos_lookup");
-  sin_lookup = (double **) Malloc(sizeof(double *)*NDIM, "ComputeMLWF: *sin_lookup");
-  for (i=0;i<NDIM;i++) {
-    // allocate memory
-    cos_lookup[i] = (double *) Malloc(sizeof(double)*LevS->Plan0.plan->N[i], "ComputeMLWF: cos_lookup");
-    sin_lookup[i] = (double *) Malloc(sizeof(double)*LevS->Plan0.plan->N[i], "ComputeMLWF: sin_lookup");
-    // reset arrays
-    SetArrayToDouble0(cos_lookup[i],LevS->Plan0.plan->N[i]);
-    SetArrayToDouble0(sin_lookup[i],LevS->Plan0.plan->N[i]);
-    // create lookup values
-    for (j=0;j<LevS->Plan0.plan->N[i];j++) {
-      tmp = 2*PI/(double)LevS->Plan0.plan->N[i]*(double)j; 
-      cos_lookup[i][j] = cos(tmp);
-      sin_lookup[i][j] = sin(tmp);
-    }
-  }
-  l=-1; // to access U matrix element (0..Num-1)
-  // fill the matrices
-  for (i=0; i < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; i++) {  // go through all wave functions
-    OnePsiA = &Psi->AllPsiStatus[i];    // grab OnePsiA
-    if (OnePsiA->PsiType == type) {   // drop all but occupied ones
-      l++;  // increase l if it is non-extra wave function
-      if (OnePsiA->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // local?
-        LPsiDatA=LevS->LPsi->LocalPsi[OnePsiA->MyLocalNo];
-      else
-        LPsiDatA = NULL;  // otherwise processes won't enter second loop, though they're supposed to send coefficients!
-      
-      //fprintf(stderr,"(%i),(%i,%i): fft'd, A[..] and B, back-fft'd acting on \\phi_A\n",P->Par.me,l,0);
-      if (LPsiDatA != NULL) {
-        CalculateOneDensityR(Lat, LevS, Dens0, LPsiDatA, Dens0->DensityArray[ActualDensity], R->FactorDensityR, 1);
-        // note: factor is not used when storing result in DensityCArray[ActualPsiDensity] in CalculateOneDensityR()!
-        for (n0=0;n0<N0;n0++)  
-          for (n[1]=0;n[1]<N[1];n[1]++) 
-            for (n[2]=0;n[2]<N[2];n[2]++) {
-              i0 = n[2]*NUpz+N[2]*NUpz*(n[1]*NUpy+N[1]*NUpy*n0*NUpx);
-              iS = n[2]+N[2]*(n[1]+N[1]*n0);
-              n[0] = n0 + LevS->Plan0.plan->start_nx;
-              for (k=0;k<max_operators;k+=2) {
-                e = k/2;
-                tmp = 2*PI/(double)(N[e])*(double)(n[e]);
-                tmp2 = PsiCR[i0] /LevS->MaxN;
-                // check lookup
-                if (!l) // perform check on first wave function only
-                  if ((fabs(cos(tmp) - cos_lookup[e][n[e]]) > MYEPSILON) || (fabs(sin(tmp) - sin_lookup[e][n[e]]) > MYEPSILON)) {
-                    Error(SomeError, "Lookup table does not match real value!");
-                  }
-                HGcR[k][iS] = cos_lookup[e][n[e]] * tmp2; /* Matrix Vector Mult */
-                HGcR2[k][iS] = cos_lookup[e][n[e]] * HGcR[k][iS]; /* Matrix Vector Mult */
-                HGcR[k+1][iS] = sin_lookup[e][n[e]] * tmp2; /* Matrix Vector Mult */
-                HGcR2[k+1][iS] = sin_lookup[e][n[e]] * HGcR[k+1][iS]; /* Matrix Vector Mult */
-              }
-            }
-        for (u=0;u<max_operators;u++) {
-          fft_3d_real_to_complex(plan, LevS->LevelNo, FFTNF1, HGcRC[u], work);
-          fft_3d_real_to_complex(plan, LevS->LevelNo, FFTNF1, HGcR2C[u], work);
+    tagS0 = (DiagData->ProcRank == DiagData->ProcNum - 1) ? WannierARTag : WannierALTag; // left p_last becomes right p_last          
+  }
+  tagS1 = (DiagData->ProcRank == 0) ? WannierALTag : WannierARTag; // right p0 always goes into left p1
+  tagR0 = WannierALTag; // 
+  tagR1 = WannierARTag; // first process
+  if (DiagData->ProcRank == 0) {
+    left = DiagData->ProcNum-1;
+    right = 1;
+    Lsend = 0;
+    Rsend = 1;
+    Lrecv = 0;
+    Rrecv = 1;        
+  } else if (DiagData->ProcRank == DiagData->ProcNum - 1) {
+    left = DiagData->ProcRank - 1;
+    right = 0;
+    Lsend = DiagData->ProcRank;
+    Rsend = DiagData->ProcRank - 1;
+    Lrecv = DiagData->ProcRank - 1;
+    Rrecv = DiagData->ProcRank;        
+  } else {
+    left = DiagData->ProcRank - 1;
+    right = DiagData->ProcRank + 1;
+    Lsend = DiagData->ProcRank+1;
+    Rsend = DiagData->ProcRank - 1;
+    Lrecv = DiagData->ProcRank - 1;
+    Rrecv = DiagData->ProcRank+1;
+  }
+  //if (P->Call.out[ReadOut]) fprintf(stderr,"(%i) left %i\t right %i --- Lsend %i\tRsend%i\tLrecv %i\tRrecv%i\n",P->Par.me, left, right, Lsend, Rsend, Lrecv, Rrecv);
+  
+  // initialise A_loc
+  debug(P,"initialise A_loc");
+  for (k=0;k<DiagData->NumMatrices+DiagData->extra;k++) {
+    //Aloc[k] = (double *) Malloc(sizeof(double)*AllocNum*2, "ComputeMLWF: Aloc[k]");
+    Around[k] = (double *) Malloc(sizeof(double)*DiagData->AllocNum*2*max_rounds, "ComputeMLWF: Around[k]");
+    Atotal[k] = (double *) Malloc(sizeof(double)*DiagData->AllocNum*DiagData->AllocNum, "ComputeMLWF: Atotal[k]");
+    Aloc[k] = (double **) Malloc(sizeof(double *)*2*max_rounds, "ComputeMLWF: Aloc[k]");
+    //Around[k] = &Atotal[k][ProcRank*AllocNum*2*max_rounds];
+    
+    for (round=0;round<max_rounds;round++) {
+      Aloc[k][2*round] = &Around[k][DiagData->AllocNum*(2*round)];
+      Aloc[k][2*round+1] = &Around[k][DiagData->AllocNum*(2*round+1)];
+      for (l=0;l<DiagData->AllocNum;l++) {
+        Aloc[k][2*round][l] = gsl_matrix_get(DiagData->A[k],l,2*(DiagData->ProcRank*max_rounds+round));
+        Aloc[k][2*round+1][l] = gsl_matrix_get(DiagData->A[k],l,2*(DiagData->ProcRank*max_rounds+round)+1);
+        //fprintf(stderr,"(%i) (%i, 0/1) A_loc1 %e\tA_loc2 %e\n",P->Par.me, l, Aloc[k][l],Aloc[k][l+AllocNum]);
+      }
+    }
+  }
+  // initialise U_loc
+  debug(P,"initialise U_loc");
+  //Uloc = (double *) Malloc(sizeof(double)*AllocNum*2, "ComputeMLWF: Uloc");
+  Uround = (double *) Malloc(sizeof(double)*DiagData->AllocNum*2*max_rounds, "ComputeMLWF: Uround");
+  Utotal = (double *) Malloc(sizeof(double)*DiagData->AllocNum*DiagData->AllocNum, "ComputeMLWF: Utotal");
+  Uloc = (double **) Malloc(sizeof(double *)*2*max_rounds, "ComputeMLWF: Uloc");
+  //Uround = &Utotal[ProcRank*AllocNum*2*max_rounds];
+  for (round=0;round<max_rounds;round++) {
+    Uloc[2*round] = &Uround[DiagData->AllocNum*(2*round)];
+    Uloc[2*round+1] = &Uround[DiagData->AllocNum*(2*round+1)];
+    for (l=0;l<DiagData->AllocNum;l++) {
+      Uloc[2*round][l] = gsl_matrix_get(DiagData->U,l,2*(DiagData->ProcRank*max_rounds+round));
+      Uloc[2*round+1][l] = gsl_matrix_get(DiagData->U,l,2*(DiagData->ProcRank*max_rounds+round)+1);
+      //fprintf(stderr,"(%i) (%i, 0/1) U_loc1 %e\tU_loc2 %e\n",P->Par.me, l, Uloc[l+AllocNum*0],Uloc[l+AllocNum*1]);
+    }
+  }
+  
+  // now comes the iteration loop
+  debug(P,"now comes the iteration loop");
+  it_steps = 0;
+  do {
+    it_steps++;
+    //if (P->Par.me == 0) fprintf(stderr,"(%i) Beginning parallel iteration %i ... ",P->Par.me,it_steps);
+    for (set=0; set < DiagData->AllocNum-1; set++) { // one column less due to column 0 staying at its place all the time
+      //fprintf(stderr,"(%i) Beginning rotation set %i ...\n",P->Par.me,set);
+      for (round = 0; round < max_rounds;round++) {
+        start = DiagData->ProcRank * max_rounds + round;
+        // get indices
+        i = DiagData->top[start] < DiagData->bot[start] ? DiagData->top[start] : DiagData->bot[start]; // minimum of the two indices
+        iloc = DiagData->top[start] < DiagData->bot[start] ? 0 : 1; 
+        j = DiagData->top[start] > DiagData->bot[start] ? DiagData->top[start] : DiagData->bot[start]; // maximum of the two indices: thus j >  i
+        jloc =  DiagData->top[start] > DiagData->bot[start] ? 0 : 1;
+        //fprintf(stderr,"(%i) my (%i,%i), loc(%i,%i)\n",P->Par.me, i,j, iloc, jloc);
+        
+        // calculate rotation angle, i.e. c and s
+        //fprintf(stderr,"(%i),(%i,%i) calculate rotation angle\n",P->Par.me,i,j);
+        gsl_matrix_set_zero(G);        
+        for (k=0;k<DiagData->NumMatrices;k++) { // go through all operators ...
+          // Calculate vector h(a) = [a_ii - a_jj, a_ij + a_ji, i(a_ji - a_ij)]         
+          //fprintf(stderr,"(%i) k%i [a_ii - a_jj] = %e - %e = %e\n",P->Par.me, k,Aloc[k][2*round+iloc][i], Aloc[k][2*round+jloc][j],Aloc[k][2*round+iloc][i] - Aloc[k][2*round+jloc][j]);          
+          //fprintf(stderr,"(%i) k%i [a_ij + a_ji] = %e + %e = %e\n",P->Par.me, k,Aloc[k][2*round+jloc][i], Aloc[k][2*round+iloc][j],Aloc[k][2*round+jloc][i] + Aloc[k][2*round+iloc][j]);          
+          gsl_vector_set(h, 0, Aloc[k][2*round+iloc][i] - Aloc[k][2*round+jloc][j]);
+          gsl_vector_set(h, 1, Aloc[k][2*round+jloc][i] + Aloc[k][2*round+iloc][j]);
+          
+          // Calculate G = Re[ \sum_k h^H (A^{(k)}) h(A^{(k)}) ]
+          for (l=0;l<2;l++)
+            for (m=0;m<2;m++)
+                gsl_matrix_set(G,l,m, gsl_vector_get(h,l) * gsl_vector_get(h,m) + gsl_matrix_get(G,l,m));
         }
-      }  
-      m = -1; // to access U matrix element (0..Num-1)
-      for (j=0; j < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; j++) {  // go through all wave functions
-        OnePsiB = &Psi->AllPsiStatus[j];    // grab OnePsiB
-        if (OnePsiB->PsiType == type) {   // drop all but occupied ones
-          m++;  // increase m if it is non-extra wave function
-          if (OnePsiB->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // local?
-             LOnePsiB = &Psi->LocalPsiStatus[OnePsiB->MyLocalNo];
-          else 
-             LOnePsiB = NULL;
-          if (LOnePsiB == NULL) {   // if it's not local ... receive it from respective process into TempPsi
-            RecvSource = OnePsiB->my_color_comm_ST_Psi;
-            MPI_Recv( LevS->LPsi->TempPsi, LevS->MaxG*ElementSize, MPI_DOUBLE, RecvSource, WannierTag2, P->Par.comm_ST_PsiT, &status );
-            LPsiDatB=LevS->LPsi->TempPsi;
-          } else {                  // .. otherwise send it to all other processes (Max_me... - 1)
-            for (p=0;p<P->Par.Max_me_comm_ST_PsiT;p++)
-              if (p != OnePsiB->my_color_comm_ST_Psi) 
-                MPI_Send( LevS->LPsi->LocalPsi[OnePsiB->MyLocalNo], LevS->MaxG*ElementSize, MPI_DOUBLE, p, WannierTag2, P->Par.comm_ST_PsiT);
-            LPsiDatB=LevS->LPsi->LocalPsi[OnePsiB->MyLocalNo];
-          } // LPsiDatB is now set to the coefficients of OnePsi either stored or MPI_Received
-
-          for (u=0;u<max_operators;u++) {
-            a_ij = 0;
-            b_ij = 0;
-            if (LPsiDatA != NULL) { // calculate, reduce and send to all ...
-              //fprintf(stderr,"(%i),(%i,%i): A[%i]: multiplying with \\phi_B\n",P->Par.me, l,m,u);
-              g=0;
-              if (LevS->GArray[0].GSq == 0.0) {
-                Index = LevS->GArray[g].Index;
-                a_ij = (LPsiDatB[0].re*HGcRC[u][Index].re + LPsiDatB[0].im*HGcRC[u][Index].im);
-                b_ij = (LPsiDatB[0].re*HGcR2C[u][Index].re + LPsiDatB[0].im*HGcR2C[u][Index].im);
-                g++;
-              }
-              for (; g < LevS->MaxG; g++) {
-                Index = LevS->GArray[g].Index;
-                a_ij += 2*(LPsiDatB[g].re*HGcRC[u][Index].re + LPsiDatB[g].im*HGcRC[u][Index].im);
-                b_ij += 2*(LPsiDatB[g].re*HGcR2C[u][Index].re + LPsiDatB[g].im*HGcR2C[u][Index].im);
-              } // due to the symmetry the resulting matrix element is real and symmetric in (i,j) ! (complex multiplication simplifies ...)
-              // sum up elements from all coefficients sharing processes
-              MPI_Allreduce ( &a_ij, &A_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_Psi);
-              MPI_Allreduce ( &b_ij, &B_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_Psi);
-              a_ij = A_ij;
-              b_ij = B_ij;
-              // send element to all Psi-sharing who don't have l local (MPI_Send is a lot slower than AllReduce!)
-              MPI_Allreduce ( &a_ij, &A_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_PsiT);
-              MPI_Allreduce ( &b_ij, &B_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_PsiT);
-            } else { // receive ...
-              MPI_Allreduce ( &a_ij, &A_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_PsiT);
-              MPI_Allreduce ( &b_ij, &B_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_PsiT);              
-            }
-            // ... and store
-            //fprintf(stderr,"(%i),(%i,%i): A[%i]: setting component (local: %lg, total: %lg)\n",P->Par.me, l,m,u,a_ij,A_ij);
-            //fprintf(stderr,"(%i),(%i,%i): B: adding upon component (local: %lg, total: %lg)\n",P->Par.me, l,m,b_ij,B_ij);
-            gsl_matrix_set(A[u], l, m, A_ij);
-            gsl_matrix_set(A[max_operators], l, m, B_ij + gsl_matrix_get(A[max_operators],l,m));
+        //fprintf(stderr,"(%i),(%i,%i) STEP 3b\n",P->Par.me,i,j);
+        // STEP 3b: retrieve eigenvector which belongs to greatest eigenvalue of G
+        EigensolverFor22Matrix(P,G,eval,evec);
+        //gsl_eigen_symmv(G, eval, evec, w);  // calculates eigenvalues and eigenvectors of G    
+
+        CalculateRotationAnglesFromEigenvalues(evec, eval, &c[round], &s[round]);
+        //fprintf(stderr,"(%i),(%i,%i) COS %e\t SIN %e\n",P->Par.me,i,j,c[round],s[round]);
+
+        //fprintf(stderr,"(%i),(%i,%i) STEP 3e\n",P->Par.me,i,j);
+        // V_loc = V_loc * V_small
+        //debug(P,"apply rotation to local U");
+        for (l=0;l<DiagData->AllocNum;l++) {
+          a_i = Uloc[2*round+iloc][l];
+          a_j = Uloc[2*round+jloc][l];
+          Uloc[2*round+iloc][l] =  c[round] * a_i + s[round] * a_j;
+          Uloc[2*round+jloc][l] = -s[round] * a_i + c[round] * a_j;
+        }
+      }  // end of round        
+      // circulate the rotation angles
+      //debug(P,"circulate the rotation angles");
+      MPI_Allgather(c, max_rounds, MPI_DOUBLE, c_all, max_rounds, MPI_DOUBLE, *(DiagData->comm));   // MPI_Allgather is waaaaay faster than ring circulation
+      MPI_Allgather(s, max_rounds, MPI_DOUBLE, s_all, max_rounds, MPI_DOUBLE, *(DiagData->comm));
+      //m = start;
+      for (l=0;l<DiagData->AllocNum/2;l++) { // for each process
+        // we have V_small from process k
+        //debug(P,"Apply V_small from other process");
+        i = DiagData->top[l] < DiagData->bot[l] ? DiagData->top[l] : DiagData->bot[l]; // minimum of the two indices
+        j = DiagData->top[l] > DiagData->bot[l] ? DiagData->top[l] : DiagData->bot[l]; // maximum of the two indices: thus j >  i
+        iloc = DiagData->top[l] < DiagData->bot[l] ? 0 : 1; 
+        jloc = DiagData->top[l] > DiagData->bot[l] ? 0 : 1;
+        for (m=0;m<max_rounds;m++) {
+          //fprintf(stderr,"(%i) %i processes' (%i,%i)\n",P->Par.me, m,i,j);
+          // apply row rotation to each A[k]
+          for (k=0;k<DiagData->NumMatrices+DiagData->extra;k++) {// one extra for B matrix !
+            //fprintf(stderr,"(%i) A:(k%i) a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, i, Aloc[k][2*m+iloc][i],j,Aloc[k][2*m+iloc][j]);
+            //fprintf(stderr,"(%i) A:(k%i) a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, i, Aloc[k][2*m+jloc][i],j,Aloc[k][2*m+jloc][j]);
+            a_i = Aloc[k][2*m+iloc][i];
+            a_j = Aloc[k][2*m+iloc][j];
+            Aloc[k][2*m+iloc][i] =  c_all[l] * a_i + s_all[l] * a_j;
+            Aloc[k][2*m+iloc][j] = -s_all[l] * a_i + c_all[l] * a_j;
+            a_i = Aloc[k][2*m+jloc][i];
+            a_j = Aloc[k][2*m+jloc][j];
+            Aloc[k][2*m+jloc][i] =  c_all[l] * a_i + s_all[l] * a_j;
+            Aloc[k][2*m+jloc][j] = -s_all[l] * a_i + c_all[l] * a_j;
+            //fprintf(stderr,"(%i) A^%i: a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, i, Aloc[k][2*m+iloc][i],j,Aloc[k][2*m+iloc][j]);
+            //fprintf(stderr,"(%i) A^%i: a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, i, Aloc[k][2*m+jloc][i],j,Aloc[k][2*m+jloc][j]);
           }
         }
       }
-    }
-  }
-  // reset extra entries
-  for (u=0;u<=max_operators;u++) {
-    for (i=Num;i<AllocNum;i++)
-      for (j=0;j<AllocNum;j++)
-        gsl_matrix_set(A[u], i,j, 0.);
-    for (i=Num;i<AllocNum;i++)
-      for (j=0;j<AllocNum;j++)
-        gsl_matrix_set(A[u], j,i, 0.);
-  }
-  // free lookups
-  for (i=0;i<NDIM;i++) {
-    Free(cos_lookup[i], "bla");
-    Free(sin_lookup[i], "bla");
-  }
-  Free(cos_lookup, "bla");
-  Free(sin_lookup, "bla");
-  
-  if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 1\n",P->Par.me);
-  // STEP 1: Initialize U = 1
-  gsl_matrix *U = gsl_matrix_alloc (AllocNum,AllocNum);
-  gsl_matrix_set_identity(U);
-
-  // init merry-go-round array
-  top = (int *) Malloc(sizeof(int)*AllocNum/2, "ComputeMLWF: top");
-  bot = (int *) Malloc(sizeof(int)*AllocNum/2, "ComputeMLWF: bot");
-  //debug(P,"init merry-go-round array");
-  for (i=0;i<AllocNum/2;i++) {
-    top[i] = 2*i;
-    bot[i] = 2*i+1;      
-  }
-
-/*// print A matrices for debug
-  if (P->Par.me == 0) 
-    for (u=0;u<max_operators+1;u++) {
-     fprintf(stderr, "A[%i] = \n",u);
-      for (i=0;i<Num;i++) {
-        for (j=0;j<Num;j++)
-          fprintf(stderr, "%e\t",gsl_matrix_get(A[u],i,j));
-        fprintf(stderr, "\n");
-      }
-    }  
-*/
-  if (Num != 1) {
-    // one- or multi-process case?
-    if (((AllocNum % 2) == 0) && (ProcNum != 1) && ((AllocNum / 2) % ProcNum == 0)) {
-      max_rounds = (AllocNum / 2)/ProcNum;  // each process must perform multiple rotations per step of a set
-      //fprintf(stderr,"(%i) start %i\tstep %i\tmax.rounds %i\n",P->Par.me, ProcRank, ProcNum, max_rounds);
-      // allocate column vectors for interchange of columns
-      //debug(P,"allocate column vectors for interchange of columns");
-      c = (double *) Malloc(sizeof(double)*max_rounds, "ComputeMLWF: c");
-      s = (double *) Malloc(sizeof(double)*max_rounds, "ComputeMLWF: s");
-      c_all = (double *) Malloc(sizeof(double)*AllocNum/2, "ComputeMLWF: c_all");
-      s_all = (double *) Malloc(sizeof(double)*AllocNum/2, "ComputeMLWF: s_all");
-      rcounts = (int *) Malloc(sizeof(int)*ProcNum, "ComputeMLWF: rcounts");
-      rdispls = (int *) Malloc(sizeof(int)*ProcNum, "ComputeMLWF: rdispls");
-/*    // print starting values of index generation tables top and bot
-      fprintf(stderr,"top\t");
-      for (k=0;k<AllocNum/2;k++)
-        fprintf(stderr,"%i\t", top[k]);
-      fprintf(stderr,"\n");
-      fprintf(stderr,"bot\t");
-      for (k=0;k<AllocNum/2;k++)
-        fprintf(stderr,"%i\t", bot[k]);
-      fprintf(stderr,"\n");*/
-      // establish communication partners
-      //debug(P,"establish communication partners");
-      if (ProcRank == 0) {
-        tagS0 = WannierALTag;   // left p0 always remains left p0
-      } else {
-        tagS0 = ProcRank == ProcNum - 1 ? WannierARTag : WannierALTag; // left p_last becomes right p_last          
-      }
-      tagS1 = ProcRank == 0 ? WannierALTag : WannierARTag; // right p0 always goes into left p1
-      tagR0 = WannierALTag; // 
-      tagR1 = WannierARTag; // first process
-      if (ProcRank == 0) {
-        left = ProcNum-1;
-        right = 1;
-        Lsend = 0;
-        Rsend = 1;
-        Lrecv = 0;
-        Rrecv = 1;        
-      } else if (ProcRank == ProcNum - 1) {
-        left = ProcRank - 1;
-        right = 0;
-        Lsend = ProcRank;
-        Rsend = ProcRank - 1;
-        Lrecv = ProcRank - 1;
-        Rrecv = ProcRank;        
-      } else {
-        left = ProcRank - 1;
-        right = ProcRank + 1;
-        Lsend = ProcRank+1;
-        Rsend = ProcRank - 1;
-        Lrecv = ProcRank - 1;
-        Rrecv = ProcRank+1;
-      }
-      //fprintf(stderr,"(%i) left %i\t right %i --- Lsend %i\tRsend%i\tLrecv %i\tRrecv%i\n",P->Par.me, left, right, Lsend, Rsend, Lrecv, Rrecv);
-      // allocate eigenvector stuff
-      //debug(P,"allocate eigenvector stuff");
-      G = gsl_matrix_calloc (2,2);
-      h = gsl_vector_alloc (2);
-      eval = gsl_vector_alloc (2);
-      evec = gsl_matrix_alloc (2,2);
-      w = gsl_eigen_symmv_alloc(2);
-      // initialise A_loc
-      //debug(P,"initialise A_loc");
-      for (k=0;k<max_operators+1;k++) {
-        //Aloc[k] = (double *) Malloc(sizeof(double)*AllocNum*2, "ComputeMLWF: Aloc[k]");
-        Around[k] = (double *) Malloc(sizeof(double)*AllocNum*2*max_rounds, "ComputeMLWF: Around[k]");
-        Atotal[k] = (double *) Malloc(sizeof(double)*AllocNum*AllocNum, "ComputeMLWF: Atotal[k]");
-        Aloc[k] = (double **) Malloc(sizeof(double *)*2*max_rounds, "ComputeMLWF: Aloc[k]");
-        //Around[k] = &Atotal[k][ProcRank*AllocNum*2*max_rounds];
-        
-        for (round=0;round<max_rounds;round++) {
-          Aloc[k][2*round] = &Around[k][AllocNum*(2*round)];
-          Aloc[k][2*round+1] = &Around[k][AllocNum*(2*round+1)];
-          for (l=0;l<AllocNum;l++) {
-            Aloc[k][2*round][l] = gsl_matrix_get(A[k],l,2*(ProcRank*max_rounds+round));
-            Aloc[k][2*round+1][l] = gsl_matrix_get(A[k],l,2*(ProcRank*max_rounds+round)+1);
-            //fprintf(stderr,"(%i) (%i, 0/1) A_loc1 %e\tA_loc2 %e\n",P->Par.me, l, Aloc[k][l],Aloc[k][l+AllocNum]);
+      // apply rotation to local operator matrices
+      // A_loc = A_loc * V_small 
+      //debug(P,"apply rotation to local operator matrices A[k]");
+      for (m=0;m<max_rounds;m++) {
+        start = DiagData->ProcRank * max_rounds + m;
+        iloc = DiagData->top[start] < DiagData->bot[start] ? 0 : 1; 
+        jloc = DiagData->top[start] > DiagData->bot[start] ? 0 : 1;
+        for (k=0;k<DiagData->NumMatrices+DiagData->extra;k++) {// extra for B matrix !
+          for (l=0;l<DiagData->AllocNum;l++) {
+            // Columns, i and j belong to this process only!
+            a_i = Aloc[k][2*m+iloc][l];
+            a_j = Aloc[k][2*m+jloc][l];
+            Aloc[k][2*m+iloc][l] =  c[m] * a_i + s[m] * a_j;
+            Aloc[k][2*m+jloc][l] = -s[m] * a_i + c[m] * a_j;
+            //fprintf(stderr,"(%i) A:(k%i) a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, l, Aloc[k][2*m+iloc][l],l,Aloc[k][2*m+jloc][l]);
           }
         }
       }
-      // initialise U_loc
-      //debug(P,"initialise U_loc");
-      //Uloc = (double *) Malloc(sizeof(double)*AllocNum*2, "ComputeMLWF: Uloc");
-      Uround = (double *) Malloc(sizeof(double)*AllocNum*2*max_rounds, "ComputeMLWF: Uround");
-      Utotal = (double *) Malloc(sizeof(double)*AllocNum*AllocNum, "ComputeMLWF: Utotal");
-      Uloc = (double **) Malloc(sizeof(double *)*2*max_rounds, "ComputeMLWF: Uloc");
-      //Uround = &Utotal[ProcRank*AllocNum*2*max_rounds];
-      for (round=0;round<max_rounds;round++) {
-        Uloc[2*round] = &Uround[AllocNum*(2*round)];
-        Uloc[2*round+1] = &Uround[AllocNum*(2*round+1)];
-        for (l=0;l<AllocNum;l++) {
-          Uloc[2*round][l] = gsl_matrix_get(U,l,2*(ProcRank*max_rounds+round));
-          Uloc[2*round+1][l] = gsl_matrix_get(U,l,2*(ProcRank*max_rounds+round)+1);
-          //fprintf(stderr,"(%i) (%i, 0/1) U_loc1 %e\tU_loc2 %e\n",P->Par.me, l, Uloc[l+AllocNum*0],Uloc[l+AllocNum*1]);
+      // Shuffling of these round's columns to prepare next rotation set
+      for (k=0;k<DiagData->NumMatrices+DiagData->extra;k++) {// one extra for B matrix ! 
+        // extract columns from A
+        //debug(P,"extract columns from A");
+        MerryGoRoundColumns(*(DiagData->comm), Aloc[k], DiagData->AllocNum, max_rounds, k, tagS0, tagS1, tagR0, tagR1);
+                  
+      }       
+      // and also for V ...
+      //debug(P,"extract columns from U");
+      MerryGoRoundColumns(*(DiagData->comm), Uloc, DiagData->AllocNum, max_rounds, 0, tagS0, tagS1, tagR0, tagR1);
+
+
+      // and merry-go-round for the indices too            
+      //debug(P,"and merry-go-round for the indices too");
+      MerryGoRoundIndices(DiagData->top, DiagData->bot, DiagData->AllocNum/2);
+    }
+
+    //fprintf(stderr,"(%i) STEP 4\n",P->Par.me);
+    // STEP 4: calculate new variance: \sum_{ik} (A^{(k)}_ii)^2
+    old_spread = Spread;
+    spread = 0.;
+    for(k=0;k<DiagData->NumMatrices;k++) {   // go through all self-adjoint operators
+      for (i=0; i < 2*max_rounds; i++) {  // go through all wave functions
+          spread += Aloc[k][i][i+DiagData->ProcRank*2*max_rounds]*Aloc[k][i][i+DiagData->ProcRank*2*max_rounds];
+          //spread += gsl_matrix_get(A[k],i,i)*gsl_matrix_get(A[k],i,i);
+      }
+    }
+    MPI_Allreduce(&spread, &Spread, 1, MPI_DOUBLE, MPI_SUM, *(DiagData->comm));
+    //Spread = spread;
+    if (P->Par.me == 0) { 
+      //if(P->Call.out[ReadOut]) 
+      //  fprintf(stderr,"(%i) STEP 5: %2.9e - %2.9e <= %lg ?\n",P->Par.me,old_spread,Spread,R->EpsWannier);
+      //else 
+        //fprintf(stderr,"%2.9e\n",Spread);
+    }
+    // STEP 5: check change of variance
+  } while (fabs(old_spread-Spread) >= R->EpsWannier);
+  // end of iterative diagonalization loop: We have found our final orthogonal U!
+
+  // gather local parts of U into complete matrix 
+  for (l=0;l<DiagData->ProcNum;l++)
+    rcounts[l] = DiagData->AllocNum;
+  debug(P,"allgather U");
+  for (round=0;round<2*max_rounds;round++) {
+    for (l=0;l<DiagData->ProcNum;l++)
+      rdispls[l] = (l*2*max_rounds + round)*DiagData->AllocNum;
+    MPI_Allgatherv(Uloc[round], DiagData->AllocNum, MPI_DOUBLE, Utotal, rcounts, rdispls, MPI_DOUBLE, *(DiagData->comm));
+  }
+  for (k=0;k<DiagData->AllocNum;k++) {    
+    for(l=0;l<DiagData->AllocNum;l++) {
+      gsl_matrix_set(DiagData->U,k,l, Utotal[l+k*DiagData->AllocNum]);
+    }
+  }
+
+  // after one set, gather A[k] from all and calculate spread
+  for (l=0;l<DiagData->ProcNum;l++)
+    rcounts[l] = DiagData->AllocNum;
+  debug(P,"gather A[k] for spread");
+  for (u=0;u<DiagData->NumMatrices+DiagData->extra;u++) {// extra for B matrix !
+    debug(P,"A[k] all gather");
+    for (round=0;round<2*max_rounds;round++) {
+      for (l=0;l<DiagData->ProcNum;l++)
+        rdispls[l] = (l*2*max_rounds + round)*DiagData->AllocNum;
+      MPI_Allgatherv(Aloc[u][round], DiagData->AllocNum, MPI_DOUBLE, Atotal[u], rcounts, rdispls, MPI_DOUBLE, *(DiagData->comm));
+    }
+    for (k=0;k<DiagData->AllocNum;k++) {    
+      for(l=0;l<DiagData->AllocNum;l++) {
+        gsl_matrix_set(DiagData->A[u],k,l, Atotal[u][l+k*DiagData->AllocNum]);
+      }
+    }
+  }
+   
+  // free eigenvector stuff     
+  gsl_vector_free(h);
+  gsl_matrix_free(G);
+  //gsl_eigen_symmv_free(w);  
+  gsl_vector_free(eval);
+  gsl_matrix_free(evec);
+ 
+  // Free column vectors
+  for (k=0;k<DiagData->NumMatrices+DiagData->extra;k++) {
+    Free(Atotal[k], "ParallelDiagonalization: Atotal[k]");
+    Free(Around[k], "ParallelDiagonalization: Around[k]");
+  }
+  Free(Uround, "ParallelDiagonalization: Uround");
+  Free(Utotal, "ParallelDiagonalization: Utotal");
+  Free(c_all, "ParallelDiagonalization: c_all");
+  Free(s_all, "ParallelDiagonalization: s_all");
+  Free(c, "ParallelDiagonalization: c");
+  Free(s, "ParallelDiagonalization: s");
+  Free(rcounts, "ParallelDiagonalization: rcounts");
+  Free(rdispls, "ParallelDiagonalization: rdispls");
+}
+/*
+ * \param **A matrices (pointer array with \a NumMatrices entries) to be diagonalized
+ * \param *U transformation matrix set to unity matrix
+ * \param NumMatrices number of matrices to be diagonalized
+ * \param extra number of additional matrices the rotation is applied to however which actively diagonalized (follow in \a **A)
+ * \param AllocNum number of rows/columns in matrices
+ * \param Num number of wave functions
+ * \param *top array with top row indices (merry-go-round)
+ * \param *bot array with bottom row indices (merry-go-round) 
+ */
+
+/** Simultaneous Diagonalization of variances matrices with one cpu.
+ * \param *P Problem at hand
+ * \param *DiagData pointer to structure DiagonalizationData containing necessary information for diagonalization
+ * \note this is faster given one cpu only than ParallelDiagonalization()
+ */
+void SerialDiagonalization(struct Problem *P, struct DiagonalizationData *DiagData)
+{
+  struct RunStruct *R = &P->R;
+  gsl_matrix *G;
+  gsl_vector *h;
+  gsl_vector *eval;
+  gsl_matrix *evec;
+  //gsl_eigen_symmv_workspace *w;
+  double *c,*s,a_i,a_j;
+  int it_steps, set, ProcRank;
+  int i,j,k,l,m;
+  double spread = 0., old_spread = 0.;\
+
+  // allocate eigenvector stuff
+  debug(P,"allocate eigenvector stuff");
+  G = gsl_matrix_calloc (2,2);
+  h = gsl_vector_alloc (2);
+  eval = gsl_vector_alloc (2);
+  evec = gsl_matrix_alloc (2,2);
+  //w = gsl_eigen_symmv_alloc(2);
+
+  c = (double *) Malloc(sizeof(double), "ComputeMLWF: c");
+  s = (double *) Malloc(sizeof(double), "ComputeMLWF: s");
+  debug(P,"now comes the iteration loop");
+  it_steps=0;
+  do {
+    it_steps++;
+    //if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 3: Iteratively maximize negative spread part\n",P->Par.me);
+    //if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) Beginning iteration %i ... ",P->Par.me,it_steps);
+    for (set=0; set < DiagData->AllocNum-1; set++) { // one column less due to column 0 stating at its place all the time
+      //fprintf(stderr,"(%i) Beginning rotation set %i ...\n",P->Par.me,set);
+      // STEP 3: for all index pairs 0<= i<j <AllocNum
+      for (ProcRank=0;ProcRank<DiagData->AllocNum/2;ProcRank++) {
+        // get indices
+        i = DiagData->top[ProcRank] < DiagData->bot[ProcRank] ? DiagData->top[ProcRank] : DiagData->bot[ProcRank]; // minimum of the two indices
+        j = DiagData->top[ProcRank] > DiagData->bot[ProcRank] ? DiagData->top[ProcRank] : DiagData->bot[ProcRank]; // maximum of the two indices: thus j >  i 
+        //fprintf(stderr,"(%i),(%i,%i) STEP 3a\n",P->Par.me,i,j);
+        // STEP 3a: Calculate G
+        gsl_matrix_set_zero(G);
+        
+        for (k=0;k<DiagData->NumMatrices;k++) { // go through all operators ...
+          // Calculate vector h(a) = [a_ii - a_jj, a_ij + a_ji, i(a_ji - a_ij)]
+          //fprintf(stderr,"(%i) k%i [a_ii - a_jj] = %e - %e = %e\n",P->Par.me, k,gsl_matrix_get(DiagData->A[k],i,i), gsl_matrix_get(DiagData->A[k],j,j),gsl_matrix_get(DiagData->A[k],i,i) - gsl_matrix_get(DiagData->A[k],j,j));          
+          //fprintf(stderr,"(%i) k%i [a_ij + a_ji] = %e + %e = %e\n",P->Par.me, k,gsl_matrix_get(DiagData->A[k],i,j), gsl_matrix_get(DiagData->A[k],j,i),gsl_matrix_get(DiagData->A[k],i,j) + gsl_matrix_get(DiagData->A[k],j,i));          
+          gsl_vector_set(h, 0, gsl_matrix_get(DiagData->A[k],i,i) - gsl_matrix_get(DiagData->A[k],j,j));
+          gsl_vector_set(h, 1, gsl_matrix_get(DiagData->A[k],i,j) + gsl_matrix_get(DiagData->A[k],j,i));
+          //gsl_vector_complex_set(h, 2, gsl_complex_mul_imag(gsl_complex_add(gsl_matrix_complex_get(A[k],j,i), gsl_matrix_complex_get(A[k],i,j)),1));
+          
+          // Calculate G = Re[ \sum_k h^H (A^{(k)}) h(A^{(k)}) ]
+          for (l=0;l<2;l++)
+            for (m=0;m<2;m++)
+                gsl_matrix_set(G,l,m, gsl_vector_get(h,l) * gsl_vector_get(h,m) + gsl_matrix_get(G,l,m));
+                
+          //PrintGSLMatrix(P,G,2, "G");
         }
-      }
-      // now comes the iteration loop
-      //debug(P,"now comes the iteration loop");
-      it_steps = 0;
-      do {
-        it_steps++;
-        fprintf(stderr,"(%i) Beginning parallel iteration %i ... ",P->Par.me,it_steps);
-        for (set=0; set < AllocNum-1; set++) { // one column less due to column 0 stating at its place all the time
-          //fprintf(stderr,"(%i) Beginning rotation set %i ...\n",P->Par.me,set);
-          for (round = 0; round < max_rounds;round++) {
-            start = ProcRank * max_rounds + round;
-            // get indices
-            i = top[start] < bot[start] ? top[start] : bot[start]; // minimum of the two indices
-            iloc = top[start] < bot[start] ? 0 : 1; 
-            j = top[start] > bot[start] ? top[start] : bot[start]; // maximum of the two indices: thus j >  i
-            jloc =  top[start] > bot[start] ? 0 : 1;
-            //fprintf(stderr,"(%i) my (%i,%i), loc(%i,%i)\n",P->Par.me, i,j, iloc, jloc);
-            
-            // calculate rotation angle, i.e. c and s
-            //fprintf(stderr,"(%i),(%i,%i) calculate rotation angle\n",P->Par.me,i,j);
-            gsl_matrix_set_zero(G);        
-            for (k=0;k<max_operators;k++) { // go through all operators ...
-              // Calculate vector h(a) = [a_ii - a_jj, a_ij + a_ji, i(a_ji - a_ij)]         
-              //fprintf(stderr,"(%i) k%i [a_ii - a_jj] = %e - %e = %e\n",P->Par.me, k,Aloc[k][2*round+iloc][i], Aloc[k][2*round+jloc][j],Aloc[k][2*round+iloc][i] - Aloc[k][2*round+jloc][j]);          
-              //fprintf(stderr,"(%i) k%i [a_ij + a_ji] = %e - %e = %e\n",P->Par.me, k,Aloc[k][2*round+jloc][i], Aloc[k][2*round+iloc][j],Aloc[k][2*round+jloc][i] + Aloc[k][2*round+iloc][j]);          
-              gsl_vector_set(h, 0, Aloc[k][2*round+iloc][i] - Aloc[k][2*round+jloc][j]);
-              gsl_vector_set(h, 1, Aloc[k][2*round+jloc][i] + Aloc[k][2*round+iloc][j]);
-              
-              // Calculate G = Re[ \sum_k h^H (A^{(k)}) h(A^{(k)}) ]
-              for (l=0;l<2;l++)
-                for (m=0;m<2;m++)
-                    gsl_matrix_set(G,l,m, gsl_vector_get(h,l) * gsl_vector_get(h,m) + gsl_matrix_get(G,l,m));
-            }
-            //fprintf(stderr,"(%i),(%i,%i) STEP 3b\n",P->Par.me,i,j);
-            // STEP 3b: retrieve eigenvector which belongs to greatest eigenvalue of G
-            gsl_eigen_symmv(G, eval, evec, w);  // calculates eigenvalues and eigenvectors of G    
-            index = gsl_vector_max_index (eval);  // get biggest eigenvalue
-            x = gsl_matrix_get(evec, 0, index);
-            y = gsl_matrix_get(evec, 1, index) * x/fabs(x);
-            x = fabs(x);  // ensure x>=0 so that rotation angles remain smaller Pi/4     
-            //fprintf(stderr,"(%i),(%i,%i) STEP 3c\n",P->Par.me,i,j);
-            // STEP 3c: calculate R = [[c,s^\ast],[-s,c^\ast]]
-            r = sqrt(x*x + y*y);
-            c[round] = sqrt((x + r) / (2*r));
-            s[round] = y / sqrt(2*r*(x+r));
-            // [[c,s],[-s,c]]= V_small
-            //fprintf(stderr,"(%i),(%i,%i) COS %e\t SIN %e\n",P->Par.me,i,j,c[round],s[round]);
-    
-            //fprintf(stderr,"(%i),(%i,%i) STEP 3e\n",P->Par.me,i,j);
-            // V_loc = V_loc * V_small
-            //debug(P,"apply rotation to local U");
-            for (l=0;l<AllocNum;l++) {
-              a_i = Uloc[2*round+iloc][l];
-              a_j = Uloc[2*round+jloc][l];
-              Uloc[2*round+iloc][l] =  c[round] * a_i + s[round] * a_j;
-              Uloc[2*round+jloc][l] = -s[round] * a_i + c[round] * a_j;
-            }
-          }  // end of round        
-          // circulate the rotation angles
-          //debug(P,"circulate the rotation angles");
-          MPI_Allgather(c, max_rounds, MPI_DOUBLE, c_all, max_rounds, MPI_DOUBLE, *comm);   // MPI_Allgather is waaaaay faster than ring circulation
-          MPI_Allgather(s, max_rounds, MPI_DOUBLE, s_all, max_rounds, MPI_DOUBLE, *comm);
-          //m = start;
-          for (l=0;l<AllocNum/2;l++) { // for each process
-            // we have V_small from process k
-            //debug(P,"Apply V_small from other process");
-            i = top[l] < bot[l] ? top[l] : bot[l]; // minimum of the two indices
-            j = top[l] > bot[l] ? top[l] : bot[l]; // maximum of the two indices: thus j >  i
-            iloc = top[l] < bot[l] ? 0 : 1; 
-            jloc =  top[l] > bot[l] ? 0 : 1;
-            for (m=0;m<max_rounds;m++) {
-              //fprintf(stderr,"(%i) %i processes' (%i,%i)\n",P->Par.me, m,i,j);
-              // apply row rotation to each A[k]
-              for (k=0;k<max_operators+1;k++) {// one extra for B matrix !
-                //fprintf(stderr,"(%i) A:(k%i) a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, i, Aloc[k][2*m+iloc][i],j,Aloc[k][2*m+iloc][j]);
-                //fprintf(stderr,"(%i) A:(k%i) a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, i, Aloc[k][2*m+jloc][i],j,Aloc[k][2*m+jloc][j]);
-                a_i = Aloc[k][2*m+iloc][i];
-                a_j = Aloc[k][2*m+iloc][j];
-                Aloc[k][2*m+iloc][i] =  c_all[l] * a_i + s_all[l] * a_j;
-                Aloc[k][2*m+iloc][j] = -s_all[l] * a_i + c_all[l] * a_j;
-                a_i = Aloc[k][2*m+jloc][i];
-                a_j = Aloc[k][2*m+jloc][j];
-                Aloc[k][2*m+jloc][i] =  c_all[l] * a_i + s_all[l] * a_j;
-                Aloc[k][2*m+jloc][j] = -s_all[l] * a_i + c_all[l] * a_j;
-                //fprintf(stderr,"(%i) A^%i: a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, i, Aloc[k][2*m+iloc][i],j,Aloc[k][2*m+iloc][j]);
-                //fprintf(stderr,"(%i) A^%i: a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, i, Aloc[k][2*m+jloc][i],j,Aloc[k][2*m+jloc][j]);
-              }
-            }
+      
+        //fprintf(stderr,"(%i),(%i,%i) STEP 3b\n",P->Par.me,i,j);
+        // STEP 3b: retrieve eigenvector which belongs to greatest eigenvalue of G
+        EigensolverFor22Matrix(P,G,eval,evec);
+        //gsl_eigen_symmv(G, eval, evec, w);  // calculates eigenvalues and eigenvectors of G
+
+        //PrintGSLMatrix(P, evec, 2, "Eigenvectors");
+        CalculateRotationAnglesFromEigenvalues(evec, eval, c, s);
+        //fprintf(stderr,"(%i),(%i,%i) COS %e\t SIN %e\n",P->Par.me,i,j,c[0],s[0]);
+     
+        //fprintf(stderr,"(%i),(%i,%i) STEP 3d\n",P->Par.me,i,j);
+        // STEP 3d: apply rotation R to rows and columns of A^{(k)}
+        for (k=0;k<DiagData->NumMatrices+DiagData->extra;k++) {// one extra for B matrix !
+          //PrintGSLMatrix(P,A[k],AllocNum, "A (before rot)");
+          for (l=0;l<DiagData->AllocNum;l++) {
+            // Rows
+            a_i = gsl_matrix_get(DiagData->A[k],i,l);
+            a_j = gsl_matrix_get(DiagData->A[k],j,l);
+            gsl_matrix_set(DiagData->A[k], i, l,  c[0] * a_i + s[0] * a_j);
+            gsl_matrix_set(DiagData->A[k], j, l, -s[0] * a_i + c[0] * a_j);
           }
-          // apply rotation to local operator matrices
-          // A_loc = A_loc * V_small 
-          //debug(P,"apply rotation to local operator matrices A[k]");
-          for (m=0;m<max_rounds;m++) {
-            start = ProcRank * max_rounds + m;
-            iloc = top[start] < bot[start] ? 0 : 1; 
-            jloc =  top[start] > bot[start] ? 0 : 1;
-            for (k=0;k<max_operators+1;k++) {// one extra for B matrix !
-              for (l=0;l<AllocNum;l++) {
-                // Columns, i and j belong to this process only!
-                a_i = Aloc[k][2*m+iloc][l];
-                a_j = Aloc[k][2*m+jloc][l];
-                Aloc[k][2*m+iloc][l] =  c[m] * a_i + s[m] * a_j;
-                Aloc[k][2*m+jloc][l] = -s[m] * a_i + c[m] * a_j;
-                //fprintf(stderr,"(%i) A:(k%i) a_i[%i] %e\ta_j[%i] %e\n",P->Par.me, k, l, Aloc[k][2*m+iloc][l],l,Aloc[k][2*m+jloc][l]);
-              }
-            }
+          for (l=0;l<DiagData->AllocNum;l++) {
+            // Columns
+            a_i = gsl_matrix_get(DiagData->A[k],l,i);
+            a_j = gsl_matrix_get(DiagData->A[k],l,j);
+            gsl_matrix_set(DiagData->A[k], l, i,  c[0] * a_i + s[0] * a_j);
+            gsl_matrix_set(DiagData->A[k], l, j, -s[0] * a_i + c[0] * a_j);
           }
-          // Shuffling of these round's columns to prepare next rotation set
-          for (k=0;k<max_operators+1;k++) {// one extra for B matrix ! 
-            // extract columns from A
-            //debug(P,"extract columns from A");
-            shiftcolumns(*comm, Aloc[k], AllocNum, max_rounds, k, tagS0, tagS1, tagR0, tagR1);
-                      
-          }       
-          // and also for V ...
-          //debug(P,"extract columns from U");
-          shiftcolumns(*comm, Uloc, AllocNum, max_rounds, 0, tagS0, tagS1, tagR0, tagR1);
-
-    
-          // and merry-go-round for the indices too            
-          //debug(P,"and merry-go-round for the indices too");
-          music(top, bot, AllocNum/2);
+          //PrintGSLMatrix(P,A[k],AllocNum, "A (after rot)");
         }
-  
-        //fprintf(stderr,"(%i) STEP 4\n",P->Par.me);
-        // STEP 4: calculate new variance: \sum_{ik} (A^{(k)}_ii)^2
-        old_spread = Spread;
-        spread = 0.;
-        for(k=0;k<max_operators;k++) {   // go through all self-adjoint operators
-          for (i=0; i < 2*max_rounds; i++) {  // go through all wave functions
-              spread += Aloc[k][i][i+ProcRank*2*max_rounds]*Aloc[k][i][i+ProcRank*2*max_rounds];
-              //spread += gsl_matrix_get(A[k],i,i)*gsl_matrix_get(A[k],i,i);
-          }
+        //fprintf(stderr,"(%i),(%i,%i) STEP 3e\n",P->Par.me,i,j);
+        //PrintGSLMatrix(P,DiagData->U,DiagData->AllocNum, "U (before rot)");
+        // STEP 3e: apply U = R*U
+        for (l=0;l<DiagData->AllocNum;l++) {
+            a_i = gsl_matrix_get(DiagData->U,i,l);
+            a_j = gsl_matrix_get(DiagData->U,j,l);
+            gsl_matrix_set(DiagData->U, i, l,  c[0] * a_i + s[0] * a_j);
+            gsl_matrix_set(DiagData->U, j, l, -s[0] * a_i + c[0] * a_j);
         }
-        MPI_Allreduce(&spread, &Spread, 1, MPI_DOUBLE, MPI_SUM, *comm);
-        //Spread = spread;
-        if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 5: %2.9e - %2.9e <= %lg ?\n",P->Par.me,old_spread,Spread,R->EpsWannier);
-        else fprintf(stderr,"%2.9e\n",Spread);
-        // STEP 5: check change of variance
-      } while (fabs(old_spread-Spread) >= R->EpsWannier);
-      // end of iterative diagonalization loop: We have found our final orthogonal U!
-
-      for (l=0;l<ProcNum;l++)
-        rcounts[l] = AllocNum;
-      //debug(P,"allgather U");
-      for (round=0;round<2*max_rounds;round++) {
-        for (l=0;l<ProcNum;l++)
-          rdispls[l] = (l*2*max_rounds + round)*AllocNum;
-        MPI_Allgatherv(Uloc[round], AllocNum, MPI_DOUBLE, Utotal, rcounts, rdispls, MPI_DOUBLE, *comm);
-      }
-      for (k=0;k<AllocNum;k++) {    
-        for(l=0;l<AllocNum;l++) {
-          gsl_matrix_set(U,k,l, Utotal[l+k*AllocNum]);
-        }
-      }
-
-
-      // after one set, gather A[k] from all and calculate spread
-      for (l=0;l<ProcNum;l++)
-        rcounts[l] = AllocNum;
-      //debug(P,"gather A[k] for spread");
-      for (u=0;u<max_operators+1;u++) {// one extra for B matrix !
-        //debug(P,"A[k] all gather");
-        for (round=0;round<2*max_rounds;round++) {
-          for (l=0;l<ProcNum;l++)
-            rdispls[l] = (l*2*max_rounds + round)*AllocNum;
-          MPI_Allgatherv(Aloc[u][round], AllocNum, MPI_DOUBLE, Atotal[u], rcounts, rdispls, MPI_DOUBLE, *comm);
-        }
-        for (k=0;k<AllocNum;k++) {    
-          for(l=0;l<AllocNum;l++) {
-            gsl_matrix_set(A[u],k,l, Atotal[u][l+k*AllocNum]);
-          }
-        }
-      }
-     
-      // free eigenvector stuff     
-      gsl_vector_free(h);
-      gsl_matrix_free(G);
-      gsl_eigen_symmv_free(w);  
-      gsl_vector_free(eval);
-      gsl_matrix_free(evec);
-      // Free column vectors
-      for (k=0;k<max_operators+1;k++) {
-        Free(Atotal[k], "bla");
-        Free(Around[k], "bla");
-      }
-      Free(Uround, "bla");
-      Free(Utotal, "bla");
-      Free(c_all, "bla");
-      Free(s_all, "bla");
-      Free(c, "bla");
-      Free(s, "bla");
-      Free(rcounts, "bla");
-      Free(rdispls, "bla");
-      
-    } else {
-      
-      c = (double *) Malloc(sizeof(double), "ComputeMLWF: c");
-      s = (double *) Malloc(sizeof(double), "ComputeMLWF: s");
-      G = gsl_matrix_calloc (2,2);
-      h = gsl_vector_alloc (2);
-      eval = gsl_vector_alloc (2);
-      evec = gsl_matrix_alloc (2,2);
-      w = gsl_eigen_symmv_alloc(2);
-      //debug(P,"now comes the iteration loop");
-      it_steps=0;
-      do {
-        it_steps++;
-        //if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 3: Iteratively maximize negative spread part\n",P->Par.me);
-        fprintf(stderr,"(%i) Beginning iteration %i ... ",P->Par.me,it_steps);
-        for (set=0; set < AllocNum-1; set++) { // one column less due to column 0 stating at its place all the time
-          //fprintf(stderr,"(%i) Beginning rotation set %i ...\n",P->Par.me,set);
-          // STEP 3: for all index pairs 0<= i<j <AllocNum
-          for (ProcRank=0;ProcRank<AllocNum/2;ProcRank++) {
-            // get indices
-            i = top[ProcRank] < bot[ProcRank] ? top[ProcRank] : bot[ProcRank]; // minimum of the two indices
-            j = top[ProcRank] > bot[ProcRank] ? top[ProcRank] : bot[ProcRank]; // maximum of the two indices: thus j >  i 
-            //fprintf(stderr,"(%i),(%i,%i) STEP 3a\n",P->Par.me,i,j);
-            // STEP 3a: Calculate G
-            gsl_matrix_set_zero(G);
-            
-            for (k=0;k<max_operators;k++) { // go through all operators ...
-              // Calculate vector h(a) = [a_ii - a_jj, a_ij + a_ji, i(a_ji - a_ij)]
-              //fprintf(stderr,"(%i) k%i [a_ii - a_ij] = %e - %e = %e\n",P->Par.me, k,gsl_matrix_get(A[k],i,i), gsl_matrix_get(A[k],j,j),gsl_matrix_get(A[k],i,i) - gsl_matrix_get(A[k],j,j));          
-              //fprintf(stderr,"(%i) k%i [a_ij + a_jij] = %e - %e = %e\n",P->Par.me, k,gsl_matrix_get(A[k],i,j), gsl_matrix_get(A[k],j,i),gsl_matrix_get(A[k],i,j) + gsl_matrix_get(A[k],j,i));          
-              gsl_vector_set(h, 0, gsl_matrix_get(A[k],i,i) - gsl_matrix_get(A[k],j,j));
-              gsl_vector_set(h, 1, gsl_matrix_get(A[k],i,j) + gsl_matrix_get(A[k],j,i));
-              //gsl_vector_complex_set(h, 2, gsl_complex_mul_imag(gsl_complex_add(gsl_matrix_complex_get(A[k],j,i), gsl_matrix_complex_get(A[k],i,j)),1));
-              
-              // Calculate G = Re[ \sum_k h^H (A^{(k)}) h(A^{(k)}) ]
-              for (l=0;l<2;l++)
-                for (m=0;m<2;m++)
-                    gsl_matrix_set(G,l,m, gsl_vector_get(h,l) * gsl_vector_get(h,m) + gsl_matrix_get(G,l,m));
-            }
-          
-            //fprintf(stderr,"(%i),(%i,%i) STEP 3b\n",P->Par.me,i,j);
-            // STEP 3b: retrieve eigenvector which belongs to greatest eigenvalue of G
-            gsl_eigen_symmv(G, eval, evec, w);  // calculates eigenvalues and eigenvectors of G
-        
-            index = gsl_vector_max_index (eval);  // get biggest eigenvalue
-            x = gsl_matrix_get(evec, 0, index);
-            y = gsl_matrix_get(evec, 1, index) * x/fabs(x);
-            //z = gsl_matrix_get(evec, 2, index) * x/fabs(x);
-            x = fabs(x);  // ensure x>=0 so that rotation angles remain smaller Pi/4
-          
-            //fprintf(stderr,"(%i),(%i,%i) STEP 3c\n",P->Par.me,i,j);
-            // STEP 3c: calculate R = [[c,s^\ast],[-s,c^\ast]]
-            r = sqrt(x*x + y*y); // + z*z);
-            c[0] = sqrt((x + r) / (2*r));
-            s[0] = y / sqrt(2*r*(x+r)); //, -z / sqrt(2*r*(x+r)));
-            //fprintf(stderr,"(%i),(%i,%i) COS %e\t SIN %e\n",P->Par.me,i,j,c[0],s[0]);
-          
-            //fprintf(stderr,"(%i),(%i,%i) STEP 3d\n",P->Par.me,i,j);
-            // STEP 3d: apply rotation R to rows and columns of A^{(k)}
-            for (k=0;k<max_operators+1;k++) {// one extra for B matrix !
-              for (l=0;l<AllocNum;l++) {
-                // Rows
-                a_i = gsl_matrix_get(A[k],i,l);
-                a_j = gsl_matrix_get(A[k],j,l);
-                gsl_matrix_set(A[k], i, l,  c[0] * a_i + s[0] * a_j);
-                gsl_matrix_set(A[k], j, l, -s[0] * a_i + c[0] * a_j);
-              }
-              for (l=0;l<AllocNum;l++) {
-                // Columns
-                a_i = gsl_matrix_get(A[k],l,i);
-                a_j = gsl_matrix_get(A[k],l,j);
-                gsl_matrix_set(A[k], l, i,  c[0] * a_i + s[0] * a_j);
-                gsl_matrix_set(A[k], l, j, -s[0] * a_i + c[0] * a_j);
-              }
-            }
-            //fprintf(stderr,"(%i),(%i,%i) STEP 3e\n",P->Par.me,i,j);
-            // STEP 3e: apply U = R*U
-            for (l=0;l<AllocNum;l++) {
-                a_i = gsl_matrix_get(U,i,l);
-                a_j = gsl_matrix_get(U,j,l);
-                gsl_matrix_set(U, i, l,  c[0] * a_i + s[0] * a_j);
-                gsl_matrix_set(U, j, l, -s[0] * a_i + c[0] * a_j);
-            }
-          }
-          // and merry-go-round for the indices too            
-          //debug(P,"and merry-go-round for the indices too");
-          if (AllocNum > 2) music(top, bot, AllocNum/2);
-        }
-       
-        //if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 4\n",P->Par.me);
-        // STEP 4: calculate new variance: \sum_{ik} (A^{(k)}_ii)^2
-        old_spread = spread;
-        spread = 0;
-        for(k=0;k<max_operators;k++) {   // go through all self-adjoint operators
-          for (i=0; i < AllocNum; i++) {  // go through all wave functions
-              spread += pow(gsl_matrix_get(A[k],i,i),2);
-          }
-        }
-        if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 5: %2.9e - %2.9e <= %lg ?\n",P->Par.me,old_spread,spread,R->EpsWannier);
-        else fprintf(stderr,"%2.9e\n",spread);
-        // STEP 5: check change of variance
-      } while (fabs(old_spread-spread) >= R->EpsWannier);
-      // end of iterative diagonalization loop: We have found our final orthogonal U!
-      gsl_vector_free(h);
-      gsl_matrix_free(G);
-      gsl_eigen_symmv_free(w);  
-      gsl_vector_free(eval);
-      gsl_matrix_free(evec);
-      Free(c, "bla");
-      Free(s, "bla");
-    }  
-    
-    if(P->Call.out[ReadOut]) {// && P->Par.me == 0) { 
-      //debug(P,"output total U");
-      fprintf(stderr,"(%i) U_tot = \n",P->Par.me);
-      for (k=0;k<Num;k++) {    
-        for (l=0;l<Num;l++)
-          fprintf(stderr,"%e\t",gsl_matrix_get(U,l,k));
-        fprintf(stderr,"\n");
-      }
-    }
-  }
-  Free(top, "bla");
-  Free(bot, "bla");
-
-  if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 6: Allocating buffer mem\n",P->Par.me);
-  // STEP 6: apply transformation U to all wave functions \sum_i^Num U_ji | \phi_i \rangle = | \phi_j^\ast \rangle
-  Num = Psi->TypeStartIndex[type+1] - Psi->TypeStartIndex[type]; // recalc Num as we can only work with local Psis from here
-  fftw_complex **coeffs_buffer = Malloc(sizeof(fftw_complex *)*Num, "ComputeMLWF: **coeffs_buffer");
-  for (l=0;l<Num;l++) // allocate for each local wave function
-    coeffs_buffer[l] = LevS->LPsi->OldLocalPsi[l];
-  
-  if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 6: Transformation ...\n",P->Par.me);
-  l=-1; // to access U matrix element (0..Num-1)
-  k=-1; // to access the above swap coeffs_buffer (0..LocalNo-1)
-  for (i=0; i < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; i++) {  // go through all wave functions
-    OnePsiA = &Psi->AllPsiStatus[i];    // grab OnePsiA
-    if (OnePsiA->PsiType == type) {   // drop all but occupied ones
-      l++;  // increase l if it is occupied wave function
-      if (OnePsiA->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) { // local?
-        k++; // increase k only if it is a local, non-extra orbital wave function
-        LPsiDatA = (fftw_complex *) coeffs_buffer[k];    // new coeffs first go to copy buffer, old ones must not be overwritten yet
-        SetArrayToDouble0((double *)LPsiDatA, 2*LevS->MaxG);  // zero buffer part
-      } else
-        LPsiDatA = NULL;  // otherwise processes won't enter second loop, though they're supposed to send coefficients!
-
-      m = -1; // to access U matrix element (0..Num-1)
-      for (j=0; j < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; j++) {  // go through all wave functions
-        OnePsiB = &Psi->AllPsiStatus[j];    // grab OnePsiB
-        if (OnePsiB->PsiType == type) {   // drop all but occupied ones
-          m++;  // increase m if it is occupied wave function
-          if (OnePsiB->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // local?
-             LOnePsiB = &Psi->LocalPsiStatus[OnePsiB->MyLocalNo];
-          else 
-             LOnePsiB = NULL;
-          if (LOnePsiB == NULL) {   // if it's not local ... receive it from respective process into TempPsi
-            RecvSource = OnePsiB->my_color_comm_ST_Psi;
-            MPI_Recv( LevS->LPsi->TempPsi, LevS->MaxG*ElementSize, MPI_DOUBLE, RecvSource, WannierTag2, P->Par.comm_ST_PsiT, &status );
-            LPsiDatB=LevS->LPsi->TempPsi;
-          } else {                  // .. otherwise send it to all other processes (Max_me... - 1)
-            for (p=0;p<P->Par.Max_me_comm_ST_PsiT;p++)
-              if (p != OnePsiB->my_color_comm_ST_Psi) 
-                MPI_Send( LevS->LPsi->LocalPsi[OnePsiB->MyLocalNo], LevS->MaxG*ElementSize, MPI_DOUBLE, p, WannierTag2, P->Par.comm_ST_PsiT);
-            LPsiDatB=LevS->LPsi->LocalPsi[OnePsiB->MyLocalNo];
-          } // LPsiDatB is now set to the coefficients of OnePsi either stored or MPI_Received
-
-          if (LPsiDatA != NULL) {
-            double tmp = gsl_matrix_get(U,l,m);
-            g=0;
-            if (LevS->GArray[0].GSq == 0.0) {
-              LPsiDatA[g].re += LPsiDatB[g].re * tmp;
-              LPsiDatA[g].im += LPsiDatB[g].im * tmp;
-              g++;
-            }
-            for (; g < LevS->MaxG; g++) {
-              LPsiDatA[g].re += LPsiDatB[g].re * tmp;
-              LPsiDatA[g].im += LPsiDatB[g].im * tmp;
-            }
-          }
-        }
-      }
-    }
-  }
-  gsl_matrix_free(U);
-
-  if(P->Call.out[StepLeaderOut]) fprintf(stderr,"(%i) STEP 6: Swapping buffer mem\n",P->Par.me);
-  // now, as all wave functions are updated, swap the buffer
-  l = -1;
-  for (k=0;k<Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT;k++) { // go through each local occupied wave function
-    if (Psi->AllPsiStatus[k].PsiType == type && Psi->AllPsiStatus[k].my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) {
-      l++;
-      if(P->Call.out[StepLeaderOut]) fprintf(stderr,"(%i) (k:%i,l:%i) LocalNo = (%i,%i)\t AllPsiNo = (%i,%i)\n", P->Par.me, k,l,Psi->LocalPsiStatus[l].MyLocalNo, Psi->LocalPsiStatus[l].MyGlobalNo, Psi->AllPsiStatus[k].MyLocalNo, Psi->AllPsiStatus[k].MyGlobalNo);
-      LPsiDatA = (fftw_complex *)coeffs_buffer[l];
-      LPsiDatB = LevS->LPsi->LocalPsi[l];
-      for (g=0;g<LevS->MaxG;g++) {
-        LPsiDatB[g].re = LPsiDatA[g].re;
-        LPsiDatB[g].im = LPsiDatA[g].im;
-      }
-      // recalculating non-local form factors which are coefficient dependent!
-      CalculateNonLocalEnergyNoRT(P, Psi->LocalPsiStatus[l].MyLocalNo);
-    }
-  } 
-  // and free allocated buffer memory
-  Free(coeffs_buffer, "bla");
-
-  if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 7\n",P->Par.me);
-  // STEP 7: Compute Wannier centers, spread and printout
-  // the spread for x,y,z resides in the respective diagonal element of A_.. for each orbital
+        //PrintGSLMatrix(P,DiagData->U,DiagData->AllocNum, "U (after rot)");
+      }
+      // and merry-go-round for the indices too            
+      //debug(P,"and merry-go-round for the indices too");
+      if (DiagData->AllocNum > 2) MerryGoRoundIndices(DiagData->top, DiagData->bot, DiagData->AllocNum/2);
+    }
+   
+    //if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) STEP 4\n",P->Par.me);
+    // STEP 4: calculate new variance: \sum_{ik} (A^{(k)}_ii)^2
+    old_spread = spread;
+    spread = 0.;
+    for(k=0;k<DiagData->NumMatrices;k++) {   // go through all self-adjoint operators
+      for (i=0; i < DiagData->AllocNum; i++) {  // go through all wave functions
+          spread += pow(gsl_matrix_get(DiagData->A[k],i,i),2);
+      }
+    }
+    if(P->Par.me == 0) {
+      //if(P->Call.out[ReadOut]) 
+      //  fprintf(stderr,"(%i) STEP 5: %2.9e - %2.9e <= %lg ?\n",P->Par.me,old_spread,spread,R->EpsWannier);
+      //else 
+        //fprintf(stderr,"%2.9e\n",spread);
+    }
+    // STEP 5: check change of variance
+  } while (fabs(old_spread-spread) >= R->EpsWannier);
+  // end of iterative diagonalization loop: We have found our final orthogonal U!
+  Free(c, "SerialDiagonalization: c");
+  Free(s, "SerialDiagonalization: s");
+  // free eigenvector stuff     
+  gsl_vector_free(h);
+  gsl_matrix_free(G);
+  //gsl_eigen_symmv_free(w);  
+  gsl_vector_free(eval);
+  gsl_matrix_free(evec);
+}
+
+/** Writes the wannier centres and spread to file.
+ * Also puts centres from array into OnePsiElementAddData structure.
+ * \param *P Problem at hand
+ * \param old_spread first term of wannier spread
+ * \param spread second term of wannier spread
+ * \param **WannierCentre 2D array (NDIM, \a Num) with wannier centres
+ * \param *WannierSpread array with wannier spread per wave function
+ */
+void WriteWannierFile(struct Problem *P, double spread, double old_spread, double **WannierCentre, double *WannierSpread)
+{
+  struct FileData *F = &P->Files;
+  struct RunStruct *R = &P->R;
+  struct Lattice *Lat = &P->Lat;
+  struct Psis *Psi = &Lat->Psi;
+  struct OnePsiElement *OnePsiA;
+  char spin[12], suffix[18];
+  int i,j,l;
+
   if(P->Call.out[ReadOut]) fprintf(stderr,"(%i) Spread printout\n", P->Par.me);
 
   switch (Lat->Psi.PsiST) {
   case SpinDouble:
-    strncpy(suffix,".spread.csv",18);    
-    strncat(spin,"SpinDouble",12);
+    strcpy(suffix,".spread.csv");    
+    strcpy(spin,"SpinDouble");
     break;
   case SpinUp:
-    strncpy(suffix,".spread_up.csv",18);    
-    strncat(spin,"SpinUp",12);
+    strcpy(suffix,".spread_up.csv");    
+    strcpy(spin,"SpinUp");
     break;
   case SpinDown:
-    strncpy(suffix,".spread_down.csv",18);    
-    strncat(spin,"SpinDown",12);
+    strcpy(suffix,".spread_down.csv");    
+    strcpy(spin,"SpinDown");
     break;
   }
@@ -1042 +1814 @@
       OpenFile(P, &F->SpreadFile, suffix, "a", P->Call.out[ReadOut]); // only open on starting level
     if (F->SpreadFile == NULL) {
-      Error(SomeError,"ComputeMLWF: Error opening Wannier File!\n");
+      Error(SomeError,"WriteWannierFile: Error opening Wannier File!\n");
     } else {
       fprintf(F->SpreadFile,"#===== W A N N I E R  C E N T R E S for Level %d of type %s ========================\n", R->LevSNo, spin);
       fprintf(F->SpreadFile,"# Orbital+Level\tx\ty\tz\tSpread\n");
     }
-  }
-  old_spread = 0;
-  spread = 0;
-  i=-1;
-  for (l=0; l < Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT; l++) {  // go through all wave functions
-    OnePsiA = &Psi->AllPsiStatus[l];    // grab OnePsiA
-    if (OnePsiA->PsiType == type) {   // drop all but occupied ones
-      i++;  // increase l if it is occupied wave function
-      //fprintf(stderr,"(%i) Wannier for %i\n", P->Par.me, i);
-      
-      // calculate Wannier Centre
-      for (j=0;j<NDIM;j++) {
-        WannierCentre[i][j] = sqrt(Lat->RealBasisSQ[j])/(2*PI) * GSL_IMAG( gsl_complex_log( gsl_complex_rect(gsl_matrix_get(A[j*2],i,i),gsl_matrix_get(A[j*2+1],i,i))));
-        if (WannierCentre[i][j] < 0) // change wrap around of above operator to smooth 0...Lat->RealBasisSQ
-          WannierCentre[i][j] = sqrt(Lat->RealBasisSQ[j]) + WannierCentre[i][j];
-      }
-      
-      // store orbital spread and centre in file
-      tmp = - pow(gsl_matrix_get(A[0],i,i),2) - pow(gsl_matrix_get(A[1],i,i),2)
-            - pow(gsl_matrix_get(A[2],i,i),2) - pow(gsl_matrix_get(A[3],i,i),2)
-            - pow(gsl_matrix_get(A[4],i,i),2) - pow(gsl_matrix_get(A[5],i,i),2);          
-      WannierSpread[i] = gsl_matrix_get(A[max_operators],i,i) + tmp; 
-      //fprintf(stderr,"(%i) WannierSpread[%i] = %e\n", P->Par.me, i, WannierSpread[i]);
-      //if (P->Par.me == 0) fprintf(F->SpreadFile,"Orbital %d:\t Wannier center (x,y,z)=(%lg,%lg,%lg)\t Spread sigma^2 = %lg - %lg = %lg\n",
-        //Psi->AllPsiStatus[i].MyGlobalNo, WannierCentre[i][0], WannierCentre[i][1], WannierCentre[i][2], gsl_matrix_get(A[max_operators],i,i), -tmp, WannierSpread[i]); 
-      //if (P->Par.me == 0) fprintf(F->SpreadFile,"%e\t%e\t%e\n",
-        //WannierCentre[i][0], WannierCentre[i][1], WannierCentre[i][2]);
-      
-      // gather all spreads
-      old_spread += gsl_matrix_get(A[max_operators],i,i); // tr(U^H B U)
-      for (k=0;k<max_operators;k++)
-        spread += pow(gsl_matrix_get(A[k],i,i),2);
-
-      // store calculated Wannier centre
-      if (OnePsiA->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // is this local?
-        for (j=0;j<NDIM;j++)
-          Psi->AddData[OnePsiA->MyLocalNo].WannierCentre[j] = WannierCentre[i][j];
-    }
-  }
-  
-  // join Wannier orbital to groups with common centres under certain conditions 
-  switch (R->CommonWannier) {
-   case 4:
-    debug(P,"Shifting each Wannier centers to cell center");
-    for (i=0; i < Num; i++) {  // go through all occupied wave functions
-      for (j=0;j<NDIM;j++)
-        WannierCentre[i][j] = sqrt(Lat->RealBasisSQ[j])/2.; 
-    }    
-    break;
-   case 3:
-    debug(P,"Shifting Wannier centers individually to nearest grid point");
-    for (i=0;i < Num; i++) {  // go through all wave functions
-      for (j=0;j<NDIM;j++) {
-        tmp = WannierCentre[i][j]/sqrt(Lat->RealBasisSQ[j])*(double)N[j];
-        //fprintf(stderr,"(%i) N[%i]: %i\t tmp %e\t floor %e\t ceil %e\n",P->Par.me, j, N[j], tmp, floor(tmp), ceil(tmp));
-        if (fabs((double)floor(tmp) - tmp) < fabs((double)ceil(tmp) - tmp))
-          WannierCentre[i][j] = (double)floor(tmp)/(double)N[j]*sqrt(Lat->RealBasisSQ[j]); 
-        else
-          WannierCentre[i][j] = (double)ceil(tmp)/(double)N[j]*sqrt(Lat->RealBasisSQ[j]); 
-      }
-    }
-    break;
-   case 2:
-    debug(P,"Combining individual orbitals according to spread.");
-    //fprintf(stderr,"(%i) Finding multiple bindings and Reweighting Wannier centres\n",P->Par.me);
-    //debug(P,"finding partners");
-    marker = (int*) Malloc(sizeof(int)*(Num+1),"ComputeMLWF: marker");
-    group = (int**) Malloc(sizeof(int *)*Num,"ComputeMLWF: group");
-    for (l=0;l<Num;l++) {
-      group[l] = (int*) Malloc(sizeof(int)*(Num+1),"ComputeMLWF: group[l]");  // each must group must have one more as end marker
-      for (k=0;k<=Num;k++)
-        group[l][k] = -1; // reset partner group
-    }
-    for (k=0;k<Num;k++)
-      partner[k] = 0;
-    //debug(P,"mem allocated");
-    // go for each orbital through every other, check distance against the sum of both spreads
-    // if smaller add to group of this orbital
-    for (l=0;l<Num;l++) {
-      j=0;  // index for partner group
-      for (k=0;k<Num;k++) {  // check this against l
-        Spread = 0.;
-        for (i=0;i<NDIM;i++) {
-          //fprintf(stderr,"(%i) Spread += (%e - %e)^2 \n", P->Par.me, WannierCentre[l][i], WannierCentre[k][i]); 
-          Spread += (WannierCentre[l][i] - WannierCentre[k][i])*(WannierCentre[l][i] - WannierCentre[k][i]);
-        }
-        Spread = sqrt(Spread);  // distance in Spread
-        //fprintf(stderr,"(%i) %i to %i: distance %e, SpreadSum = %e + %e = %e \n", P->Par.me, l, k, Spread, WannierSpread[l], WannierSpread[k], WannierSpread[l]+WannierSpread[k]); 
-        if (Spread < 1.5*(WannierSpread[l]+WannierSpread[k])) {// if distance smaller than sum of spread
-          group[l][j++] = k;  // add k to group of l
-          partner[l]++;
-          //fprintf(stderr,"(%i) %i added as %i-th member to %i's group.\n", P->Par.me, k, j, l);
-        } 
-      }
-    }
-    
-    // consistency, for each orbital check if this orbital is also in the group of each referred orbital
-    //debug(P,"checking consistency");
-    totalflag = 1;
-    for (l=0;l<Num;l++) // checking l's group
-      for (k=0;k<Num;k++) { // k is partner index
-        if (group[l][k] != -1) {  // if current index k is a partner
-          flag = 0;
-          for(j=0;j<Num;j++) {  // go through each entry in l partner's partner group if l exists
-            if ((group[ group[l][k] ][j] == l))
-              flag = 1;
-          }
-          //if (flag == 0) fprintf(stderr, "(%i) in %i's group %i is referred as a partner, but not the other way round!\n", P->Par.me, l, group[l][k]);
-          if (totalflag == 1) totalflag = flag;
-        }
-      }
-    // for each orbital group (marker group) weight each center to a total and put this into the local WannierCentres
-    //debug(P,"weight and calculate new centers for partner groups");
-    for (l=0;l<=Num;l++)
-      marker[l] = 1;
-    if (totalflag) {
-      for (l=0;l<Num;l++) { // go through each orbital
-        if (marker[l] != 0) { // if it hasn't been reweighted
-          marker[l] = 0;
-          for (i=0;i<NDIM;i++) 
-            q[i] = 0.;
-          j = 0;
-          while (group[l][j] != -1) {
-            marker[group[l][j]] = 0;
-            for (i=0;i<NDIM;i++) {
-              //fprintf(stderr,"(%i) Adding to %i's group, %i entry of %i: %e\n", P->Par.me, l, i, group[l][j], WannierCentre[ group[l][j] ][i]);
-              q[i] += WannierCentre[ group[l][j] ][i];
-            } 
-            j++;
-          }
-          //fprintf(stderr,"(%i) %i's group: (%e,%e,%e)/%i = (%e,%e,%e)\n", P->Par.me, l, q[0], q[1], q[2], j, q[0]/(double)j, q[1]/(double)j, q[2]/(double)j);
-          for (i=0;i<NDIM;i++) {// weight by number of elements in partner group
-            q[i] /= (double)(j);
-          }
-  
-          // put WannierCentre into own and all partners'      
-          for (i=0;i<NDIM;i++)
-            WannierCentre[l][i] = q[i];
-          j = 0;  
-          while (group[l][j] != -1) {
-            for (i=0;i<NDIM;i++)
-              WannierCentre[group[l][j]][i] = q[i];
-            j++;
-          }
-        }
-      }
-    }
-    if (P->Call.out[StepLeaderOut]) {
-      fprintf(stderr,"Summary:\n");
-      fprintf(stderr,"========\n");
-      for (i=0;i<Num;i++)
-        fprintf(stderr,"%i belongs to a %i-ple binding.\n",i,partner[i]);
-    } 
-    //debug(P,"done");
-  
-    Free(marker, "bla");
-    for (l=0;l<Num;l++)
-      Free(group[l], "bla");
-    Free(group, "bla");
-    break;
-   case 1:
-    debug(P,"Individual orbitals are changed to center of all.");
-    for (i=0;i<NDIM;i++)  // zero center of weight
-      q[i] = 0.;
-    for (k=0;k<Num;k++)
-      for (i=0;i<NDIM;i++) {  // sum up all orbitals each component
-        q[i] += WannierCentre[k][i];
-      }
-    for (i=0;i<NDIM;i++)  // divide by number
-      q[i] /= Num;
-    for (k=0;k<Num;k++)
-      for (i=0;i<NDIM;i++) {  // put into this function's array
-        WannierCentre[k][i] = q[i];
-      }
-    break;
-   case 0:
-   default:
-   break;
   }
   
@@ -1244 +1838 @@
     fprintf(F->SpreadFile,"\n#Matrix traces\tB_ii\tA_ii^2\tTotal (B_ii - A_ii^2)\n");
     fprintf(F->SpreadFile,"TotalSpread_L%d\t%lg\t%lg\t%lg\n\n",R->LevSNo, old_spread, spread, old_spread - spread);
-  }
-  fflush(F->SpreadFile);
-
-  // and the spread was calculated in the loop above
-/*
-  i=-1;     
-  for (l=0;l<Psi->MaxPsiOfType+P->Par.Max_me_comm_ST_PsiT;l++)
-    if (Psi->AllPsiStatus[l].PsiType == type) {
-      i++;
-      spread = CalculateSpread(P,l);
-      tmp = gsl_matrix_get(A[max_operators],i,i) - pow(gsl_matrix_get(A[0],i,i),2) - pow(gsl_matrix_get(A[1],i,i),2)
-          - pow(gsl_matrix_get(A[2],i,i),2) - pow(gsl_matrix_get(A[3],i,i),2)
-          - pow(gsl_matrix_get(A[4],i,i),2) - pow(gsl_matrix_get(A[5],i,i),2);
-      if(P->Call.out[ValueOut]) fprintf(stderr, "(%i) Check of spread of %ith wave function: %lg against %lg\n",P->Par.me, i, Psi->AddData[i].WannierSpread, tmp);  
-    }*/
-  // free all remaining memory
-  for (k=0;k<max_operators+1;k++)
-    gsl_matrix_free(A[k]);
-}
+    fflush(F->SpreadFile);
+  }
+}
+
 
 /** Parses the spread file and puts values into OnePsiElementAddData#WannierCentre.
@@ -1280 +1859 @@
   double WannierCentre[NDIM+1]; // combined centre and spread
   MPI_Status status;
-  enum PsiTypeTag type = Occupied;
   int signal = 0; // 1 - ok, 0 - error
 
   switch (Lat->Psi.PsiST) {
   case SpinDouble:
-    strncpy(suffix,".spread.csv",18);    
+    strcpy(suffix,".spread.csv");    
     break;
   case SpinUp:
-    strncpy(suffix,".spread_up.csv",18);    
+    strcpy(suffix,".spread_up.csv");    
     break;
   case SpinDown:
-    strncpy(suffix,".spread_down.csv",18);    
+    strcpy(suffix,".spread_down.csv");    
     break;
   }
+  if (P->Call.out[NormalOut]) fprintf(stderr,"(%i) Parsing Wannier Centres from file ... \n", P->Par.me);
   
   if (P->Par.me_comm_ST == 0) { 
@@ -1354 +1933 @@
           //return 0;
         // and store 'em (for all who have this Psi local)
-        fprintf(stderr,"(%i) Psi %i, L %i: (x,y,z) = (%lg, %lg, %lg), Spread %lg\n",P->Par.me,i, R->LevSNo, WannierCentre[0], WannierCentre[1], WannierCentre[2], WannierCentre[NDIM]);
+        if ((P->Par.me == 0) && P->Call.out[ValueOut]) fprintf(stderr,"(%i) Psi %i, L %i: (x,y,z) = (%lg, %lg, %lg), Spread %lg\n",P->Par.me,i, R->LevSNo, WannierCentre[0], WannierCentre[1], WannierCentre[2], WannierCentre[NDIM]);
         for (j=0;j<NDIM;j++) Psi->AddData[OnePsiA->MyLocalNo].WannierCentre[j] = WannierCentre[j];
         Psi->AddData[OnePsiA->MyLocalNo].WannierSpread = WannierCentre[NDIM];
-        if (P->Call.out[ValueOut]) fprintf(stderr,"(%i) %s\t%lg\t%lg\t%lg\t\t%lg\n",P->Par.me, tagname,Psi->AddData[OnePsiA->MyLocalNo].WannierCentre[0],Psi->AddData[OnePsiA->MyLocalNo].WannierCentre[1],Psi->AddData[OnePsiA->MyLocalNo].WannierCentre[2],Psi->AddData[OnePsiA->MyLocalNo].WannierSpread);
+        //if (P->Par.me ==0 && P->Call.out[ValueOut]) fprintf(stderr,"(%i) %s\t%lg\t%lg\t%lg\t\t%lg\n",P->Par.me, tagname,Psi->AddData[OnePsiA->MyLocalNo].WannierCentre[0],Psi->AddData[OnePsiA->MyLocalNo].WannierCentre[1],Psi->AddData[OnePsiA->MyLocalNo].WannierCentre[2],Psi->AddData[OnePsiA->MyLocalNo].WannierSpread);
       } else if (P->Par.me_comm_ST == 0) { // if they are not local, yet we are process 0, send 'em to leader of its Psi group
         if (MPI_Send(WannierCentre, NDIM+1, MPI_DOUBLE, OnePsiA->my_color_comm_ST_Psi, ParseWannierTag, P->Par.comm_ST_PsiT) != MPI_SUCCESS)
@@ -1367 +1946 @@
   if ((SpreadFile != NULL) && (P->Par.me_comm_ST == 0)) 
     fclose(SpreadFile);
-  fprintf(stderr,"(%i) Parsing Wannier files succeeded!\n", P->Par.me);
+  //fprintf(stderr,"(%i) Parsing Wannier files succeeded!\n", P->Par.me);
   return 1;
 }
@@ -1408 +1987 @@
   b_ij = 0;
 
-  // create lookup table for sin/cos values
-  cos_lookup = (double **) Malloc(sizeof(double *)*NDIM, "ComputeMLWF: *cos_lookup");
-  sin_lookup = (double **) Malloc(sizeof(double *)*NDIM, "ComputeMLWF: *sin_lookup");
-  for (k=0;k<NDIM;k++) {
-    // allocate memory
-    cos_lookup[k] = (double *) Malloc(sizeof(double)*LevS->Plan0.plan->N[k], "ComputeMLWF: cos_lookup");
-    sin_lookup[k] = (double *) Malloc(sizeof(double)*LevS->Plan0.plan->N[k], "ComputeMLWF: sin_lookup");
-    // reset arrays
-    SetArrayToDouble0(cos_lookup[k],LevS->Plan0.plan->N[k]);
-    SetArrayToDouble0(sin_lookup[k],LevS->Plan0.plan->N[k]);
-    // create lookup values
-    for (g=0;g<LevS->Plan0.plan->N[k];g++) {
-      tmp = 2*PI/(double)LevS->Plan0.plan->N[k]*(double)g; 
-      cos_lookup[k][g] = cos(tmp);
-      sin_lookup[k][g] = sin(tmp);
-    }
-  }  
+  CreateSinCosLookupTable(&cos_lookup,&sin_lookup,N);
+
   // fill matrices
   OnePsiA = &Psi->AllPsiStatus[i];    // grab the desired OnePsiA
@@ -1499 +2063 @@
   // store spread in OnePsiElementAdd
   Psi->AddData[i].WannierSpread = B_ij - spread;
-  // free lookups
-  for (k=0;k<NDIM;k++) {
-    Free(cos_lookup[k], "bla");
-    Free(sin_lookup[k], "bla");
-  }
-  Free(cos_lookup, "bla");
-  Free(sin_lookup, "bla");
-
+
+  FreeSinCosLookupTable(cos_lookup,sin_lookup);
+  
   return (B_ij - spread);
 }

pcp/src/wannier.h

@@ -12 +12 @@
 
   File: wannier.h
-  $Id: wannier.h,v 1.8 2007/02/05 14:00:58 foo Exp $
+  $Id: wannier.h,v 1.3 2007-10-08 15:43:29 heber Exp $
 */
 
 #include <gsl/gsl_complex.h>
 
+/** Structure contains all variables needed for diagonalization of a matrix with 
+ * SerialDiagonalization() or ParallelDiagonalization()
+ */
+struct DiagonalizationData {
+  int Num;        //!< Number of rows/columns
+  int AllocNum;   //!< even number of rows/columns
+  int NumMatrices;//!< number of matrices to be simultaneously "actively" diagonalized
+  int extra;      //!< number of additional matrices that are also, yet "passively" diagonalized (not considered in rotation angle evaluation)
+  gsl_matrix *U;   //!< transformation matrix
+  gsl_matrix **A;  //!< matrix to be diagonlized
+  int *top;       //!< merry-go-round top row of indices
+  int *bot;       //!< merry-go-round bottom row of indices
+  MPI_Comm *comm; //!< MPI communicator for ParallelDiagonalization()
+  int ProcRank;   //!< Rank of this process, used in ParallelDiagonalization()
+  int ProcNum;    //!< Number of process in communicator, used in ParallelDiagonalization()
+};
+
+void PrintGSLMatrix(struct Problem *P, gsl_matrix *U, int Num, const char *msg);
 void ComputeMLWF(struct Problem *P);
+void WriteWannierFile(struct Problem *P, double spread, double old_spread, double **WannierCentre, double *WannierSpread);
 int ParseWannierFile(struct Problem *P);
+void SerialDiagonalization(struct Problem *P, struct DiagonalizationData *DiagData);
+void ParallelDiagonalization(struct Problem *P, struct DiagonalizationData *DiagData);
 double CalculateSpread(struct Problem *P, int i);
 gsl_complex convertComplex (fftw_complex a);
+void InitDiagonalization(struct Problem *P, struct DiagonalizationData *DiagData, int Num, int NumMatrices, int extra);
+void FreeDiagonalization(struct DiagonalizationData *DiagData);
+void OrthogonalizePsis(struct Problem *P);
+void StrongOrthogonalizePsis(struct Problem *P);
+void Diagonalize(struct Problem *P, struct DiagonalizationData *DiagData);
+
+void CalculateSecondOrderReciprocalMoment(struct Problem *P);
 
 #endif /*WANNIER_H_*/