/*

  Copyright (C) 2010-2011-2012 Andrea Nobile
  
  FGMRES-DR mixed precision algorithm 
    
  Andrea Nobile
  
 */

#define FGCR_C

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include "mpi.h"
#include "su3.h"
#include "start.h"
#include "linalg.h"
#include "global.h"

#define PRECISION_LIMIT ((double)(100.0f*FLT_EPSILON))

#define PRECISION_LIMIT_DFL ((double)(5*1e-2))

typedef complex_dble complexd;


void zero_memory(void *addr, int sz);
void convert_dble2float(void *saddr, void *daddr, int nelements);

void dbl_sub_cvt2fl(void *res, void *a, void *b, int nelem);
void flt2dbl_add(void *res, void *a, int nelem);

void givens(void *f, void *g, void *cs, void *sn, void *rot);


void copy_H_column(int cn, int n);
void alloc_or_H(int n);
void alloc_h_H(int n);



void build_projectors(complexd *g, complexd *p_k, int *srt, int n, int k);
void harmonic_ritz(complexd *h, complexd *theta, complexd *g, int n, int *srt);
void harmonic_h(complexd *hin, complexd *ht, double b, int n, int nit);
void sorteigs(double *norms, int *sortedidx, int sz);
void solve_ls(complexd *m, complexd *c, complexd *x, int rows, int cols);
void deflated_restart(int nkv, int nv, int neig);
void buildH(complexd *p_k, int nkv, int nv, int neig);
void buildQ(int nkv, int ne);
void insert_true_res(int nkv, int k);
void rotate_rh(int n);
void rotate_vector(complex *v, int n);
void apply_QtriHh(complexd *in, complexd *out, int n);
static void new_ZV(complexd *p_k, int n, int k);
void consistency_check(void (*Op)(spinor_dble *pk,spinor_dble *pl), int nkv, int neig);



void zeig(complexd *m, complexd *eigenvalues, complexd *eigenvectors,  int sz);
void zqr_(void*, void*, void*, void*);
void zgesv_(void*, void*, void*, void*, void*, void*, void*, void*);
void zgels_(void*, void*, void*, void*, void*, void*, void*, void*, 
	    void*, void*, void*); 

static int dds_myrank;

static int nsp,com;
static int deflated_nv;

static double rn;
static spinor **psq,**psz,*psr, *psx, *psnr;
static spinor_dble *pdb,*pdx,*pdw;
static spinor **sflds;
static complex *H;
static complex *rh;
static complexd *or_rh;

static complex *rot_s;
static float *rot_c;

static float beta;
static double bb;

static complexd *or_H;
static complexd *h_H;


static complexd *p_k;
static complexd *QtriH;
static int def_nv = 10;

static FILE *drlog;

void zero_memory(void *addr, int sz)
{

  memset(addr, 0, sz);

}

void convert_dble2float(void *saddr, void *daddr, int nelements)
{
  double *s;
  float *d;
  
  int i;
  
  s = saddr;
  d = daddr;
  
  for (i=0;i<nelements;i++) {
    d[i] = (float) s[i];
  }

}

void flt2dbl_add(void *res, void *a, int nelem)
{
  double *d;
  float *s;
  
  int i;
  s=a;
  d = res;
  
  for (i=0;i<nelem;i++) {
    d[i] = d[i] + (double) s[i];
  }

}

void dbl_sub_cvt2fl(void *res, void *a, void *b, int nelem)
{
  double *s, *s1;
  float *d;
  
  int i;
  s=a;
  s1 = b;
  d = res;
  
  for (i=0;i<nelem;i++) {
    d[i] = (float) (s[i] - s1[i]);
  }
  
}


void set_ndfl(int ndfl)
{
  def_nv = ndfl;

}

static void zero_H(int n) 
{
  int i;
  
  for(i=0;i<n*(n+1);i++) {
    H[i].re = 0.0;
    H[i].im = 0.0;
  }
  
}

static void zero_or_H(int n) 
{
  int i;
  
  for(i=0;i<n*(n+1);i++) {
    or_H[i].re = 0.0;
    or_H[i].im = 0.0;
  }
  
}


static void alloc_H(int n) 
{
  H=malloc((n+1)*n*sizeof(complex));
}

static void free_H(int n)
{
  
  free(H);
}


