!===============================================================================
!
! cg_matrix.F90 - solves "matrix_mult * x = b" and returns number of iterations
!
!-------------------------------------------------------------------------------
!
! Copyright (C) 1998-2008 Hinnerk Stueben
!               2008 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/>.
!
!-------------------------------------------------------------------------------
! CG for any given matrix
!
! Editor: Taylor Haar
! Last modified: 05/04/2017
!-------------------------------------------------------------------------------
# include "defs.h"

#ifdef PRECISION_R16
# define stop_condition stop_condition_r16
#endif

!-------------------------------------------------------------------------------
subroutine cg_matrix(x, b, id, mid, iter, res, matrix) 
  use module_function_decl
  use module_check_solution
  use module_action
  use module_cg
  use module_vol
  use module_counter
  implicit none

  SPINCOL_OVERINDEXED,  intent(inout) :: x
  SPINCOL_OVERINDEXED,  intent(in)    :: b
  integer,              intent(out)   :: iter
  integer,              intent(in)    :: id, mid
  DOUBLE,               intent(in)    :: res
  external :: matrix

!!  SPINCOL_OVERINDEXED :: r, p, aap
  REAL, allocatable ::r(:),p(:),aap(:)
  REAL          :: ak, bk
  DOUBLE        :: rtr, rtrold, paap, bb, dtmp, dtmp1, dtmp2
  integer       :: i, niter, ierr
  character(72) :: msg

!
! needed for checking solution
!
  DOUBLE        :: norm(3)
  integer, save :: headline_check_written = 0
  character(*), parameter :: key_chk = "%cgChk"
  character(*), parameter :: fmt_chk_h = "(1x, 2a, a6, 4a20)"
  character(*), parameter :: fmt_chk_b = "(1x, a7, i6, 3e20.10, i20)"

!-------------------------------------------------------------------------------

#ifdef IBM
  call alignx(16, x(1))
  call alignx(16, b(1))
#endif

  DEBUG2S("Start: cg")
  TIMING_START(timing_bin_cg)

  allocate(r(SIZE_COMPLEX*NDIRAC*NCOL*volh_tot), STAT = ierr)
  if (ierr /= 0) then
     call stderr2_int("allocation failed", 1, ierr)
     call die("cg: 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("cg: 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("cg: allocation failed aap")
  endif


  bb = 0.0_8
  !$omp parallel do reduction(+: bb) private(dtmp)
  do i = 1, size_sc_field
     dtmp = b(i)
     bb = bb + dtmp**2
  enddo
  call global_sum_vec(1, bb)

  DEBUG2I("fermiID    ",  id)
  DEBUG2R("norm(b)=   ",  sqrt(bb)/2/action%mtilde(id)%kappa)
  DEBUG2R("rsd    =   ",  dble(res)**2*bb/(2*action%mtilde(id)%kappa)**2)

  DEBUG2S("Start: cg 1")
  call matrix(r, x, id, mid)
  niter = 1

  rtrold = ZERO
  !$omp parallel do reduction(+: rtrold) private(dtmp)
  do i = 1, size_sc_field
     r(i) = b(i) - r(i)
     p(i) = r(i)
     dtmp = r(i)
     rtrold = rtrold + dtmp**2
  enddo
  call global_sum_vec(1, rtrold)

  if (stop_condition(rtrold, res, bb)) goto 9999

  bk=ZERO
  do niter = 1, cg_para%maxiter
     call matrix(aap, p, id, mid)
     paap = ZERO
     !$omp parallel do reduction(+: paap) private(dtmp1, dtmp2)
     do i = 1, size_sc_field
        dtmp1 = p(i)
        dtmp2 = aap(i)
        paap = paap + dtmp1 * dtmp2
     enddo
     call global_sum_vec(1, paap)
     ak = rtrold / paap 

     rtr = 0.0_8
     !$omp parallel do reduction(+: rtr) private(dtmp)
     do i = 1, size_sc_field
        x(i) = x(i) + ak * p(i)
        r(i) = r(i) - ak * aap(i)
        dtmp = r(i) 
        rtr = rtr + dtmp**2
     enddo 
     call global_sum_vec(1, rtr)

#ifdef DEBUG2
if(my_pe()==0 .and. mod(niter,100)==0  )write(0,*)"CG rtr iter       :",rtr, niter
#endif

     if (stop_condition(rtr, res, bb)) goto 9999
     bk = rtr / rtrold
     rtrold = rtr

     call sc_xpby(p, r, bk)  ! p = r + bk * p
  enddo

  niter = niter - 1

  if (cg_para%log /= 2) then
if(my_pe()==0) write(0,*)"no conv=============="
if(my_pe()==0) write(0,*)"p(1)=",p(1)
     call matrix(aap, p, id, mid)
if(my_pe()==0) write(0,*)"matrix aap(1)=",aap(1)
if(my_pe()==0) write(0,*)"matrix aap(2)=",aap(2)
if(my_pe()==0) write(0,*)"matrix aap(3)=",aap(3)
if(my_pe()==0) write(0,*)"matrix aap(4)=",aap(4)

     write(msg, *) "cg(): no convergence; rtr = ", rtr 
     call die(msg)
  endif

9999 continue

  call update_cg_stat(cg_stat, niter, 0)
  cg_iterations_total = cg_stat%niter_tot_run
  iter = niter

  TIMING_STOP(timing_bin_cg)

  if (cg_para%check /= 0) then  ! check solution

     ! This will not work with a custom matrix
     ! Must modify or cut
     call check_solution(id, x, b, norm)

     if (headline_check_written == 0) then
        if (my_pe() == 0) then
           write(UREC, fmt_chk_h) &
               "T", key_chk, "traj", "rtr", "Rb", "Rx", "iterations"
        endif
        headline_check_written = 1
     endif
     
     if (my_pe() == 0) then
        write(UREC, fmt_chk_b) &
           key_chk, counter%traj, rtr, &
           norm(1)/norm(2), norm(1)/norm(3), iter
     endif

  endif

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

  DEBUG2S("End: cg")
end

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