/** \file wannier.c
 * Maximally Localized Wannier Functions.
 * 
 * Contains the on function that minimises the spread of all orbitals in one rush in a parallel
 * Jacobi-Diagonalization implementation, ComputeMLWF(), and one routine CalculateSpread() to
 * calculate the spread of a specific orbital, which may be useful in checking on the change of
 * spread during other calculations. convertComplex() helps in typecasting fftw_complex to gsl_complex.
 * 
  Project: ParallelCarParrinello
 \author Frederik Heber
 \date 2006

  File: wannier.c
  $Id: wannier.c,v 1.63 2007-02-13 14:15:29 foo Exp $
*/

#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_heapsort.h>
#include <gsl/gsl_blas.h>
#include <string.h>

#include "data.h"
#include "density.h"
#include "errors.h"
#include "helpers.h"
#include "init.h"
#include "myfft.h"
#include "mymath.h"
#include "output.h"
#include "wannier.h"


#define max_operators NDIM*2 //!< number of chosen self-adjoint operators when evaluating the spread


/** Converts type fftw_complex to gsl_complex.
 * \param a complex number
 * \return b complex number
 */
gsl_complex convertComplex (fftw_complex a) {
  return gsl_complex_rect(c_re(a),c_im(a));
}

/** "merry go round" implementation for parallel index ordering.
 * Given two arrays, one for the upper/left matrix columns, one for the lower/right ones, one step of an index generation is
 * performed which generates once each possible pairing.  
 * \param *top index array 1
 * \param *bot index array 2
 * \param m N/2, where N is the matrix row/column dimension
 * \note taken from [Golub, Matrix computations, 1989, p451]
 */
void music(int *top, int *bot, int m)
{
  int *old_top, *old_bot;
  int k;
  old_top = (int *) Malloc(sizeof(int)*m, "music: old_top");
  old_bot = (int *) Malloc(sizeof(int)*m, "music: old_bot");
/*  fprintf(stderr,"oldtop\t");
  for (k=0;k<m;k++)
    fprintf(stderr,"%i\t", top[k]);
  fprintf(stderr,"\n");
  fprintf(stderr,"oldbot\t");
  for (k=0;k<m;k++)
    fprintf(stderr,"%i\t", bot[k]);
  fprintf(stderr,"\n");*/
  // first copy arrays    
  for (k=0;k<m;k++) {
    old_top[k] = top[k];
    old_bot[k] = bot[k];
  }
  // then let the music play
  for (k=0;k<m;k++) {
    if (k==1)
      top[k] = old_bot[0];
    else if (k > 1)
      top[k] = old_top[k-1];
    if (k==m-1)
      bot[k] = old_top[k];
    else
      bot[k] = old_bot[k+1];
  }
/*  fprintf(stderr,"top\t");
  for (k=0;k<m;k++)
    fprintf(stderr,"%i\t", top[k]);
  fprintf(stderr,"\n");
  fprintf(stderr,"bot\t");
  for (k=0;k<m;k++)
    fprintf(stderr,"%i\t", bot[k]);
  fprintf(stderr,"\n");*/
  // and finito
  Free(old_top);
  Free(old_bot);
}

