!===============================================================================
!
! multi_shift_cg.F90 - cg multi shift solver  ( A + a(i) ) x(i) = b
!
!-------------------------------------------------------------------------------
!
! Copyright (C) 2006-2010 Yoshifumi Nakamura
!
! This file is part of BQCD -- Berlin Quantum ChromoDynamics program
!
! BQCD is free software: you can redistribute it and/or modify
! it under the terms of the GNU General Public License as published by
! the Free Software Foundation, either version 3 of the License, or
! (at your option) any later version.
!
! BQCD is distributed in the hope that it will be useful,
! but WITHOUT ANY WARRANTY; without even the implied warranty of
! MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
! GNU General Public License for more details.
!
! You should have received a copy of the GNU General Public License
! along with BQCD.  If not, see <http://www.gnu.org/licenses/>.
!
!-------------------------------------------------------------------------------
! Added a complex-shift version of MCG for PF-HMC
!
! Editor: Taylor Haar
! Last modified: 29/09/2016
!-------------------------------------------------------------------------------
# include "defs.h"

!-------------------------------------------------------------------------------
subroutine multi_shift_cg(matrix, id, n, shift_switch, betabar, b, iterations, niter_shift, res) 
  use module_field
  use module_cg
  use module_function_decl
  use module_p_interface
  use module_vol
  use module_input
  implicit none
  SPINCOL_OVERINDEXED,           intent(in)  :: b
  integer,                       intent(out) :: iterations, niter_shift(n)
  integer,                       intent(in)  :: shift_switch(n), n, id
  REAL,                          intent(in)  :: betabar(n)
  REAL,                          intent(in) :: res(n)
  external :: matrix

  REAL, allocatable ::r(:),p(:),aap(:)
  REAL              :: alpha, alpha_old, beta, rtr, rtrold, paap, bb, sbeta, salpha
  REAL,dimension(n) :: sxi_old,sxi,sxi_new
  integer           :: i,j, niter, n_min, n_con(n), niter_sub ,   ierr, ncon
  character(72)     :: msg


  TIMING_START(timing_bin_cg_mcg)

  if (input%tuning_cg_version == 2) then

#ifdef PRECISION_R4
     call die("cg(): multi_shift_cg version 2 is not available for PRECISION_R4")
#elif PRECISION_R16
     call die("cg(): multi_shift_cg version 2 is not available for PRECISION_R16")
#else
     call cg_wrapper_mcg(matrix, id, n, shift_switch, betabar, b, iterations, niter_shift, res) 
