!===============================================================================
!
! solver.F90 for general small matrix and common subroutines
!
!-------------------------------------------------------------------------------
!
! Copyright (C) 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/>.
!
!-------------------------------------------------------------------------------
# include "defs.h"

!  external 
!             global_sum_vec
!             die
!  internal
!  real(8),   vecnorm2
!  complex(8) veccdotc

!-------------------------------------------------------------------------------
subroutine smallgmres(matrix, x, b, m, step)
  implicit none
  complex(8), dimension(m,m), intent(in) :: matrix
  complex(8), dimension(m),   intent(out):: x
  complex(8), dimension(m),   intent(in) :: b
  integer, intent(in) :: m, step
  integer :: iter, i,j
  real(8) :: bb, rtr, inv
  complex(8) :: r(m), v(m,step+1), h(step+1,step), y(step), g(step+1)
  real(8),    external :: vecnorm2
  complex(8), external :: veccdotc

!write(0,*)matrix
!write(0,*)x
!write(0,*)b
!stop

  x=b
  bb = vecnorm2(b, m)

  if (bb==ZERO)call die("bcgs was called zero source")

  do iter = 1, 1000*m**3
     call matvecmult(matrix, v(1,1), x ,m)
     v(:,1) = b(:) - v(:,1)

     rtr=vecnorm2(v(1,1),m)
     if (sqrt(rtr/bb) < 1d-14) goto 9999

     inv=ONE/sqrt(rtr)
     v(:,1)=v(:,1)*inv

     h=ZERO;g=ZERO;g(1)=sqrt(rtr)
     do j = 1, step
        call matvecmult(matrix, v(1,j+1), v(1,j), m)
        do i = 1, j
           h(i,j) = veccdotc(v(1,i), v(1,j+1), m)
           v(:,j+1) = v(:,j+1) - h(i,j) * v(:,i)
        enddo
        rtr=vecnorm2(v(:,j+1),m)
        inv=ONE/sqrt(rtr)
        v(:,j+1)=v(:,j+1)*inv
        h(j+1,j)=sqrt(rtr)
     enddo
     call min_gmhy(y,h,g,step)
     do j = 1, step
        x(:) = x(:) + v(:,j) * y(j)
     enddo
  enddo

  write(0, *) "smallgmres(): no convergence; rtr = ", rtr
  call die("aborted due to smallgmres no convergence")

9999 continue

!  call matvecmult(matrix,r,x,m)
!  r=r-b
!write(0,*)r
!write(0,*)i,rtr,bb,sqrt(rtr/bb)
!stop
end

!-------------------------------------------------------------------------------
subroutine bcgs(matrix, x, b, m) 
  implicit none
  complex(8), dimension(m,m), intent(in) :: matrix
  complex(8), dimension(m),   intent(out):: x
  complex(8), dimension(m),   intent(in) :: b
  integer, intent(in) :: m
!  integer, intent(out):: iter
  integer :: i
  real(8) :: bb!, rtr, rtrold
  complex(8) :: r(m), r0(m), p(m), ap(m), s(m), as(m), ak, bk, pap, ss1, ss2, omega, rtr,rtrold
  real(8),    external :: vecnorm2
  complex(8), external :: veccdotc

!write(0,*)matrix
!write(0,*)x
!write(0,*)b
!stop

  x=b
  bb = vecnorm2(b, m)

  if (bb==ZERO)call die("bcgs was called zero source")

  call matvecmult(matrix, r, x ,m)

  r=b-r; r0=r; p=r
  rtrold = vecnorm2(r,m)

  if ( sqrt(dble(rtrold)/bb) < 1d-32 ) goto 9999

  do i = 1, 100*m**3
     call matvecmult(matrix, ap, p, m)
     pap = veccdotc(ap,r0,m)

     ak = rtrold / pap
     s = r - ak * ap

     call matvecmult(matrix, as, s, m)
     ss1=veccdotc(s,as,m)
     ss2=veccdotc(as,as,m)
     omega = ss1/ss2

     x = x + ak * p + omega * s
     r = s          - omega * as

     ss1=veccdotc(r,r,m)
     ss2=veccdotc(r,r0,m)

     rtr =  ss1
     write(0,*)"bcgs",rtr,bb,sqrt(dble(rtr)/bb)

     if ( sqrt(dble(rtr)/bb) < 1d-32 ) goto 9999

     bk = ss2 / rtrold * ak / omega
     rtrold = ss2
     p = r + bk * (p - omega * ap)
  enddo

  write(0, *) "bcgs(): no convergence; rtr = ", rtr
  call die("aborted due to bcgs no convergence")