/** merry-go-round for matrix columns.
 * The trick here is that we must be aware of multiple rotations per process, thus only some of the
 * whole lot of local columns get sent/received, most of them are just shifted via exchanging the various
 * pointers to the matrix columns within the local array.
 * \param comm communicator for circulation
 * \param *Aloc local array of columns
 * \param Num entries per column
 * \param max_rounds number of column pairs in \a *Around
 * \param k offset for tag
 * \param tagS0 MPI tag for sending left column
 * \param tagS1 MPI tag for sending right column
 * \param tagR0 MPI tag for receiving left column
 * \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) {
  //double *A_locS1, *A_locS2;  // local columns of A[k]
  //double *A_locR1, *A_locR2;  // local columns of A[k]
  MPI_Request requestS0, requestS1, requestR0, requestR1;
  MPI_Status status;
  int ProcRank, ProcNum;
  int l; 
  MPI_Comm_size (comm, &ProcNum);
  MPI_Comm_rank (comm, &ProcRank);
  double *Abuffer1, *Abuffer2;  // mark the columns that are circulated
  
  //fprintf(stderr,"shifting...");
  if (ProcRank == 0) {
    if (max_rounds > 1) { 
      // get last left column
      Abuffer1 = Aloc[2*(max_rounds-1)]; // note down the free column
      MPI_Isend(Abuffer1, Num, MPI_DOUBLE, ProcRank+1, WannierALTag+2*k, comm, &requestS0);
    } else {
      // get right column
      Abuffer1 = Aloc[1]; // note down the free column
      MPI_Isend(Abuffer1, Num, MPI_DOUBLE, ProcRank+1,  tagS1+2*k, comm, &requestS0);
    } 
    
    //fprintf(stderr,"...left columns...");
    for(l=2*max_rounds-2;l>2;l-=2) // left columns become shifted one place to the right
      Aloc[l] = Aloc[l-2];

    if (max_rounds > 1) { 
      //fprintf(stderr,"...first right...");
      Aloc[2] = Aloc[1]; // get first right column
    }

    //fprintf(stderr,"...right columns...");
    for(l=1;l<2*max_rounds-1;l+=2) // right columns become shifted one place to the left
      Aloc[l] = Aloc[l+2];

    //fprintf(stderr,"...last right...");
    Aloc[(2*max_rounds-1)] = Abuffer1;
    MPI_Irecv(Abuffer1, Num, MPI_DOUBLE, ProcRank+1, WannierARTag+2*k, comm, &requestR1);  

  } else if (ProcRank == ProcNum-1) {
    //fprintf(stderr,"...first right...");
    // get first right column
    Abuffer2 = Aloc[1]; // note down the free column
    MPI_Isend(Abuffer2, Num, MPI_DOUBLE, ProcRank-1, WannierARTag+2*k, comm, &requestS1);

    //fprintf(stderr,"...right columns...");
    for(l=1;l<2*max_rounds-1;l+=2) // right columns become shifted one place to the left
      Aloc[(l)] = Aloc[(l+2)];

    //fprintf(stderr,"...last right...");
    Aloc[(2*max_rounds-1)] = Aloc[2*(max_rounds-1)]; // Put last left into last right column

    //fprintf(stderr,"...left columns...");
    for(l=2*(max_rounds-1);l>0;l-=2) // left columns become shifted one place to the right
      Aloc[(l)] = Aloc[(l-2)];

    //fprintf(stderr,"...first left...");
//    if (max_rounds > 1)
      Aloc[0] = Abuffer2; // get first left column
    MPI_Irecv(Abuffer2, Num, MPI_DOUBLE, ProcRank-1, WannierALTag+2*k, comm, &requestR0);

  } else {
    // get last left column
    MPI_Isend(Aloc[2*(max_rounds-1)], Num, MPI_DOUBLE, ProcRank+1, WannierALTag+2*k, comm, &requestS0);
    Abuffer1 = Aloc[2*(max_rounds-1)]; // note down the free column

    //fprintf(stderr,"...first right...");
    // get first right column
    MPI_Isend(Aloc[1], Num, MPI_DOUBLE, ProcRank-1, WannierARTag+2*k, comm, &requestS1);
    Abuffer2 = Aloc[1]; // note down the free column

    //fprintf(stderr,"...left columns...");
    for(l=2*(max_rounds-1);l>0;l-=2) // left columns become shifted one place to the right
      Aloc[(l)] = Aloc[(l-2)];

    //fprintf(stderr,"...right columns...");
    for(l=1;l<2*max_rounds-1;l+=2) // right columns become shifted one place to the left
      Aloc[(l)] = Aloc[(l+2)];

    //fprintf(stderr,"...first left...");
    Aloc[0] = Abuffer1; // get first left column
    MPI_Irecv(Aloc[0], Num, MPI_DOUBLE, ProcRank-1, WannierALTag+2*k, comm, &requestR0);

    //fprintf(stderr,"...last right...");
    Aloc[(2*max_rounds-1)] = Abuffer2;
    MPI_Irecv(Aloc[(2*max_rounds-1)], Num, MPI_DOUBLE, ProcRank+1, WannierARTag+2*k, comm, &requestR1);
  }
    
  //fprintf(stderr,"...waiting...");
  if (ProcRank != ProcNum-1)
    MPI_Wait(&requestS0, &status);
  if (ProcRank != 0)  // first left column
    MPI_Wait(&requestR0, &status);
  if (ProcRank != 0)
    MPI_Wait(&requestS1, &status);
  if (ProcRank != ProcNum-1)
    MPI_Wait(&requestR1, &status);
  //fprintf(stderr,"...done\n");
}

/** Computation of Maximally Localized Wannier Functions.
 * Maximally localized functions are prime when evulating a Hamiltonian with
 * magnetic fields under periodic boundary conditions, as the common position
 * operator is no longer valid. These can be obtained by orbital rotations, which
 * are looked for iteratively and gathered in one transformation matrix, to be
 * later applied to the set of orbital wave functions.
 * 
 * In order to obtain these, the following algorithm is applied:
 * -# Initialize U (identity) as the sought-for transformation matrix
 * -# Compute the real symmetric (due to Gamma point symmetry!) matrix elements
 *    \f$A^{(k)}_{ij} = \langle \phi_i | A^{(k)} | \phi_j \rangle\f$ for the six operators
 *    \f$A^{(k)}\f$
 * -# For each pair of indices (i,j) (i<j) do the following:
 *    -# Compute the 2x2 matrix \f$G = \Re \Bigl ( \sum_k h^H(A^{(k)}) h(A^{(k)}) \Bigr)\f$
 *       where \f$h(A) = [a_{ii} - a_{jj}, a_{ij} + a_{ji}]\f$
 *    -# Obtain eigenvalues and eigenvectors of G. Set \f$[x,y]^T\f$ to the eigenvector of G
 *       corresponding to the greatest eigenvalue, such that \f$x\geq0\f$
 *    -# Compute the rotation matrix R elements (ii,ij,ji,jj) \f$[c,s,-s,c]\f$ different from the 
 *       identity matrix by \f$r=\sqrt{x^2+y^2}\f$, \f$c = \sqrt{\frac{x+r}{2r}}\f$
 *       \f$s=\frac{y}{\sqrt{2r(x+r)}}\f$
 *    -# Perform the similarity operation \f$A^{(k)} \rightarrow R A^{(k)} R\f$
 *    -# Gather the rotations in \f$U = U R\f$
 * -# Compute the total spread \f$\sigma^2_{A^{(k)}}\f$
 * -# Compare the change in spread to a desired minimum RunStruct#EpsWannier, if still greater go to step 3.
 * -# Apply transformations to the orbital wavefunctions \f$ | \phi_i \rangle = \sum_j U_{ij} | \phi_j \rangle\f$
 * -# Compute the position of the Wannier centers from diagonal elements of \f$A^{(k)}\f$, store in
 *    OnePsiElementAddData#WannierCentre
 * 
 * Afterwards, the routine applies the found unitary rotation to the unperturbed group of wave functions.
 * Note that hereby additional memory is needed as old and transformed wave functions must be present at the same
 * time.
 * 
 * The routine uses parallelization if possible. A parallel Jacobi-Diagonalization is implemented using the index
 * generation in music() and shift-columns() such that the evaluated position operator eigenvalue matrices
 * may be diagonalized simultaneously and parallely. We use the implementation explained in
 * [Golub, Matrix computations, 1989, p451].
 * 
 * \param *P Problem at hand
 */
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 Num = Psi->NoOfPsis;  // is number of occupied plus unoccupied states for rows
  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;
  int tagR0, tagR1, tagS0, tagS1;
  int iloc, jloc;
  double *s_all, *c_all;
  int round, max_rounds;
  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);       
    }
  } 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);
        }
      }  
      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.);
  }
  // free lookups
  for (i=0;i<NDIM;i++) {
    Free(cos_lookup[i]);
    Free(sin_lookup[i]);
  }
  Free(cos_lookup);
  Free(sin_lookup);
  
  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]);
          }
        }
      }
      // 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]);
        }
      }
      // 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]);
              }
            }
          }
          // 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]);
              }
            }
          }
          // 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);
        }
  
        //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);
          }
        }
        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]);
        Free(Around[k]);
      }
      Free(Uround);
      Free(Utotal);
      Free(c_all);
      Free(s_all);
      Free(c);
      Free(s);
      Free(rcounts);
      Free(rdispls);
      
    } 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);
      Free(s);
    }  
    
    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);
  Free(bot);

  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);

  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
  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);
    break;
  case SpinUp:
    strncpy(suffix,".spread_up.csv",18);    
    strncat(spin,"SpinUp",12);
    break;
  case SpinDown:
    strncpy(suffix,".spread_down.csv",18);    
    strncat(spin,"SpinDown",12);
    break;
  }
  if (P->Par.me_comm_ST == 0) {
    if (R->LevSNo == Lat->MaxLevel-1) // open freshly if first level 
      OpenFile(P, &F->SpreadFile, suffix, "w", P->Call.out[ReadOut]); // only open on starting level
    else if (F->SpreadFile == NULL) // re-open if not first level and not opened yet (or closed from ParseWannierFile)
      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");
    } 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] = Lat->RealBasisQ[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] = Lat->RealBasisQ[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] = Lat->RealBasisQ[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]/Lat->RealBasisQ[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]*Lat->RealBasisQ[j]; 
        else
          WannierCentre[i][j] = (double)ceil(tmp)/(double)N[j]*Lat->RealBasisQ[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);
    for (l=0;l<Num;l++)
      Free(group[l]);
    Free(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;
  }
  
  // put (new) WannierCentres into local ones and into file
  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
      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];
      }
      if (P->Par.me_comm_ST == 0) 
        fprintf(F->SpreadFile,"Psi%d_Lev%d\t%lg\t%lg\t%lg\t%lg\n", Psi->AllPsiStatus[i].MyGlobalNo, R->LevSNo, WannierCentre[i][0], WannierCentre[i][1], WannierCentre[i][2], WannierSpread[i]); 
    }
  }    
  if (P->Par.me_comm_ST == 0) {
    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]);
}