static void alloc_rotations(int n)
{
  rot_s = malloc(n*sizeof(complex));
  rot_c = malloc(n*sizeof(float));
}


static void free_rotations()
{
  free(rot_s);
  free(rot_c);
}


static void alloc_rh(int n)
{
  
  rh = malloc((n+1)*sizeof(complex));
  or_rh = malloc((n+1)*sizeof(complexd));

}

static void zero_rh(int n)
{
  int i;
  complex z;
  z.re = 0.0;
  z.im = 0.0;
  
  for (i=0;i<=n;i++) {
    rh[i]=z;
    or_rh[i].re = 0.0;
    or_rh[i].im = 0.0;
  }
  //zero_rotations(n);

}


// projects a vector to the space orthogonal to V
void prj_ortho_V_cd(spinor *fldin, spinor *fldout, spinor **vects, int n, complexd *coeff)
{
  int i;
  complex c;

  if(fldin != fldout) {
    memcpy(fldout, fldin, nsp*sizeof(spinor));
  }
  

  for (i=0;i<n;i++) {
    c=spinor_prod(nsp,com,vects[i],fldin);
    coeff[i].re = c.re;
    coeff[i].im = c.im;
    c.re = -c.re;
    c.im = -c.im;
    mulc_spinor_add(nsp,fldout,vects[i],c);
    
  }

}


static void alloc_kv(int nkv)
{
  int i;
  sflds = malloc((2*nkv+1)*sizeof(spinor*));
  psq = malloc((nkv+1)*sizeof(spinor*));
  psz = malloc(nkv*sizeof(spinor*));
  for (i=0;i<2*nkv+1;i++) {
    sflds[i]=alloc_ssfield();
  }
  
  
  for (i=0;i<nkv;i++) {
    psz[i] = sflds[i];
    psq[i] = sflds[i+nkv];
    
  }
  psq[nkv] = sflds[2*nkv];
  psx = alloc_ssfield();
  psnr = alloc_ssfield();
  
}

static void free_kv(int nkv)
{
  
  int i;
  
  for (i=0;i<2*nkv+1;i++) {
    afree(sflds[i]);

  }


  free(sflds);
  free(psq);
  free(psz);
  afree(psx);
  afree(psnr);
  
}

static inline complexd cddiv(complexd a, complexd b)
{
  complexd res;
  double nn;

  nn = b.re*b.re + b.im*b.im;
  res.re = (a.re*b.re + a.im*b.im)/nn;
  res.im = (a.im*b.re - a.re*b.im)/nn;

  return res;
}

static inline complex cdiv(complex a, complex b)
{
  complex res;
  float nn;

  nn = b.re*b.re + b.im*b.im;
  res.re = (a.re*b.re + a.im*b.im)/nn;
  res.im = (a.im*b.re - a.re*b.im)/nn;

  return res;
}

static inline complex csmul(float a, complex b)
{
  complex res;
  
  res.re = a*b.re;
  res.im = a*b.im;

  return res;
}

static inline complexd cdsmul(double a, complexd b)
{
  complexd res;
  
  res.re = a*b.re;
  res.im = a*b.im;

  return res;
}

static inline complex cmul(complex a, complex b)
{
  complex res;
  
  res.re = a.re*b.re - a.im*b.im;
  res.im = a.re*b.im + b.re*a.im;

  return res;
}

static inline complexd cdmul(complexd a, complexd b)
{
  complexd res;
  
  res.re = a.re*b.re - a.im*b.im;
  res.im = a.re*b.im + b.re*a.im;

  return res;
}

static inline complex csub(complex a, complex b)
{
  complex res;

  res.re = a.re - b.re;
  res.im = a.im - b.im;

  return res;
}

static inline complexd cdsub(complexd a, complexd b)
{
  complexd res;

  res.re = a.re - b.re;
  res.im = a.im - b.im;

  return res;
}


static inline complex cadd(complex a, complex b)
{
  complex res;

  res.re = a.re + b.re;
  res.im = a.im + b.im;

  return res;
}

static inline complexd cdadd(complexd a, complexd b)
{
  complexd res;

  res.re = a.re + b.re;
  res.im = a.im + b.im;

  return res;
}

static inline complexd cdconj(complexd c)
{

  complexd ret;

  ret.re = c.re;
  ret.im = -c.im;

  return ret;
}