9999 continue

!  call matvecmult(matrix,r,x,m)
!  r=r-b
!write(0,*)r
!write(0,*)i,rtr,bb,sqrt(rtr/bb)
!stop
end

!-------------------------------------------------------------------------------
subroutine gram_schmidt(inout, r, len, n, global) ! and QR dcmp
  implicit none
  complex(8), dimension(len,n), intent(inout):: inout
  complex(8), dimension(n,n), intent(out):: r
  integer, intent(in) :: len,n
  real(8),   external :: vecnorm2
  complex(8),external :: veccdotc
  real(8)   :: xx
  complex(8):: zz
  logical :: global
  integer :: i, j

  r = ZERO
!  do i=1, n
!     xx=vecnorm2(inout(1,i), len)
!     if(global) call global_sum_vec(1, xx)
!     xx=ONE/sqrt(xx)
!     inout(:,i)=inout(:,i)*xx
!     do j=i+1,n
!        zz = veccdotc(inout(1,i), inout(1,j), len)
!        if(global) call global_sum_vec(2, zz)
!        inout(:,j) = inout(:,j) -zz * inout(:,i)
!     enddo
!  enddo

!!                same as above
  do i=1, n
     do j= 1,i-1
        zz = veccdotc(inout(1,j), inout(1,i), len)
        if(global) call global_sum_vec(2, zz)
        r(j,i)=zz
        inout(:,i) = inout(:,i) -zz * inout(:,j)
     enddo
     xx=vecnorm2(inout(1,i), len)
     if(global) call global_sum_vec(1, xx)
     r(i,i)=sqrt(xx)
     xx=ONE/Re(r(i,i))
     inout(:,i)=inout(:,i)*xx
  enddo

  xx = ZERO
  do i = 1, n
     do j = 1, n
        zz = veccdotc(inout(1,i), inout(1,j), len)
        if(global) call global_sum_vec(2, zz)
        if (i==j)then
           xx = xx + ( real(zz) - ONE )**2
           xx = xx + (aimag(zz) )**2
        else
           xx = xx + ( real(zz) )**2
           xx = xx + (aimag(zz) )**2
        endif
     enddo
  enddo
#ifdef DEBUG2
  write(0,*)"orthonormal check: len=",len,"n=",n,"global:",global,"res=",xx
#endif

end

!-------------------------------------------------------------------------------
subroutine gram_schmidt2(inout, r, lenall, len, n, global) ! and QR dcmp
  use module_function_decl
  implicit none
  complex(8), dimension(lenall,n), intent(inout):: inout
  complex(8), dimension(n,n), intent(out):: r
  integer, intent(in) :: lenall, len,n
  real(8),   external :: vecnorm2
  complex(8),external :: veccdotc
  real(8)   :: xx
  complex(8):: zz
  logical :: global
  integer :: i, j

  r = ZERO
  do i=1, n
     do j= 1,i-1
        zz = veccdotc(inout(1,j), inout(1,i), len)
        if(global) call global_sum_vec(2, zz)
        r(j,i)=zz
        inout(1:len,i) = inout(1:len,i) -zz * inout(1:len,j)
     enddo
     xx=vecnorm2(inout(1,i), len)
     if(global) call global_sum_vec(1, xx)
     r(i,i)=sqrt(xx)
     xx=ONE/r(i,i)
     inout(1:len,i)=inout(1:len,i)*xx
  enddo

!  xx = ZERO
!  do i = 1, n
!     do j = 1, n
!        zz = veccdotc(inout(1,i), inout(1,j), len)
!        if(global) call global_sum_vec(2, zz)
!        if (i==j)then
!           xx = xx + ( real(zz) - ONE )**2
!           xx = xx + (aimag(zz) )**2
!        else
!           xx = xx + ( real(zz) )**2
!           xx = xx + (aimag(zz) )**2
!        endif
!     enddo
!  enddo

!!  if (my_pe()==0) write(0,*)"orthonormal check: len=",len,"n=",n,"global:",global,"res=",xx
!!  call stderr2_real("gram_schmidt2:res=",1,xx)
#ifdef DEBUG2
  write(0,*)"orthonormal check: len=",len,"n=",n,"global:",global,"res=",xx