/** Parses the spread file and puts values into OnePsiElementAddData#WannierCentre.
 * \param *P Problem at hand
 * \return 1 - success, 0 - failure
 */
int ParseWannierFile(struct Problem *P)
{
  struct Lattice *Lat = &P->Lat;
  struct RunStruct *R = &P->R;
  struct Psis *Psi = &Lat->Psi;
  struct OnePsiElement *OnePsiA;
  int i,l,j, msglen;
  FILE *SpreadFile;
  char tagname[255];
  char suffix[18];
  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);    
    break;
  case SpinUp:
    strncpy(suffix,".spread_up.csv",18);    
    break;
  case SpinDown:
    strncpy(suffix,".spread_down.csv",18);    
    break;
  }
  
  if (P->Par.me_comm_ST == 0) { 
    if(!OpenFile(P, &SpreadFile, suffix, "r", P->Call.out[ReadOut])) { // check if file exists
      if (MPI_Bcast(&signal,1,MPI_INT,0,P->Par.comm_ST) != MPI_SUCCESS)
        Error(SomeError,"ParseWannierFile: Bcast of signal failed\n");
      return 0;
      //Error(SomeError,"ParseWannierFile: Opening failed\n");
    }
    signal = 1;
    if (MPI_Bcast(&signal,1,MPI_INT,0,P->Par.comm_ST) != MPI_SUCCESS)
      Error(SomeError,"ParseWannierFile: Bcast of signal failed\n");
  } else {
    if (MPI_Bcast(&signal,1,MPI_INT,0,P->Par.comm_ST) != MPI_SUCCESS)
      Error(SomeError,"ParseWannierFile: Bcast of signal failed\n");
    if (signal == 0)
      return 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
      if (P->Par.me_comm_ST == 0) { // only process 0 may access the spread file
        sprintf(tagname,"Psi%d_Lev%d",i,R->LevSNo);
        signal = 0;
        if (!ParseForParameter(0,SpreadFile,tagname,0,3,1,row_double,WannierCentre,optional)) {
          //Error(SomeError,"ParseWannierFile: Parsing WannierCentre failed");
          if (MPI_Bcast(&signal,1,MPI_INT,0,P->Par.comm_ST) != MPI_SUCCESS)
            Error(SomeError,"ParseWannierFile: Bcast of signal failed\n");
          return 0;
        }
        if (!ParseForParameter(0,SpreadFile,tagname,0,4,1,double_type,&WannierCentre[NDIM],optional)) {
          //Error(SomeError,"ParseWannierFile: Parsing WannierSpread failed");
          if (MPI_Bcast(&signal,1,MPI_INT,0,P->Par.comm_ST) != MPI_SUCCESS)
            Error(SomeError,"ParseWannierFile: Bcast of signal failed\n");
          return 0;
        }
        signal = 1;
        if (MPI_Bcast(&signal,1,MPI_INT,0,P->Par.comm_ST) != MPI_SUCCESS)
          Error(SomeError,"ParseWannierFile: Bcast of signal failed\n");
      } else {
        if (MPI_Bcast(&signal,1,MPI_INT,0,P->Par.comm_ST) != MPI_SUCCESS)
          Error(SomeError,"ParseWannierFile: Bcast of signal failed\n");
        if (signal == 0)
          return 0;
      }
      if (OnePsiA->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) { // is this Psi local?
        if ((P->Par.me_comm_ST != 0) && (P->Par.me_comm_ST_Psi == 0)) { // if they don't belong to process 0 and we are a leader of a Psi group, receive 'em
          if (MPI_Recv(WannierCentre, NDIM+1, MPI_DOUBLE, 0, ParseWannierTag, P->Par.comm_ST_PsiT, &status) != MPI_SUCCESS)
            Error(SomeError,"ParseWannierFile: MPI_Recv of WannierCentre/Spread from process 0 failed");
            //return 0;
          MPI_Get_count(&status, MPI_DOUBLE, &msglen);
          if (msglen != NDIM+1) 
            Error(SomeError,"ParseWannierFile: MPI_Recv of WannierCentre/Spread from process 0 failed due to wrong item count");
            //return 0;
        }
        if (MPI_Bcast(WannierCentre, NDIM+1, MPI_DOUBLE, 0, P->Par.comm_ST_Psi) != MPI_SUCCESS) // Bcast to all processes of the Psi group from leader
          Error(SomeError,"ParseWannierFile: MPI_Bcast of WannierCentre/Spread to sub process in Psi group failed");
          //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]);
        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);
      } 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)
          Error(SomeError,"ParseWannierFile: MPI_Send of WannierCentre/Spread to process 0 of owning Psi group failed");
          //return 0;
      }
    }
  }
  if ((SpreadFile != NULL) && (P->Par.me_comm_ST == 0)) 
    fclose(SpreadFile);
  fprintf(stderr,"(%i) Parsing Wannier files succeeded!\n", P->Par.me);
  return 1;
}