static void fgmres_reset(void (*Op)(spinor_dble *pk,spinor_dble *pl))
{   
   
  assign_s2s(nsp, psnr, psr);
  rn = normalize(nsp,com,psr);
  beta = rn;
  
}


static double fgmres_true_res(void (*Op)(spinor_dble *pk,spinor_dble *pl))
{   

  double ret;
  

  (*Op)(pdx,pdw);
  
  

  //r = b - w
  dbl_sub_cvt2fl(psnr, pdb, pdw, nsp*24);
 
  ret = norm_square(nsp, com, psnr);
  ret = sqrt(ret);
  
  if(!dds_myrank) {
    fprintf(drlog, "\n\n\t TRUE RES = %e", ret);
    fflush(drlog);
  }

  return ret;
  
}


static void fgmres_start(spinor_dble *psi, spinor_dble *src, int nkv, void (*Op)(spinor_dble *pk,spinor_dble *pl)) 
{

  //alloca tutto il necessario

  alloc_kv(nkv);
  alloc_H(nkv);
  alloc_rotations(nkv);
  alloc_rh(nkv);
  
  alloc_or_H(nkv);

  zero_H(nkv);
  zero_or_H(nkv);
  

  /* dp flds */
  pdx = psi;
  pdb = src;
  
  /* sp flds */
  psr = psq[0];

  convert_dble2float(pdb, psr, nsp*24);
  
  rn = normalize(nsp,com,psr);
  
  zero_memory(pdx, nsp*sizeof(spinor_dble));
  
    
  
  
  //first arnoldi vector is psq[0], alias psr, normalized
  //  printf("bf zero\n");
  zero_rh(nkv);
  beta = rn;

  if(!dds_myrank) {
    fprintf(drlog, "arnoldi %d rn = %e\n", 0, rn);
	 //fflush(drlog);
  }
  

}


void Op_dble(spinor_dble *pk,spinor_dble *pl);

static void fgmres_arnoldi(void (*Mj)(int j,spinor *pr,spinor *pz,spinor *paz),
		  int j,int nkv)
{
  int i;
  complex w;
  spinor *psw;
   
   psw=psq[j+1];

   //apply precoditioner and Dz
   //psq[j] left as is... 
   (*Mj)(j,psq[j],psz[j],psw);   


   //orthonormalization
   for(i=0;i<=j;i++) {
     
     //Hij in fortan style is hji h is nkv+1 rows and nkv cols
     
     //H[i][j]=sprod_spu(nsp,com,psq[i],psw);

     H[(nkv+1)*j+i]=spinor_prod(nsp,com,psq[i],psw);
     w.re=-H[(nkv+1)*j+i].re;//-H[i][j].re;
     w.im=-H[(nkv+1)*j+i].im;//-H[i][j].im;
     mulc_spinor_add(nsp,psw,psq[i],w);
     
   }
   
   H[(nkv+1)*j + j+1].re = normalize(nsp,com,psw);
   H[(nkv+1)*j + j+1].im = 0.0;
   bb = H[(nkv+1)*j+ j+1].re*H[(nkv+1)*j + j+1].re;
         
   copy_H_column(j, nkv);
}

static void apply_givens(complex *inv0, complex *inv1, float c, complex s)
{
  complex v[2];
  complex vout[2];
  complex mcs;
  
  v[0] = *inv0;
  v[1] = *inv1;

  mcs.re = -s.re;
  mcs.im = s.im;

  vout[0] = cadd(csmul(c, v[0]), cmul(s, v[1]));
  vout[1] = cadd(cmul(mcs, v[0]), csmul(c, v[1]));

  *inv0 = vout[0];
  *inv1 = vout[1];

}



//perform previous transformations  on j-th column of h
static void prev_rot(int nkv, int j)
{
  int k, k1;
  //complex t;
  //complex mcs;

  if(deflated_nv) {
      rotate_vector(&H[j*(nkv+1)], deflated_nv+1);
  }


  for(k=deflated_nv+1;k<=j;k++) {
      k1 = k - 1;
      apply_givens(&H[j*(nkv+1)+k1], &H[j*(nkv+1)+k], rot_c[k1], rot_s[k1]);
      
    }
}