#endif

end

!-------------------------------------------------------------------------------
subroutine gram_schmidt_last(inout, len, n, global)! orthnormalize n-th vector
  implicit none                                    ! against 1:n-1 vectors
  complex(8), dimension(len,n), intent(inout):: inout
  integer, intent(in) :: len,n
  real(8),   external :: vecnorm2
  complex(8),external :: veccdotc
  real(8)   :: xx
  complex(8):: zz
  logical :: global
  integer :: i, j

  do i = 1, n-1
     zz = veccdotc(inout(1,i), inout(1,n), len)
     if(global) call global_sum_vec(2, zz)
     inout(:,n) = inout(:,n) - zz * inout(:,i)
  enddo
  xx=vecnorm2(inout(1,n), len)
  if(global) call global_sum_vec(1, xx)
  xx=ONE/sqrt(xx)
  inout(:,n)=inout(:,n)*xx

  xx = ZERO
  do i = 1, n
     zz = veccdotc(inout(1,i), inout(1,n), len)
     if(global) call global_sum_vec(2, zz)
     if (i==n)then
        xx = xx + ( real(zz) - ONE )**2
        xx = xx + (aimag(zz) )**2
     else
        xx = xx + ( real(zz) )**2
        xx = xx + (aimag(zz) )**2
     endif
  enddo
#ifdef DEBUG2
  write(0,*)"orthonormal check: len=",len,"n=",n,"global:",global,"res=",xx
#endif
end

!-------------------------------------------------------------------------------
!
! stand alone
!
!-------------------------------------------------------------------------------
subroutine min_gmhy(y, hin, gin, m)   !  H : upper Hessenberg
  implicit none                       !  return y as
  integer, intent(in) :: m            !  min |g - H y|
  complex(8), intent(in) :: gin(m+1)  !  by Givens rotation
  complex(8), dimension(m),   intent(out):: y
  complex(8), dimension(m+1,m), intent(in):: hin
  complex(8) :: h(m+1,m), g(m+1), h1, h2, g1, g2, cs
  real(8) :: sn, r2
  integer :: i, j

  h=hin
  g=gin
  do i = 1, m
     r2 = ONE/sqrt( dble(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)*g(i) + sn*g(i+1)
     g2 =-      sn *g(i) + cs*g(i+1)
     g(i)   = g1
     g(i+1) = g2
  enddo

  do i = m, 1, -1
     do j = i+1, m
        g(i) = g(i) - h(i,j) * y(j)
     enddo
     y(i) = g(i) / dble(h(i,i))
  enddo

end
!-------------------------------------------------------------------------------
subroutine matvecmult(matrix, out, in ,m) 
  implicit none
  integer, intent(in) :: m
  complex(8), dimension(m,m), intent(in):: matrix
  complex(8), dimension(m),   intent(out):: out
  complex(8), dimension(m),   intent(in) :: in
  integer :: i, j

  out=dcmplx(ZERO,ZERO)
  do i = 1, m
  do j = 1, m
     out(i)=out(i)+matrix(i,j)*in(j)
  enddo
  enddo
end
!-------------------------------------------------------------------------------
subroutine matvecmult_add(matrix, out, in ,m) 
  implicit none
  integer, intent(in) :: m
  complex(8), dimension(m,m), intent(in):: matrix
  complex(8), dimension(m),   intent(inout):: out
  complex(8), dimension(m),   intent(in) :: in
  integer :: i, j

  do i = 1, m
  do j = 1, m
     out(i)=out(i)+matrix(i,j)*in(j)
  enddo
  enddo
end
!-------------------------------------------------------------------------------
real(8) function vecnorm2(in ,m) 
  implicit none
  integer, intent(in) :: m
  complex(8), dimension(m),   intent(in) :: in
  real(8) :: tmp
  integer :: i

  tmp=ZERO
  do i = 1, m
     tmp=tmp+conjg(in(i))*in(i)
  enddo
  vecnorm2=tmp
end
!-------------------------------------------------------------------------------
complex(8) function veccdotc(in1,in2,m) 
  implicit none
  integer, intent(in) :: m
  complex(8), dimension(m),   intent(in) :: in1, in2
  complex(8) :: tmp
  integer :: i

  tmp=dcmplx(ZERO,ZERO)
  do i = 1, m
     tmp=tmp+conjg(in1(i))*in2(i)
  enddo
  veccdotc=tmp
end
!===============================================================================