/** Calculates the spread of orbital \a i.
 * Stored in OnePsiElementAddData#WannierSpread.
 * \param *P Problem at hand
 * \param i i-th wave function (note "extra" ones are not counted!)
 * \return spread \f$\sigma^2_{A^{(k)}}\f$
 */
double CalculateSpread(struct Problem *P, int i) {
  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[HGcDensity];  // use HGcDensity, 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 *OnePsiA, *LOnePsiA;
  int ElementSize = (sizeof(fftw_complex) / sizeof(double)), RecvSource;
  fftw_complex *LPsiDatA=NULL;
  int k,n[NDIM],n0, *N,N0, g, p, iS, i0, Index;
  N0 = LevS->Plan0.plan->local_nx;
  N = LevS->Plan0.plan->N;
  const int NUpx = LevS->NUp[0];
  const int NUpy = LevS->NUp[1];
  const int NUpz = LevS->NUp[2];
  double a_ij, b_ij, A_ij, B_ij;
  double tmp, tmp2, spread = 0;
  double **cos_lookup, **sin_lookup;
  
  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);
    }
  }  
  // fill matrices
  OnePsiA = &Psi->AllPsiStatus[i];    // grab the desired OnePsiA
  if (OnePsiA->PsiType != Extra) {   // drop if extra one
    if (OnePsiA->my_color_comm_ST_Psi == P->Par.my_color_comm_ST_Psi) // local?
       LOnePsiA = &Psi->LocalPsiStatus[OnePsiA->MyLocalNo];
    else 
       LOnePsiA = NULL;
    if (LOnePsiA == NULL) {   // if it's not local ... receive it from respective process into TempPsi
      RecvSource = OnePsiA->my_color_comm_ST_Psi;
      MPI_Recv( LevS->LPsi->TempPsi, LevS->MaxG*ElementSize, MPI_DOUBLE, RecvSource, WannierTag1, P->Par.comm_ST_PsiT, &status );
      LPsiDatA=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 != OnePsiA->my_color_comm_ST_Psi) 
          MPI_Send( LevS->LPsi->LocalPsi[OnePsiA->MyLocalNo], LevS->MaxG*ElementSize, MPI_DOUBLE, p, WannierTag1, P->Par.comm_ST_PsiT);
      LPsiDatA=LevS->LPsi->LocalPsi[OnePsiA->MyLocalNo];
    } // LPsiDatA is now set to the coefficients of OnePsi either stored or MPI_Received
      
    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) {
            tmp = 2*PI/(double)(N[k/2])*(double)(n[k/2]);
            tmp2 = PsiCR[i0] /LevS->MaxN;
            // check lookup
            if ((fabs(cos(tmp) - cos_lookup[k/2][n[k/2]]) > MYEPSILON) || (fabs(sin(tmp) - sin_lookup[k/2][n[k/2]]) > MYEPSILON)) {
              fprintf(stderr,"(%i) (cos) %2.15e against (lookup) %2.15e,\t(sin) %2.15e against (lookup) %2.15e\n", P->Par.me, cos(tmp), cos_lookup[k/2][n[k/2]],sin(tmp),sin_lookup[k/2][n[k/2]]);   
              Error(SomeError, "Lookup table does not match real value!");
            }
//            HGcR[k][iS] = cos_lookup[k/2][n[k/2]] * tmp2; /* Matrix Vector Mult */
//            HGcR2[k][iS] = cos_lookup[k/2][n[k/2]] * HGcR[k][iS]; /* Matrix Vector Mult */
//            HGcR[k+1][iS] = sin_lookup[k/2][n[k/2]] * tmp2; /* Matrix Vector Mult */
//            HGcR2[k+1][iS] = sin_lookup[k/2][n[k/2]] * HGcR[k+1][iS]; /* Matrix Vector Mult */
            HGcR[k][iS] = cos(tmp) * tmp2; /* Matrix Vector Mult */
            HGcR2[k][iS] = pow(cos(tmp),2) * tmp2; /* Matrix Vector Mult */
            HGcR[k+1][iS] = sin(tmp) * tmp2; /* Matrix Vector Mult */
            HGcR2[k+1][iS] = pow(sin(tmp),2) * tmp2; /* Matrix Vector Mult */
          }
        }
    for (k=0;k<max_operators;k++) {
      fft_3d_real_to_complex(plan, LevS->LevelNo, FFTNF1, HGcRC[k], work);
      fft_3d_real_to_complex(plan, LevS->LevelNo, FFTNF1, HGcR2C[k], work);
    }


    for (k=0;k<max_operators;k++) {
      a_ij = 0;
      //fprintf(stderr,"(%i),(%i,%i): A[%i]: multiplying with \\phi_B\n",P->Par.me, l,m,k);
      // sum directly in a_ij and b_ij the two desired terms
      g=0;
      if (LevS->GArray[0].GSq == 0.0) {
        Index = LevS->GArray[g].Index;
        a_ij += (LPsiDatA[0].re*HGcRC[k][Index].re + LPsiDatA[0].im*HGcRC[k][Index].im);
        b_ij += (LPsiDatA[0].re*HGcR2C[k][Index].re + LPsiDatA[0].im*HGcR2C[k][Index].im);
        g++;
      }
      for (; g < LevS->MaxG; g++) {
        Index = LevS->GArray[g].Index;
        a_ij += 2*(LPsiDatA[g].re*HGcRC[k][Index].re + LPsiDatA[g].im*HGcRC[k][Index].im);
        b_ij += 2*(LPsiDatA[g].re*HGcR2C[k][Index].re + LPsiDatA[g].im*HGcR2C[k][Index].im);
      } // due to the symmetry the resulting matrix element is real and symmetric in (i,i) ! (complex multiplication simplifies ...)
      MPI_Allreduce ( &a_ij, &A_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_Psi);
      spread += pow(A_ij,2);
    }
  }
  MPI_Allreduce ( &b_ij, &B_ij, 1, MPI_DOUBLE, MPI_SUM, P->Par.comm_ST_Psi);

  // store spread in OnePsiElementAdd
  Psi->AddData[i].WannierSpread = B_ij - spread;
  // free lookups
  for (k=0;k<NDIM;k++) {
    Free(cos_lookup[k]);
    Free(sin_lookup[k]);
  }
  Free(cos_lookup);
  Free(sin_lookup);

  return (B_ij - spread);
}