//get  next plane rotation and compute implicit residual norm 
static double next_rot(int nkv, int j)
{

  complex rot;
   
  givens(&H[j*(nkv+1)+j], &H[j+1 + j*(nkv+1)], &rot_c[j], &rot_s[j], &rot);
    
  apply_givens(&H[j*(nkv+1)+j], &H[j+1 + j*(nkv+1)], rot_c[j], rot_s[j]);
  apply_givens(&rh[j], &rh[j+1], rot_c[j], rot_s[j]);

  beta = sqrt(rh[j+1].re*rh[j+1].re + rh[j+1].im*rh[j+1].im);
  
  return beta;

}




static void build_solution(int nkv,int n)
{
  int i, j;
  complex t;
  complexd *m, *c, *x;
  int nn, nn1;
  
  nn = n+1;
  nn1 = nn+1;
  
  m = malloc(nn*nn1*sizeof(complexd));
  c = malloc(nn1*sizeof(complexd));
  x = malloc(nn*sizeof(complexd)); 
  //copy or_H in m
  //loop on rows
  for(i=0;i<nn1;i++) {
    //loop on cols
      for(j=0;j<nn;j++) {
	m[i+j*nn1]=or_H[i+j*(nkv+1)];
      }
  }
 
  for(i=0;i<nn1;i++) {
    c[i].re = or_rh[i].re;
    c[i].im = or_rh[i].im;
  }
  
  solve_ls(m, c, x, nn1, nn);

  free(c);
  free(m);
   
  //build linear combination
  zero_memory(psx, nsp*sizeof(spinor)); 
  //printf("building solution %d\n", n);
  for(j=0;j<=n;j++) {
    t.re = x[j].re;
    t.im = x[j].im;
    mulc_spinor_add(nsp,psx,psz[j],t);
  }
  

  free(x);
  flt2dbl_add(pdx, psx, nsp*24);
  
}

static int first=1;


double fgmres_dr(int vol,int icom,void (*Op)(spinor_dble *pk,spinor_dble *pl),
            void (*Mj)(int j,spinor *pr,spinor *pz,spinor *paz),
            spinor *ws[],spinor_dble *wsd[],int nkv,int nmx,double res,
            spinor_dble *pk,spinor_dble *pl,int *status)
{
  int nv,j,clean=0;
  double rn_old,tol;
  
  double last_trn;
  
  if(first) {
    
    MPI_Comm_rank(MPI_COMM_WORLD,&dds_myrank);
    if(!dds_myrank) {
      drlog = fopen("drlog", "w");
    }
     first = 0;
   }

   
   if ((icom==1)&&(NPROC>1))
   {
     error_root((vol<=0)||(nkv<1)||(nmx<1)||(res<=DBL_EPSILON),1,
                 "fgmres-dr.c [fgmres_dr]","Improper choice of vol %d ,nkv %d ,nmx %d or res %e", vol, nkv, nmx, res);
      
      
   }
   else if (error_loc((vol<=0)||(nkv<1)||(nmx<1)||(res<=DBL_EPSILON),1,
                      "fgcr [fgcr.c]","Improper choice of vol,nkv,nmx or res"))
   {
      (*status)=0;
      return 1.0;
   }
   

   pdw=wsd[0];
   nsp = vol;
   com = icom;
   
   
   //pl = 0
   //pk is the residue
   
   fgmres_start(pl, pk, nkv, Op);
   
   last_trn = rn;
   
   if(!dds_myrank) {
     fprintf(drlog, "after start %e\n", rn);
     fflush(drlog);
   }
   
   
   tol=res*rn;
   (*status)=0;
   deflated_nv = 0;
   while (rn>tol)
   {
     /* first rhs vector element */
     
     if(!deflated_nv) {
       rh[0].re = or_rh[0].re = beta;
       rh[0].im = or_rh[0].im = 0.0;
       last_trn = rn;
     }

     
     double prlim;
     if(def_nv>1) {
       prlim = PRECISION_LIMIT_DFL;
     }
     else prlim = PRECISION_LIMIT;
     
     
     rn_old = rn;
     nv = deflated_nv;
     
     // -----------------------  ARNOLDI ----------------------------------------------
     //------------------------  FGMRES CYCLE -----------------------------------------
     for (j=deflated_nv;j<nkv;j++) {
       //remove if possible
       MPI_Barrier(MPI_COMM_WORLD);
     
       fgmres_arnoldi(Mj,j,nkv);
       
       prev_rot(nkv, j);
       rn = next_rot(nkv, j);
       
       if(!dds_myrank) {
	 fprintf(drlog, "arnoldi %d rn = %e\n", j+1, rn);
	 //fflush(drlog);
       }
       
       (*status)+=1;
       nv++;
       
       if ((rn<=tol)||(rn<(prlim*rn_old))||((*status)>=nmx)) {
	 //printf("exiting before nkv reached\n");
	 break;
       }
       
     }
     // ------------------ ARNOLDI END --------------------------------------------




     // build solution, compute res in DP, if ok exit 
     build_solution(nkv,nv-1);
     int neig = def_nv < (nv/2) ? def_nv : nv/2;
     double aprn = rn;
     
     //remove if possible
     MPI_Barrier(MPI_COMM_WORLD);
     //compute residue in dp
     rn = fgmres_true_res(Op);

     //clean if inconsistent
     if(aprn < 0.1*rn) {
        clean++;
       neig = 0;
     }

     if(rn >1.01*last_trn) {
      
       neig = 0;

     } else {

       last_trn = rn;
     }
     

     if(!dds_myrank) {
       fprintf(drlog, " %d \n\n", *status);
       fflush(drlog);
     }
     if(rn <= tol) break;
               
     //dr stuff
     
     if(neig<=1) {

       fgmres_reset(Op);
       zero_rh(nkv);
       zero_H(nkv);
       zero_or_H(nkv);
       
       deflated_nv = 0;
     } else {
              
       deflated_restart(nkv, nv, neig);
              
     } 
     
     if (((*status)>=nmx)&&(rn>tol)) {
       (*status)=-1;
       break;
     }
   }
   if(!dds_myrank) {
     fprintf(drlog, "\n");
     fprintf(drlog, "clean = %d\n", clean);
     fflush(drlog);
   }
   
   deflated_nv = 0;
   
   free_kv(nkv);
   free_H(nkv);
   free_rotations();
   free(rh);
   free(or_rh);
   free(or_H);
   free(h_H);

   return rn;
}


