subroutine conjgrad_tridiag(diag,subdiag,supdiag,x0,b,n,eps)
      implicit none

      real*8 amult,x0,eps,diag,supdiag,subdiag,b
      integer n,nmax

      real*8 alpha,beta,err,newerr,sum
      real*8 p,r
      integer i,k

      parameter(nmax=1000)

      dimension x0(*),b(*)
      dimension p(nmax),r(nmax)

c      write (*,*) diag, subdiag, supdiag
c      pause

c *** Performs a CG algorithm for a tridiagonal matrix with 
c     constant value diag on the diagonal, constant value subdiag
c     on the sub-diagonal and constant value supdiag on the super-diagonal.
c     On entry, x0 is the guess, b is the RHS. On exit, x0 is the updated
c     solution to precision eps. n is the real dimension of A. nmax must
c     be set to a value greater than n. Note that b is used as working 
c     array in the routine so cannot be re-used in calling program.
      
c *** Initialization of the CG algorithm
      
      do i=1,n
         p(i)=b(i)-amult(diag,subdiag,supdiag,n,x0,i)
         r(i) = p(i)
      enddo
            
      alpha = 0.d0
      beta = 0.d0

c     *** Calculate r_0^T r_0
      err = 0.d0
      do i=1,n
         err = err + r(i)**2
      enddo
      if(dsqrt(err).lt.eps) goto 80
            
      do k=1,n
         
c     ****** Store Ap_k in b
         do i=1,n
            b(i) = amult(diag,subdiag,supdiag,n,p,i)
         enddo

c ****** Calculate alpha_k
         sum = 0.d0
         do i=1,n
            sum = sum + b(i)*p(i)
         enddo
         alpha = err/sum

c ****** Update x and r
         do i=1,n
            x0(i) = x0(i) + alpha*p(i)
            r(i) = r(i) - alpha*b(i)
         enddo

c ****** Calculate r_k^T r_k
         newerr = 0.d0
         do i=1,n
            newerr = newerr + r(i)**2
         enddo
         if(dsqrt(newerr).lt.eps) goto 80
               
c ****** Update beta and err
         beta = newerr/err
         err = newerr
         
c ****** Update p_k
         do i=1,n
            p(i) = r(i) + beta*p(i)
         enddo
         
      enddo
      
 80   continue
      return
      end


c     ***********************************************************
c
c
c
c
c     ***********************************************************

      subroutine conjgrad_2d_lat(diag,xdiag,ydiag,x0,b,
     *     nX, nY, nMax, eps, bYBoundaries)
      implicit none

      real*8 multLat2d,x0,eps,
     *     diag,xdiag,ydiag,b
      logical bForwardsTest
      logical bYBoundaries
      integer nX, nY, nTimes, nMax

      real*8 alpha,beta,err,newerr,sum
      real*8 lastErr
      real*8 p,r
      real*8 x,y
      real*8 bTmp
      integer i,j,k, nIterCnt

c      parameter(nmax=1000)

      dimension x0(0:nMax+1,0:nMax+1),b(0:nMax+1,0:nMax+1)

      dimension p(0:nx+1,0:ny+1),r(0:nx+1,0:ny+1)
      dimension x(0:nx+1), y(0:ny+1)

      nIterCnt = 0
c     For performance evaluation,
c     count the number of iterations it took

c *** Performs a CG algorithm for a 2d lattice matrix with 
c     constant value diag on the diagonal, etc
c     On entry, x0 is the guess, b is the RHS. On exit, x0 is the updated
c     solution to precision eps. nX,nY is the real dimension of A. 
c     nmax must
c     be set to a value greater than nX*nY. 
c     Note that b is used as working 
c     array in the routine so cannot be re-used in calling program.
      
c *** Initialization of the CG algorithm
      

c      write (*,*) nX, nY, nMax
c      write (*,*) "nX, nY, nMax"
c      pause

      call set_const_2d(p, nx, ny, nx, 0.d0)
      do j=1,nY
         do i=1,nX
            p(i,j)=b(i,j)-multLat2d(
     *           diag,xdiag,ydiag,nX,nY,
     *           nMax,
     *           x0,i,j)
            r(i,j) = p(i,j)
         enddo
      enddo

c      do i=1,nX
c         p(i,0) = x0(i,0)
c         p(i,nY) = x0(i,nY)
c      enddo

c      do j=1,nY
c         p(0,j) = x0(0,j)
c         p(nX,j) = x0(nX,j)
c      enddo

c      write (*,*) "Here"

c      call step(x, 0.d0, 1.d0, nx, .TRUE.)
c      call step(y, 0.d0, 1.d0, ny, .FALSE.)


      alpha = 0.d0
      beta = 0.d0
      
c     *** Calculate r_0^T r_0
      err = 0.d0
      do j=1,nY
         do i=1,nX
            err = err + r(i,j)**2
         enddo
      enddo

      lastErr = err
c      write (*,*) "Here (2)"

      if(dsqrt(err).lt.eps) goto 80
      
c      write (*,*) "Here (2.5)"
      
      nTimes = nX*nY
      do k=1,nTimes
         
c     ****** Store Ap_k in b
         do j=1,nY
            do i=1,nX
               b(i,j)=multLat2d(
     *              diag,xdiag,ydiag,nX,nY,
     *              nX, 
     *              p,i,j)
            enddo
         enddo
         

c         write (*,*) "Here (3)"
         
c     ****** Calculate alpha_k
         sum = 0.d0
         do j=1,nY
            do i=1,nX
               sum = sum + b(i,j)*p(i,j)
            enddo
         enddo
         alpha = err/sum
         
c     ****** Update x and r
         do j=1,nY
            do i=1,nX
               x0(i,j) = x0(i,j) + 
     *              alpha*p(i,j)
               r(i,j) = r(i,j) - 
     *              alpha*b(i,j)
            enddo
         enddo
         
c     ****** Calculate r_k^T r_k
         newerr = 0.d0
         do j=1,nY
            do i=1,nX
               newerr = newerr + 
     *              r(i,j)**2
            enddo
         enddo
         
         if(newerr.GT.lasterr) then
            if(newerr.GT.1.d10) then
               write (*,*) "Error is ", newerr, " at iteration ", k
               write (*,*) "sum is ",sum
               write (*,*) "err is ", err
               write (*,*) "alpha is ", alpha
               write (*,*) "diag, xdiag, ydiag are "
               write (*,*) diag, xdiag, ydiag
               pause "Check various vars"
            endif
         endif

         if(mod(k,1000) .EQ.0) then
            write (*,*) "On iter ",k
         endif

         lasterr = newerr

         nIterCnt = nIterCnt+1

         if(dsqrt(newerr).lt.eps) goto 80
         
c     ****** Update beta and err
         beta = newerr/err
         err = newerr
         
c     ****** Update p_k
         do j=1,nY
            do i=1,nX
               p(i,j) = r(i,j) + 
     *              beta*p(i,j)
            enddo
         enddo
         
      enddo
      
 80   continue
      

      write (*,*) "Finished a conjgrad in ",
     *     nIterCnt

      return
      end