#endif

  else  ! (input%cg_version /= 2)

  allocate(r(SIZE_COMPLEX*NDIRAC*NCOL*volh_tot), STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("allocation failed", 1, ierr)
     call die("cgm: allocation failed r")
  endif
  allocate(p(SIZE_COMPLEX*NDIRAC*NCOL*volh_tot), STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("allocation failed", 1, ierr)
     call die("cgm: allocation failed p")
  endif
  allocate(aap(SIZE_COMPLEX*NDIRAC*NCOL*volh_tot), STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("allocation failed", 1, ierr)
     call die("cgm: allocation failed aap")
  endif

  n_min = n
  do i = 1, n
     n_con(i) = shift_switch(i)
     if (shift_switch(i) == 0) n_min = min(n_min,i)
  enddo

  alpha_old=ONE
  beta=ZERO
  sxi_old  =ONE
  sxi      =ONE

  call sc_copy(r,b)
  rtrold = sc_norm2(r);rtrold = global_sum(rtrold)
  bb = rtrold

  call sc_copy(p,r)
  do i=1,n
     call sc_copy(sco(2,i)%sco(1),b)
     call sc_zero(sco(1,i)%sco(1))
  enddo

niter_sub=0
!----------------------------------------
  do niter = 1, cg_para%maxiter
     call matrix(aap, p, id)

     paap = sc_dot(p, aap) ; paap = global_sum(paap)
     alpha= - rtrold / paap

     call sc_axpy(r, aap, alpha)
     rtr = sc_norm2(r);rtr = global_sum(rtr)

     do j = 1,n
        if ( n_con(j) == 0) then
           sxi_new(j)=sxi(j)*sxi_old(j)*alpha_old/(alpha*beta*(sxi_old(j) &
                     -sxi(j))+sxi_old(j)*alpha_old*(ONE-betabar(j)*alpha))
           salpha=alpha*sxi_new(j)/sxi(j)
           call sc_axpy(sco(1,j)%sco(1), sco(2,j)%sco(1), -salpha)
           niter_sub = niter_sub + 1

           if (stop_condition(sxi_new(j)*sxi_new(j)*rtr, res(j), bb)) niter_shift(j)= niter
           if (stop_condition(sxi_new(j)*sxi_new(j)*rtr, res(j), bb)) n_con(j)=1
        endif
     enddo

#ifdef DEBUG2
if(my_pe()==0 .and. mod(niter,100)==0  )write(0,*)niter,"mcg:rtr",sxi_new(n_min)*sxi_new(n_min)*rtr
#endif
     if (stop_condition(sxi_new(n_min)*sxi_new(n_min)*rtr, res(n_min), bb)) then
        ncon=0
        do j = n_min, n
           ncon=ncon + n_con(j)
        enddo
        if (ncon == n - n_min +1) goto 9999
     endif

     beta = rtr / rtrold
     rtrold   = rtr
     call sc_xpby(p, r, beta)  ! p = r + beta * p

     do j = 1,n
        if ( n_con(j) == 0) then
           sbeta = beta * (sxi_new(j)/sxi(j))**2
           call sc_axpby(sco(2, j)%sco(1), r, sbeta, sxi_new(j))
           sxi_old(j)  =sxi(j)
           sxi(j)      =sxi_new(j)
        endif
     enddo

     alpha_old=alpha
  enddo
!----------------------------------------
  niter = niter - 1

  if (cg_para%log /= 2) then
     if(my_pe()==0)write(UREC, *) "cgm(): no convergence; rtr = ", rtr
     if(my_pe()==0)write(UREC, *) "b    = ",b(1)
     if(my_pe()==0)write(UREC, *) "r    = ",r(1)
     if(my_pe()==0)write(UREC, *) "aa   = ",sco(1,1)%sco(1)
     if(my_pe()==0)write(UREC, *) "pm   = ",sco(2,1)%sco(1)
     if(my_pe()==0)write(UREC, *) "alpha= ",alpha
     if(my_pe()==0)write(UREC, *) "beta = ",beta
     write(msg, *) "multi_shift_cg(): no convergence; rtr = ", rtr
     call die(msg)
  endif

9999 iterations = niter

  call update_cg_stat(cgm_stat, niter, niter_sub)

  deallocate(r, STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("deallocation failed", 1, ierr)
     call die("cgm: deallocation failed r")
  endif
  deallocate(p, STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("deallocation failed", 1, ierr)
     call die("cgm: deallocation failed p")
  endif
  deallocate(aap, STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("deallocation failed", 1, ierr)
     call die("cgm: deallocation failed aap")
  endif

  endif  ! (input%cg_version /= 2)

  if (cg_para%check /= 0) then
     call check_solution_shifts(matrix, id, n, shift_switch, betabar, niter_shift, b)
  endif

  TIMING_STOP(timing_bin_cg_mcg)

end

!-------------------------------------------------------------------------------
! Complex version of multi-shift CG
! for complex shifts
!-------------------------------------------------------------------------------

subroutine multi_shift_cgz(matrix, id, n, shift_switch, betabar, b, iterations, niter_shift, res)
  use module_field
  use module_cg
  use module_function_decl
  use module_p_interface
  use module_vol
  implicit none
  SPINCOL_OVERINDEXED,           intent(in)  :: b
  integer,                       intent(out) :: iterations, niter_shift(n)
  integer,                       intent(in)  :: shift_switch(n), n, id
  COMPLEX,                       intent(in)  :: betabar(n)
  REAL,                          intent(in)  :: res(n)
  external :: matrix

  REAL, allocatable ::r(:),p(:),aap(:)
  REAL              :: alpha, alpha_old, beta, rtr, rtrold, paap, bb
  COMPLEX           :: sbeta, salpha
  COMPLEX, dimension(n) :: sxi_old, sxi, sxi_new
  integer           :: i,j, niter, n_min, n_con(n), niter_sub, ierr, ncon
  character(72)     :: msg


  TIMING_START(timing_bin_cg_mcg)

  allocate(r(SIZE_COMPLEX*NDIRAC*NCOL*volh_tot), STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("allocation failed", 1, ierr)
     call die("cgm: allocation failed r")
  endif
  allocate(p(SIZE_COMPLEX*NDIRAC*NCOL*volh_tot), STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("allocation failed", 1, ierr)
     call die("cgm: allocation failed p")
  endif
  allocate(aap(SIZE_COMPLEX*NDIRAC*NCOL*volh_tot), STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("allocation failed", 1, ierr)
     call die("cgm: allocation failed aap")
  endif

  n_min = n
  do i = 1, n
     n_con(i) = shift_switch(i)
     if (shift_switch(i) == 0) n_min = min(n_min,i)
  enddo

  alpha_old=ONE
  beta=ZERO
  sxi_old  =ONE
  sxi      =ONE

  call sc_copy(r,b)
  rtrold = sc_norm2(r);rtrold = global_sum(rtrold)
  bb = rtrold

  call sc_copy(p,r)
  do i=1,n
     call sc_copy(sco(2,i)%sco(1),b)
     call sc_zero(sco(1,i)%sco(1))
  enddo

niter_sub=0
!----------------------------------------
  do niter = 1, cg_para%maxiter
     call matrix(aap, p, id)

     paap = sc_dot(p, aap) ; paap = global_sum(paap)
     alpha= - rtrold / paap

     call sc_axpy(r, aap, alpha)
     rtr = sc_norm2(r);rtr = global_sum(rtr)

     do j = 1,n
        if ( n_con(j) == 0) then
           sxi_new(j)=sxi(j)*sxi_old(j)*alpha_old/(alpha*beta*(sxi_old(j) &
                     -sxi(j))+sxi_old(j)*alpha_old*(ONE-betabar(j)*alpha))
           salpha=alpha*sxi_new(j)/sxi(j)
           call sc_caxpy(sco(1,j)%sco(1), sco(2,j)%sco(1), -salpha)
           niter_sub = niter_sub + 1

           if (stop_condition((Re(sxi_new(j))**2 + Im(sxi_new(j))**2)*rtr, &
                res(j), bb)) then
              niter_shift(j)= niter
              n_con(j)=1
           endif
        endif
     enddo

#ifdef DEBUG2
if(my_pe()==0 .and. mod(niter,100)==0  )write(0,*)niter,"mcg:rtr",sxi_new(n_min)*sxi_new(n_min)*rtr
#endif
     if (stop_condition((Re(sxi_new(n_min))**2 + Im(sxi_new(n_min))**2)*rtr, &
          res(n_min), bb)) then
        ncon=0
        do j = n_min, n
           ncon=ncon + n_con(j)
        enddo
        if (ncon == n - n_min +1) goto 9999
     endif

     beta = rtr / rtrold
     rtrold   = rtr
     call sc_xpby(p, r, beta)  ! p = r + beta * p

     do j = 1,n
        if ( n_con(j) == 0) then
           sbeta = beta * (sxi_new(j)/sxi(j))**2
           call sc_caxpby(sco(2, j)%sco(1), r, sbeta, sxi_new(j))
           sxi_old(j)  =sxi(j)
           sxi(j)      =sxi_new(j)
        endif
     enddo

     alpha_old=alpha
  enddo
!----------------------------------------
  niter = niter - 1

  if (cg_para%log /= 2) then
     if(my_pe()==0)write(UREC, *) "cgm(): no convergence; rtr = ", rtr 
     if(my_pe()==0)write(UREC, *) "b    = ",b(1)
     if(my_pe()==0)write(UREC, *) "r    = ",r(1)
     if(my_pe()==0)write(UREC, *) "aa   = ",sco(1,1)%sco(1)
     if(my_pe()==0)write(UREC, *) "pm   = ",sco(2,1)%sco(1)
     if(my_pe()==0)write(UREC, *) "alpha= ",alpha
     if(my_pe()==0)write(UREC, *) "beta = ",beta
     write(msg, *) "multi_shift_cgz(): no convergence; rtr = ", rtr 
     call die(msg)
  endif

9999 iterations = niter
  call update_cg_stat(cgm_stat, niter, niter_sub)

  deallocate(r, STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("deallocation failed", 1, ierr)
     call die("cgm: deallocation failed r")
  endif
  deallocate(p, STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("deallocation failed", 1, ierr)
     call die("cgm: deallocation failed p")
  endif
  deallocate(aap, STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("deallocation failed", 1, ierr)
     call die("cgm: deallocation failed aap")
  endif

  if (cg_para%check /= 0) then
     call check_solution_zshifts(matrix, id, n, shift_switch, betabar, niter_shift, b)
  endif
  
  TIMING_STOP(timing_bin_cg_mcg)

end subroutine multi_shift_cgz

!-------------------------------------------------------------------------------
!
! RESTARTED FULL ORTHOGONILIZATION METHOD FOR SHIFTED LINEAR SYSTEMS
! V. SIMONCIN, BIT Numerial Mathematics 43 (2003) 459-466
!
!-------------------------------------------------------------------------------
subroutine fom_shift(matrix, id, n, shift_switch, betabar, b, iterations, niter_shift, res) 
  use module_field
  use module_cg
  use module_function_decl
  use module_vol
  implicit none
  complex(8), intent(in) :: b(12*volh_tot)
  integer,    intent(out):: iterations, niter_shift(n)
  integer,    intent(in) :: shift_switch(n), n, id
  REAL,       intent(in) :: betabar(n)
  REAL,       intent(in) :: res(n)
  external :: matrix

  integer,parameter :: m = 3
  complex(8) :: x(12*volh_tot), r(12*volh_tot), v(12*volh_tot,m+1)
  complex(8) :: h1,h2, g1,g2,cs, w(12*volh_tot,m+1)
  complex(8) :: h(m+1,m), htmp(m+1,m), c(m+1), ctmp(m+1),s(m+1), stmp(m+1),d(m,n)
  real(8)    :: bb, rtr, rho(n), r2, sn, tmp, inv ,fac
  integer    :: i,j, niter, n_min, n_con(n), ncon, ns, scsize, niter_sub
  character(72) :: msg
  logical :: nsend(n)

#ifdef LAPACK

  complex(8)::work(80)
  integer :: info, lwork
  lwork=80

  TIMING_START(timing_bin_cg_mcg)
  scsize=12*volh_tot
  nsend=.false.
  niter_sub=0

  n_min = n
  do i = 1, n
     n_con(i) = shift_switch(i)
     if (shift_switch(i) == 0) n_min = min(n_min,i)
  enddo

  bb = sc_norm2(b) ; bb=global_sum(bb)
  r(1:scsize)=b(1:scsize)
  do ns=1, n
     call sc_zero(sco(1, ns)%sco(1))
     rho(ns)=ONE
  enddo

do niter=1, cg_para%maxiter

  rtr = sc_norm2(r) ; rtr=global_sum(rtr)
  inv=ONE/dsqrt(rtr)
  v(1:scsize,1) = r(1:scsize) * inv

  h=ZERO
  c=ZERO
  do j = 1, m
     call matrix(v(1,j+1), v(1,j), id)
     do i = 1, j
        h(i,j) = sc_cdotc(v(1,i), v(1,j+1))
        call global_sum_vec(2,h(i,j))
        v(1:scsize,j+1) = v(1:scsize,j+1) - h(i,j) * v(1:scsize,i)
     enddo

     tmp=sc_norm2(v(1,j+1)) ; tmp=global_sum(tmp)
     h(j+1,j)=dsqrt(tmp)
     inv = ONE/dsqrt(tmp)
     v(1:scsize,j+1)=v(1:scsize,j+1) * inv
  enddo

  htmp=h   ! keep h

  do ns = 1, n
  if (.not. nsend(ns)) then
     c=ZERO
     c(1)=rho(ns) * dsqrt(rtr)
     call hm_plus_a(h, htmp, betabar(ns),m)

     d(1:m,ns)=c(1:m)
     call zgels("N", m, m, 1, h(1:m,1:m), m, d(1:m,ns), m, work,lwork, info)
     if(info/=0)call die("zgels failed")

     rho(ns) = - htmp(m+1,m) * d(m,ns)

     call sc_copy(x,sco(1, ns)%sco(1))
     do j = 1, m
        x(1:scsize) = x(1:scsize) + v(1:scsize,j) * d(j,ns)
     enddo
     call sc_copy(sco(1, ns)%sco(1),x)

     niter_shift(ns)=niter*m
     nsend(ns)=stop_condition(rho(ns)**2, res(ns), bb)
     niter_sub = niter_sub + 1
  endif
  enddo   ! end of ns

  call sc_copy(r,v(1,m+1))

  if (nsend(n_min)) then
     ncon=0
     do i = n_min, n
        if (nsend(i)) ncon=ncon + 1
     enddo
     if (ncon == n - n_min +1) goto 9999
  endif
enddo

9999 iterations = niter
  if (cg_para%check /= 0) then
     call check_solution_shifts(matrix, id, n, shift_switch, betabar, niter_shift, b)
  endif
  call update_cg_stat(cgm_stat, niter, niter_sub)
  TIMING_STOP(timing_bin_cg_mcg)
#else
  call die("fom_shift needs lapack")
#endif
end

!-------------------------------------------------------------------------------
!
!
!Deflated GMRES for systems with multiple shifts
!        and multiple right-hand sides
!
!ref:LinearAlgebraAppl.429:2415-2434,2008 
!
! not yet finished
!
!-------------------------------------------------------------------------------
subroutine gmres_shift(matrix, id, n, shift_switch, betabar, b, iterations, niter_shift, res) 
  use module_field
  use module_cg
  use module_function_decl
  use module_vol
  implicit none
  complex(8), intent(in)  :: b(12*volh_tot)
  integer,    intent(out) :: iterations, niter_shift(n)
  integer,    intent(in)  :: shift_switch(n), n, id
  REAL,       intent(in)  :: betabar(n)
  REAL,       intent(in) :: res(n)
  external :: matrix

  integer,parameter :: m = 5
  complex(8) ::x(12*volh_tot), r(12*volh_tot), v(12*volh_tot,m+1), w(12*volh_tot,m+1)
  REAL         :: rtr, beta(n), r2, sn, tmp, inv
  integer      :: i,j, niter, n_min, n_con(n), ns, scsize, iter4
  character(72):: msg

  complex(8) :: h1,h2, g1,g2,cs
  complex(8) :: h(m+1,m), htmp(m+1,m), c(m+1), ctmp(m+1),s(m+1), stmp(m+1),d(m,n)
  logical :: nsend(n)

  TIMING_START(timing_bin_cg_mcg)
  scsize=12*volh
  nsend=.false.

  n_min = n
  do i = 1, n
     n_con(i) = shift_switch(i)
     if (shift_switch(i) == 0) n_min = min(n_min,i)
  enddo

  r = b 
  do ns=1,n
     call sc_zero(sco(1, ns)%sco(1))
  enddo
  beta=ONE

do niter=1, cg_para%maxiter
  c=ZERO
  s=ZERO
  h=ZERO
  rtr = sc_norm2(r) ; rtr=global_sum(rtr)
  if (rtr<res(1))go to 9999
  inv=ONE/sqrt(rtr)
  v(1:scsize,1) = r(1:scsize) * inv

  c(1)=sc_cdotc(v(1,1), r) ; call global_sum_vec(2,c(1))

  do j = 1, m

     call matrix(v(1,j+1), v(1,j), id)

     do i = 1, j
        h(i,j) = sc_cdotc(v(1,j+1), v(1,i)) ; call global_sum_vec(2,h(i,j))
        v(1:scsize,j+1) = v(1:scsize,j+1) - h(i,j) * v(1:scsize,i)
     enddo
     tmp=sc_norm2(v(1,j+1)) ; tmp=global_sum(tmp)
     h(j+1,j)=sqrt(tmp)
     inv=ONE/sqrt(tmp)
     v(1:scsize,j+1)=v(1:scsize,j+1) * inv
     c(j+1)=sc_cdotc(v(1,j+1), r) ; call global_sum_vec(2,c(j+1))
  enddo

  htmp=h !!!
  ctmp=c !!!

  !
  ! H + beta1 Im
  !
  call hm_plus_a(h, htmp, betabar(1),m)

  !
  ! solve d1
  !
  do i = 1, m
     r2 = ONE/sqrt( dconjg(h(i,i))*h(i,i) + dble(h(i+1,i))**2 )
     cs = h(i,  i) * r2
     sn = h(i+1,i) * r2
     do j = i, m
        h1 = dconjg(cs)*h(i,j) + sn*h(i+1,j)
        h2 =-       sn *h(i,j) + cs*h(i+1,j)
        h(i,  j) = h1
        h(i+1,j) = h2
     enddo
     g1 = dconjg(cs)*c(i) + sn*c(i+1)
     g2 =-       sn *c(i) + cs*c(i+1)
     c(i  ) = g1
     c(i+1) = g2
  enddo
  do i = m, 1, -1
     do j = i+1, m
        c(i) = c(i) - h(i,j) * d(j,1)
     enddo
     d(i,1) = c(i) / dble(h(i,i))
  enddo

  !
  ! H + beta1 Im
  !
  call hm_plus_a(h, htmp, betabar(1),m)

  !
  ! s = (Hm + beta1)d1
  !
  do i=1,m+1
     s(i)=ZERO
     do j=1,m
        s(i)=s(i)+h(i,j)*d(j,1)
     enddo
  enddo

  s=ctmp-s
  stmp=s

  do ns = 2, n
     if (nsend(ns)) cycle
     s=stmp
     c=ctmp * beta(ns)
     call hm_plus_a(h, htmp, betabar(ns),m)
     do i = 1, m
        r2 = ONE/sqrt( dconjg(h(i,i))*h(i,i) + dble(h(i+1,i))**2 )
        cs = h(i,  i)*r2
        sn = h(i+1,i)*r2
        do j = i, m
           h1 = dconjg(cs)*h(i,j) + sn*h(i+1,j)
           h2 =-       sn *h(i,j) + cs*h(i+1,j)
           h(i,  j) = h1
           h(i+1,j) = h2
        enddo
        g1 = dconjg(cs)*s(i) + sn*s(i+1)
        g2 =-       sn *s(i) + cs*s(i+1)
        s(i)   = g1
        s(i+1) = g2
        g1 = dconjg(cs)*c(i) + sn*c(i+1)
        g2 =-       sn *c(i) + cs*c(i+1)
        c(i)   = g1
        c(i+1) = g2
     enddo
     beta(ns) = c(m+1) / dble(s(m+1))
     c = c - beta(ns) * s
     do i = m, 1, -1
        do j = i+1, m
           c(i) = c(i) - h(i,j) * d(j,ns)
        enddo
        d(i,ns) = c(i) / dble(h(i,i))
     enddo
  enddo   ! end of ns

  do ns=n,1,-1
     if (nsend(ns)) cycle
     call sc_copy(x,sco(1, ns)%sco(1))
     do j = 1, m
        x(1:scsize) = x(1:scsize) + v(1:scsize,j) * d(j,ns)
!!!        x(1:scsize) = x(1:scsize) + w(1:scsize,j) * d(j,ns)
     enddo
     call sc_copy(sco(1, ns)%sco(1),x)
     niter_shift(ns)=niter
     nsend(ns)=(beta(ns)<res(ns))
  enddo

  do i=1,m+1
     s(i)=ZERO
     do j=1,m
        s(i)=s(i)+htmp(i,j)*d(j,1)
     enddo
  enddo
  do j = 1, m+1
     r(1:scsize) = r(1:scsize) - v(1:scsize,j) * s(j)
  enddo

enddo

9999 iterations = niter
  if (cg_para%check /= 0) then
     call check_solution_shifts(matrix, id, n, shift_switch, betabar, niter_shift, b)
  endif
!  call update_cg_stat(cgm_stat, niter, niter_sub)

!!!!!!!!!!!!!!!!!!!!!
stop !           STOP
!!!!!!!!!!!!!!!!!!!!!

  TIMING_STOP(timing_bin_cg_mcg)
end

!-------------------------------------------------------------------------------
subroutine check_solution_shifts(matrix, id, n, shift_switch, shifts, niter_shift, b)
  use module_field
  use module_vol
  use module_function_decl
  implicit none
  SPINCOL_OVERINDEXED, intent(in)  :: b
  integer,intent(in):: id, n, shift_switch(n),niter_shift(n)
  REAL, intent(in)  :: shifts(n)
  SPINCOL_OVERINDEXED :: x, r
  integer, save :: count=0
  integer :: i,j
  REAL::rtr

  if(my_pe()==0 .and. count==0) then
     write(UREC,400)"T%shift(  )",  " niter"," sum^Vol|b-Mx|^2"
     count=1
  endif

  do j = 1, n
     if ( shift_switch(j) == 1) goto 100
     call matrix(r, sco(1, j)%sco(1), id)

     rtr = ZERO
     !$omp parallel do reduction(+: rtr)
     do i = 1, size_sc_field
       r(i) = r(i) + shifts(j)*sco(1,j)%sco(i) - b(i)
       rtr = rtr + r(i)**2
     enddo
     rtr = global_sum(rtr)
    if(my_pe()==0)  write(UREC,410) "%shift(",j,")", niter_shift(j), rtr
  100 continue
  enddo
  400 format (1x,a11,      a6,1x,a16)
  410 format (2x,a7 ,i2,a1,i6,1x,es16.6)
end

!-------------------------------------------------------------------------------
subroutine check_solution_zshifts(matrix, id, n, shift_switch, shifts, niter_shift, b)
  use module_field
  use module_vol
  use module_function_decl
  implicit none
  SPINCOL_OVERINDEXED, intent(in)  :: b
  integer,intent(in):: id, n, shift_switch(n),niter_shift(n)
  COMPLEX, intent(in)  :: shifts(n)
  SPINCOL_OVERINDEXED :: x, r
  integer, save :: count=0
  integer :: i,j
  REAL::rtr

  if(my_pe()==0 .and. count==0) then
     write(UREC,400)"T%shift(  )",  " niter"," sum^Vol|b-Mx|^2"
     count=1
  endif

  do j = 1, n
     if ( shift_switch(j) == 1) goto 100
     call matrix(r, sco(1, j)%sco(1), id)

!write(*,*)b(1),b(2)
!write(*,*)r(1),r(2)
!write(*,*)shifts(j)
!write(*,*) sco(1,j)%sco(1) , sco(1,j)%sco(2) 
!!stop


     rtr = ZERO
     !$omp parallel do reduction(+: rtr)
     do i = 1, size_sc_field / 2, 2
       r(i  ) = r(i  ) + Re(shifts(j))* sco(1,j)%sco(i)   -  Im(shifts(j))* sco(1,j)%sco(i+1)   - b(i)
       r(i+1) = r(i+1) + Re(shifts(j))* sco(1,j)%sco(i+1) +  Im(shifts(j))* sco(1,j)%sco(i  )   - b(i+1)
       rtr = rtr + r(i)**2 + r(i+1)**2

!write(*,*)r(1),r(2),rtr
!stop

     enddo
     rtr = global_sum(rtr)
    if(my_pe()==0)  write(UREC,410) "%shift(",j,")", niter_shift(j), rtr
  100 continue
  enddo
  400 format (1x,a11,      a6,1x,a16)
  410 format (2x,a7 ,i2,a1,i6,1x,es16.6)
end

!-------------------------------------------------------------------------------
subroutine check_solution_zshifts_full(matrix, id, n, shift_switch, shifts, niter_shift, xe, xo, be, bo)
  use module_field
  use module_vol
  use module_function_decl
  implicit none
  complex(8), dimension (12*volh_tot, n), intent(out) :: xe, xo
  complex(8), dimension (12*volh_tot),    intent(in) :: be, bo
  integer,intent(in):: id, n, shift_switch(n),niter_shift(n)
  COMPLEX, intent(in)  :: shifts(n)
  complex(8), dimension (12*volh_tot) :: re, ro
  integer, save :: count=0
  integer :: i,j
  REAL::rtr

  if(my_pe()==0 .and. count==0) then
     write(UREC,400)"T%shift(  )",  " niter"," sum^Vol|b-Mx|^2"
     count=1
  endif

  do j = 1, n
     if ( shift_switch(j) == 1) goto 100
     call matrix(re, ro, xe(1,j), xo(1,j), id)
     rtr = ZERO
     !$omp parallel do reduction(+: rtr)
     do i = 1, 12*volh
        re(i) = re(i) + shifts(j) * xe(i,j)  - be(i)
        ro(i) = ro(i) + shifts(j) * xo(i,j)  - bo(i)
        rtr = rtr + Re(re(i))**2 + Im(re(i))**2+ Re(ro(i))**2 + Im(ro(i))**2
     enddo
     rtr = global_sum(rtr)
    if(my_pe()==0)  write(UREC,410) "%shift(",j,")", niter_shift(j), rtr
  100 continue
  enddo
  400 format (1x,a11,      a6,1x,a16)
  410 format (2x,a7 ,i2,a1,i6,1x,es16.6)
end

!===============================================================================
!
! some subroutines for this 
!
!-------------------------------------------------------------------------------
subroutine hm_plus_a(hout,hin,a,m)
  implicit none
  integer, intent(in) :: m
  real(8), intent(in) :: a
  complex(8), intent(in)  :: hin(m+1,m)
  complex(8), intent(out) :: hout(m+1,m)
  integer :: i

  hout = hin
  do i=1,m
     hout(i,i) = hout(i,i) + a
  enddo
end

!-------------------------------------------------------------------------------
subroutine hm_plus_z(hout,hin,z,m)
  implicit none
  integer, intent(in) :: m
  complex(8), intent(in)  :: z
  complex(8), intent(in)  :: hin(m+1,m)
  complex(8), intent(out) :: hout(m+1,m)
  integer :: i

  hout = hin
  do i=1,m
     hout(i,i) = hout(i,i) + z
  enddo
end

!===============================================================================

!-------------------------------------------------------------------------------
subroutine fom_zshift(matrix, id, n, shift_switch, betabar, b, iterations, niter_shift, res) 
  use module_field
  use module_cg
  use module_function_decl
  use module_vol
  implicit none
  complex(8), intent(in) :: b(12*volh_tot)
  integer,    intent(out):: iterations, niter_shift(n)
  integer,    intent(in) :: shift_switch(n), n, id
  complex(8), intent(in) :: betabar(n)
  REAL,       intent(in) :: res(n)
  external :: matrix

  integer,parameter :: m = 5
  complex(8) :: x(12*volh_tot), r(12*volh_tot), v(12*volh_tot,m+1)
  complex(8) :: h1,h2, g1,g2,cs, w(12*volh_tot,m+1)
  complex(8) :: h(m+1,m), htmp(m+1,m), c(m+1), ctmp(m+1),s(m+1), stmp(m+1),d(m,n)
  complex(8)    :: rho(n)
  real(8)    :: bb, rtr, r2, sn, tmp, inv ,fac
  integer    :: i,j, niter, n_min, n_con(n), ncon, ns, scsize, niter_sub
  character(72) :: msg
  logical :: nsend(n)

#ifdef LAPACK

  complex(8)::work(80)
  integer :: info, lwork
  lwork=80

  TIMING_START(timing_bin_cg_mcg)
  scsize=12*volh_tot
  nsend=.false.
  niter_sub=0

  n_min = n
  do i = 1, n
     n_con(i) = shift_switch(i)
     if (shift_switch(i) == 0) n_min = min(n_min,i)
  enddo

  bb = sc_norm2(b) ; bb=global_sum(bb)
  r(1:scsize)=b(1:scsize)
  do ns=1, n
     call sc_zero(sco(1, ns)%sco(1))
     rho(ns)=ONE
  enddo

do niter=1, cg_para%maxiter

  rtr = sc_norm2(r) ; rtr=global_sum(rtr)
  inv=ONE/dsqrt(rtr)
  v(1:scsize,1) = r(1:scsize) * inv

  h=ZERO
  c=ZERO
  do j = 1, m
     call matrix(v(1,j+1), v(1,j), id)
     do i = 1, j
        h(i,j) = sc_cdotc(v(1,i), v(1,j+1))
        call global_sum_vec(2,h(i,j))
        v(1:scsize,j+1) = v(1:scsize,j+1) - h(i,j) * v(1:scsize,i)
     enddo

     tmp=sc_norm2(v(1,j+1)) ; tmp=global_sum(tmp)
     h(j+1,j)=dsqrt(tmp)
     inv = ONE/dsqrt(tmp)
     v(1:scsize,j+1)=v(1:scsize,j+1) * inv
  enddo

  htmp=h   ! keep h

  do ns = 1, n
  if (.not. nsend(ns)) then
     c=ZERO
     c(1)=rho(ns) * dsqrt(rtr)
     call hm_plus_z(h, htmp, betabar(ns),m)

     d(1:m,ns)=c(1:m)
!write(*,*) d(1:m,ns)
     call zgels("N", m, m, 1, h(1:m,1:m), m, d(1:m,ns), m, work,lwork, info)
     if(info/=0)call die("zgels failed")

     rho(ns) = - htmp(m+1,m) * d(m,ns)

     call sc_copy(x,sco(1, ns)%sco(1))
     do j = 1, m
        x(1:scsize) = x(1:scsize) + v(1:scsize,j) * d(j,ns)
     enddo
     call sc_copy(sco(1, ns)%sco(1),x)

     niter_shift(ns)=niter*m
!!write(*,*)niter, ns,rho(ns),res(ns)
     nsend(ns)=stop_condition(Re(rho(ns))**2+Im(rho(ns))**2, res(ns), bb)
     niter_sub = niter_sub + 1
  endif
  enddo   ! end of ns

  call sc_copy(r,v(1,m+1))

  if (nsend(n_min)) then
     ncon=0
     do i = n_min, n
        if (nsend(i)) ncon=ncon + 1
     enddo
     if (ncon == n - n_min +1) goto 9999
  endif
enddo

9999 iterations = niter
  if (cg_para%check /= 0) then
     call check_solution_zshifts(matrix, id, n, shift_switch, betabar, niter_shift, b)
  endif
  call update_cg_stat(cgm_stat, niter, niter_sub)
  TIMING_STOP(timing_bin_cg_mcg)
#else
  call die("fom_shift needs lapack")
#endif
end

!-------------------------------------------------------------------------------
subroutine fom_zshift_full(matrix, id, n, shift_switch, betabar, be, bo, iterations, niter_shift, res) 
  use module_field
  use module_cg
  use module_function_decl
  use module_vol
  implicit none
  complex(8), intent(in) :: be(12*volh_tot), bo(12*volh_tot)
  integer,    intent(out):: iterations, niter_shift(n)
  integer,    intent(in) :: shift_switch(n), n, id
  complex(8), intent(in) :: betabar(n)
  REAL,       intent(in) :: res(n)
  external :: matrix

  integer,parameter :: m = 5
  complex(8) :: xe(12*volh_tot,n), re(12*volh_tot), ve(12*volh_tot,m+1), we(12*volh_tot,m+1)
  complex(8) :: xo(12*volh_tot,n), ro(12*volh_tot), vo(12*volh_tot,m+1), wo(12*volh_tot,m+1)
  complex(8) :: h1,h2, g1,g2,cs
  complex(8) :: h(m+1,m), htmp(m+1,m), c(m+1), ctmp(m+1),s(m+1), stmp(m+1),d(m,n)
  complex(8)    :: rho(n)
  real(8)    :: bb, rtr, r2, sn, tmp, inv ,fac
  integer    :: i,j, niter, n_min, n_con(n), ncon, ns, scsize, niter_sub
  character(72) :: msg
  logical :: nsend(n)


#ifdef LAPACK

  complex(8)::work(80)
  integer :: info, lwork
  lwork=80

  TIMING_START(timing_bin_cg_mcg)
  scsize=12*volh_tot
  nsend=.false.
  niter_sub=0

  n_min = n
  do i = 1, n
     n_con(i) = shift_switch(i)
     if (shift_switch(i) == 0) n_min = min(n_min,i)
  enddo

  bb = sc_norm2(be) + sc_norm2(bo) ; bb=global_sum(bb)
  re(1:scsize)=be(1:scsize)
  ro(1:scsize)=bo(1:scsize)
  do ns=1, n
     call sc_zero(xe(1,ns))
     call sc_zero(xo(1,ns))
     rho(ns)=ONE
  enddo

do niter=1, cg_para%maxiter

  rtr = sc_norm2(re) + sc_norm2(ro); rtr=global_sum(rtr)
  inv=ONE/dsqrt(rtr)
  ve(1:scsize,1) = re(1:scsize) * inv
  vo(1:scsize,1) = ro(1:scsize) * inv

  h=ZERO
  c=ZERO
  do j = 1, m
     call matrix(ve(1,j+1), vo(1,j+1), ve(1,j), vo(1,j), id)
     do i = 1, j
        h(i,j) = sc_cdotc(ve(1,i), ve(1,j+1)) +  sc_cdotc(vo(1,i), vo(1,j+1))
        call global_sum_vec(2,h(i,j))
        ve(1:scsize,j+1) = ve(1:scsize,j+1) - h(i,j) * ve(1:scsize,i)
        vo(1:scsize,j+1) = vo(1:scsize,j+1) - h(i,j) * vo(1:scsize,i)
     enddo

     tmp=sc_norm2(ve(1,j+1)) + sc_norm2(vo(1,j+1)) ; tmp=global_sum(tmp)
     h(j+1,j)=dsqrt(tmp)
     inv = ONE/dsqrt(tmp)
     ve(1:scsize,j+1)=ve(1:scsize,j+1) * inv
     vo(1:scsize,j+1)=vo(1:scsize,j+1) * inv
  enddo

  htmp=h   ! keep h

  do ns = 1, n
  if (.not. nsend(ns)) then
     c=ZERO
     c(1)=rho(ns) * dsqrt(rtr)
     call hm_plus_z(h, htmp, betabar(ns),m)

     d(1:m,ns)=c(1:m)
!write(*,*) d(1:m,ns)
     call zgels("N", m, m, 1, h(1:m,1:m), m, d(1:m,ns), m, work,lwork, info)
     if(info/=0)call die("zgels failed")

     rho(ns) = - htmp(m+1,m) * d(m,ns)

!     call sc_copy(xe,sco(1, ns)%sco(1))
!     call sc_copy(xo,sco(2, ns)%sco(1))
     do j = 1, m
        xe(1:scsize,ns) = xe(1:scsize,ns) + ve(1:scsize,j) * d(j,ns)
        xo(1:scsize,ns) = xo(1:scsize,ns) + vo(1:scsize,j) * d(j,ns)
     enddo
!     call sc_copy(sco(1, ns)%sco(1),xe)
!     call sc_copy(sco(2, ns)%sco(1),xo)

     niter_shift(ns)=niter*m

     nsend(ns)=stop_condition(Re(rho(ns))**2+Im(rho(ns))**2, res(ns), bb)
     niter_sub = niter_sub + 1
  endif
  enddo   ! end of ns

  call sc_copy(re,ve(1,m+1))
  call sc_copy(ro,vo(1,m+1))

  if (nsend(n_min)) then
     ncon=0
     do i = n_min, n
        if (nsend(i)) ncon=ncon + 1
     enddo
     if (ncon == n - n_min +1) goto 9999
  endif
enddo

9999 iterations = niter
  if (cg_para%check /= 0) then
     call check_solution_zshifts_full(matrix, id, n, shift_switch, betabar, niter_shift, xe, xo, be, bo)
  endif
  call update_cg_stat(cgm_stat, niter, niter_sub)
  TIMING_STOP(timing_bin_cg_mcg)
#else
  call die("fom_shift needs lapack")
#endif
end