/*-----------------------------------------------------------------------
  DR functionality
------------------------------------------------------------------------*/

void deflated_restart(int nkv, int nv, int neig)
{
  
  complexd *ht;
  complexd *eval;
  complexd *evec;
  int *srt;
  
    
  ht = malloc(nv*nv*sizeof(complexd));

  harmonic_h(or_H, ht, bb, nkv, nv); 

  eval = calloc(nv, sizeof(complexd));
  evec = calloc(nv*nv, sizeof(complexd));
  srt = malloc(nv*sizeof(int));
  

  harmonic_ritz(ht, eval, evec, nv, srt);
  
  free(eval);
  free(ht);

  if(p_k){
    free(p_k);
  }
  p_k = calloc((nv+1)*(neig+1), sizeof(complexd));
  
  build_projectors(evec, p_k, srt, nv, neig);
  
  free(srt);
  free(evec);
  
  buildH(p_k, nkv, nv, neig);
  new_ZV(p_k, nv, neig);
  insert_true_res(nkv, neig);
  buildQ(nkv, neig);
  rotate_rh(neig+1);
  deflated_nv = neig;

}


static void combine_flds(spinor **flds, complexd *prj, int n, int k, int prjn)
{
  spinor **tmp;
  complex c;
  int i, j;

  tmp = malloc(k*sizeof(spinor*));

  for(i=0;i<k;i++) {
    tmp[i] = alloc_ssfield();
    zero_memory(tmp[i], nsp*sizeof(spinor));
  }
  

  for (i=0;i<k;i++) {
    for (j=0;j<n;j++) {
      c.re = prj[i*prjn + j].re;
      c.im = prj[i*prjn + j].im;
      mulc_spinor_add(nsp, tmp[i], flds[j], c);
    }
  }
 
  for(i=0;i<k;i++) {
    assign_s2s(nsp, tmp[i], flds[i]);
  }
  
  //free space
  for(i=0;i<k;i++) {
    afree(tmp[i]);
  }
  
  free(tmp);

}

static void new_ZV(complexd *p_k, int n, int k)
{

  
  //do new Z
  combine_flds(psz, p_k, n, k, n+1);
  

  //do new V
  combine_flds(psq, p_k, n, k, n+1);

}


void rotate_rh(int n)
{
  rotate_vector(rh, n);
  if(!dds_myrank) {
    n--;
    fprintf(drlog, "implicit norm : %e\n", sqrt(rh[n].re*rh[n].re+rh[n].im*rh[n].im));
  }

}

void rotate_vector(complex *v, int n)
{
  complexd *in, *out;
  int i;
  
  in = malloc(2*n*sizeof(complexd));
  out = in + n;
  
  for(i=0;i<n;i++) {
    in[i].re = v[i].re;
    in[i].im = v[i].im;
  }

  apply_QtriHh(in , out, n);
  
  for(i=0;i<n;i++) {
    v[i].re = out[i].re;
    v[i].im = out[i].im;
  }

  free(in);

}

void apply_QtriHh(complexd *in, complexd *out, int n)
{
  int i, j;
  complexd t;
  complexd *myin;


  if(in == out) {
    myin = malloc(n*sizeof(complexd));
    memcpy(myin, in, n*sizeof(complexd));
  } else {
    myin = in;
  }
  
  for(i=0;i<n;i++) {
    t.re = 0;
    t.im = 0;
    for(j=0;j<n;j++) {
      t = cdadd(t, cdmul(cdconj(QtriH[j+i*n]), myin[j]));
    }
    out[i] = t;
  }
  
  if(in == out) {
    free(myin);
  }
}


// creates the matrix that triangularize the piece of upper ne x ne piece of H, stored in QtriH
// QtriH is the hermitianconj of the matrix that triangularize H
// ne x ne piece of H is then triangularized

void buildQ(int nkv, int ne)
{
  int i,j, nr;
  complexd *r, *r2;

  if(QtriH) {
    free(QtriH);
  }
  
  QtriH = calloc((ne+1)*(ne+1), sizeof(complexd));

  //put upper ne x ne of H in QtriH
  for(j=0;j<ne;j++) {
    for(i=0;i<=ne;i++) {
      QtriH[(ne+1)*j + i].re = or_H[i+j*(nkv+1)].re;
      QtriH[(ne+1)*j + i].im = or_H[i+j*(nkv+1)].im;
    }
  }
  
  nr = ne +1;

  r = calloc(ne*ne, sizeof(complexd));
  r2 = calloc(nr*nr, sizeof(complexd));

  zqr_(QtriH, r, &nr, &ne);

  for(i=0;i<ne;i++) {
    QtriH[(ne+1)*ne + i].re = 0.0;
    QtriH[(ne+1)*ne + i].im = 0.0;
  }
  QtriH[(ne+1)*ne + ne].re = 1.0;
  QtriH[(ne+1)*ne + ne].im = 0.0;
  
    
  zqr_(QtriH, r2, &nr, &nr);

  free(r2);

  //put r in  upper ne x ne of H 
  for(j=0;j<ne;j++) {
    for(i=0;i<ne;i++) {
      H[(nkv+1)*j + i].re = r[i+j*ne].re;
      H[(nkv+1)*j + i].im = r[i+j*ne].im ;
    }

    H[(nkv+1)*j + i].re = 0.0;
    H[(nkv+1)*j + i].im = 0.0;
  }

  free(r);


}

void buildH(complexd *p_k, int nkv, int nv, int neig)
{

  int i, j, k;
  complexd *new_h;
  complexd *tmp_h;
  
  complexd t;

  new_h = malloc((neig+1)*(neig)*sizeof(complexd));
  tmp_h = malloc((nv+1)*neig*sizeof(complexd));
  
  // compute H_nv * P_k
  
  for(k=0;k<neig;k++) {

    //loop su righe di H
    for(i=0;i<nv+1;i++) {
      t.re = 0.0;
      t.im = 0.0;
      for(j=0;j<nv;j++) { //loop su colonne di H e indice di P_k
	t = cdadd(t, cdmul(or_H[i+ j*(nkv+1)], p_k[j + k*(nv+1)]));
      }
      tmp_h[i+ k*(nv+1)] = t;
    }
  }

  // compute P_k^h * Htmp
  
  // colonne di new_h e colonne di h_tmp
  for(k=0;k<neig;k++) {
    //righe di p_k^h
    for(i=0;i<neig+1;i++) {
      t.re = 0.0;
      t.im = 0.0;
      //colonne di p_k^h e righe di h_tmp  
      for(j=0;j<nv+1;j++) { 
	t = cdadd(t, cdmul(cdconj(p_k[i*(nv+1) + j]) , tmp_h[j+k*(nv+1)]));
      }
      new_h[i+ k*(neig+1)] = t;
    }
  }
  
  free(tmp_h);
  
  //assign new_h to H

  //  H[(nkv+1)*j+i]=sprod_spu(nsp,com,psq[i],psw);
  zero_H(nkv);
  zero_or_H(nkv);
    
  for(j=0;j<neig;j++) {
    for(i=0;i<neig+1;i++) {
      H[(nkv+1)*j + i].re = or_H[(nkv+1)*j + i].re = new_h[i+j*(neig+1)].re;
      H[(nkv+1)*j + i].im = or_H[(nkv+1)*j + i].im = new_h[i+j*(neig+1)].im;
    }
  }
  
  free(new_h);


}

void alloc_or_H(int n)
{
  or_H = malloc(n*(n+1)*sizeof(complexd));

}

void alloc_h_H(int n)
{
  h_H = malloc(n*(n)*sizeof(complexd));

}


//copies the cn col of H in the matrix or_H, stored in fortran style
//or_H is a nkv x nkv+1 matrix
void copy_H_column(int cn, int n){

  complexd c;
  int i;
  
  for (i=0;i<=cn+1;i++) {
    c.re = H[i+cn*(n+1)].re;
    c.im = H[i+cn*(n+1)].im;
    or_H[i+cn*(n+1)] = c;
  }
  
}


//produces the matrix nxn, H + beta^2*H^-h e_m*e_m^t
void harmonic_h(complexd *hin, complexd *ht, double b, int n, int nit)
{
  
  //NOTE: n is nkv
  //hin is the or_H n+1 rows, n col matrix, fortran style
  complexd *hcol;
  complexd *em;
  int i, j;

  //for lapack call 
  int nrhs = 1;
  int *pivot;
  int info;


  hcol = malloc(nit*sizeof(complexd));
  em = malloc(nit*sizeof(complexd));
  
  //build hermitian of hin and store in ht, fortran layout
  for(i=0;i<nit;i++) {
    for(j=0;j<nit;j++) {
      ht[i*nit + j] = cdconj(hin[j*(n+1) + i]);
    }
  }

  //now solve for e_n to get H^-h e_n
  
  //create em
  for(i=0;i<nit;i++) {
    em[i].re = 0.0;
    em[i].im = 0.0;
  }
  em[nit-1].re = 1.0;

  pivot = calloc(nit, sizeof(int));
  
  //solve using lapack
  // ht is modified!!
  
  zgesv_(&nit, &nrhs, ht, &nit, pivot, em, &nit, &info);
  
  //solution is in em!!
 
  //now build harmonic h in ht, size nit x nit
  //copy the nit x nit piece
  for(i=0;i<nit;i++) {
    for(j=0;j<nit;j++) {
      ht[i*nit + j] = hin[i*(n+1) + j];
    }
  }

  for(i=0;i<nit;i++) {
    ht[nit*(nit-1)+i] = cdadd(ht[nit*(nit-1)+i], cdsmul(b, em[i]));
  }
  
 
  free(hcol);
  free(em);
  free(pivot);

}


//computes eig pairs of h, theta are eigenvalues, g are eigenvectors, 
//h is nxn, k is the number of eig pairs to compute, starting from lowest
void harmonic_ritz(complexd *h, complexd *theta, complexd *g, int n, 
		   int *srt)
{
  
  int i;
  double *eignorms;
 
  eignorms = malloc(n*sizeof(double));
  zeig(h, theta, g, n);
  
  for(i=0;i<n;i++) {
    eignorms[i] = sqrt(theta[i].re*theta[i].re + theta[i].im*theta[i].im);
  }
  
  sorteigs(eignorms, srt, n);

  if(!dds_myrank) {
    fprintf(drlog, "\n eigenvalues :\n\n");
  }
  
  if(!dds_myrank) {

    for(i=0;i<n;i++) {
      fprintf(drlog, "\t %10.3e %10.3e norm = %10.3e\n", theta[srt[i]].re, theta[srt[i]].im, 
	      eignorms[srt[i]]);
    }
  
    fprintf(drlog, "\n\n");
  }

  free(eignorms);

} 


void build_projectors(complexd *g, complexd *p_k, int *srt, int n, int k)
{
  int i;
  int j;
  int n1 = n+1;
  complexd *r;
  complexd *pkr;
  complex c;

  //copy the first k gs into the first cols of p_k fortran layout!

  for(j=0;j<k;j++) {
    for(i=0;i<n;i++) {
      p_k[j*(n1)+i] = g[srt[j]*n+i];       //j loop sui vettori, i loop interno! change to srt[j]*n
    }
    p_k[j*(n1)+n].re = 0.0;
    p_k[j*(n1)+n].im = 0.0;

  }

  //put c - Hbary inside p_k[k*(n+1)]
  pkr = &p_k[k*(n1)];
  //do the expression on base V

  for (i=0;i<n1;i++) {
    c=spinor_prod(nsp, com, psq[i], psnr);
    pkr[i].re = c.re;
    pkr[i].im = c.im;
  }
  
  k++;

  r = malloc(k*k*sizeof(complexd));
  zqr_(p_k, r, &n1, &k);
  
  if(r[k*k-1].re < 0.0) {
    for(i=0;i<n1;i++) {
      pkr[i].re = -pkr[i].re;
      pkr[i].im = -pkr[i].im;
    }
  }
  
  free(r);
  
}

void insert_true_res(int nkv, int k)
{
  int i;
  complexd *c;
  double nrm;
  
  if(!dds_myrank) {
    fprintf(drlog, "inserting true res in ksp %d\n", k);
  }
  zero_rh(nkv); //optimize: zero only the first nit + 1 elements
  
  c = malloc((k+1)*sizeof(complexd));
  
  //psnr is the new true res
  //ortonormalize against new basis
  prj_ortho_V_cd(psnr, psq[k], psq, k, c);

  // psq[k] is the new krilov vector on which preconditioner will relax:
  // at restart, arnoldi iteration starts with this vector (j = k)
  nrm = normalize(nsp, com, psq[k]);
  
  for(i=0;i<k;i++) {
    rh[i].re = or_rh[i].re = c[i].re;
    rh[i].im = or_rh[i].im = c[i].im;
  }

  rh[k].re = or_rh[k].re = nrm; //lst.re;
  rh[k].im = or_rh[k].im = 0.0; //lst.im;
    
  free(c);

  //rh is the new RHS of the system to minimize (expression of the source in the new basis V)

}

void solve_ls(complexd *m, complexd *c, complexd *x, int rows, int cols)
{

  //lenght of c is nrows, lenght of x is ncols

  char trans;
  int nrhs = 1;
  complexd *myc;
  int i;
  int lwork = 2*rows*rows + 4*rows;
  complexd *work;
  int info;

  trans = 'N';
  //copy c;

  myc = malloc(rows*sizeof(complexd));
  work = malloc(lwork*sizeof(complexd));

  for(i=0;i<rows;i++) {
    myc[i] = c[i];
  }
  
  zgels_(&trans, &rows, &cols, &nrhs, m, &rows, myc, &rows, work, &lwork, &info); 

  for(i=0;i<cols;i++) {
    x[i] = myc[i];
  }
  
  free(work);
  free(myc);
 
}

#if 0

void consistency_check(void (*Op)(spinor_dble *pk,spinor_dble *pl), int nkv, int neig)
{

  //take the zs 
  //produce image in DP
  //convert to sp
  //do scalar products with vs
  //compare with H

  spinor *sf0;
  spinor_dble *sd0, *sd1;
  int i, j, k;

  sd0 = alloc_dsfield();
  sd1 = alloc_dsfield();
  
  sf0 = alloc_ssfield();

  float *f;
  double *d;

  for(j=0;j<neig;j++) {
  
    d = (double*)sd0;
    f = (float*)psz[j];
    for(i=0;i<nsp*24;i++) {
      d[i]=(double) f[i];
    }
        
    (*Op)(sd0,sd1);
        
    //convert to float
    
    convert_dble2float(sd1, sf0, nsp*24);
    
    complex cc;
    
    for(k=0;k<neig+1;k++) {
      cc = spinor_prod(nsp,com,psq[k],sf0);
      
      if(!dds_myrank) {
	fprintf(drlog, "D(z[%d]) * v[%d] = %11.3e %11.3e     %11.3e %11.3e\n", j, k, cc.re, cc.im, 
		or_H[k+ j*(nkv+1)].re, or_H[j*(nkv+1)+k].im);
	
      }
      
            
    }
  }
  
  if(!dds_myrank) {
    fflush(drlog);
  }
  
  free(sd0);
  free(sd1);
  free(sf0);

}

#endif
