program test_pent
      implicit none
      save

c..test the pentadiagonal and cyclic pentadiagonal solvers

c..declare
      integer          i,np
      parameter        (np=7)
      double precision a(np),b(np),c(np),d(np),e(np),u(np),rhs(np),
     1                 x(np),cp1,cp2,cp3,cp4,cp5,cp6


      data   a/0.0d0, 0.0d0, 0.2d0, 0.9d0, 4.0d0, 2.2d0, 1.1d0/
      data   b/0.0d0, 3.6d0, 2.8d0, 3.1d0, 6.7d0, 1.2d0, 0.1d0/
      data   c/4.1d0, 2.2d0, 6.2d0, 8.5d0, 3.8d0, 3.7d0, 2.1d0/
      data   d/0.4d0, 1.0d0, 5.0d0, 4.9d0, 2.3d0, 5.1d0, 0.0d0/
      data   e/0.5d0, 6.1d0, 2.9d0, 4.5d0, 0.7d0, 0.0d0, 0.0d0/
      data rhs/7.8d0, 2.9d0, 6.7d0, 8.0d0, 9.0d0, 3.3d0, 0.8d0/

      data   cp1/0.2d0/, cp2/0.5d0/, cp3/0.4d0/, 
     1       cp4/0.1d0/, cp5/0.9d0/, cp6/1.0d0/


c..formats
 01   format(1x,1p7e10.2)
 02   format(1x,i4,1pe10.2)
 03   format(1x,i4,1p3e10.2)


c..first test the pentadiagonal solver

      write(6,*)
      write(6,*) 'pentdag matrix:'
      write(6,01) c(1),d(1),e(1),0.0,0.0,0.0,0.0
      write(6,01) b(2),c(2),d(2),e(2),0.0,0.0,0.0
      write(6,01) a(3),b(3),c(3),d(3),e(3),0.0,0.0
      write(6,01) 0.0,a(4),b(4),c(4),d(4),e(4),0.0
      write(6,01) 0.0,0.0,a(5),b(5),c(5),d(5),e(5)
      write(6,01) 0.0,0.0,0.0,a(6),b(6),c(6),d(6)
      write(6,01) 0.0,0.0,0.0,0.0,a(7),b(7),c(7)


c..solve the cyclic pentadiagonal matrix
      call pentdag(a,b,c,d,e,rhs,u,np)


c..write the solution
      write(6,*) 
      write(6,*) 'solution'
      write(6,02) (i, u(i), i=1,np)


c..test the solution
      x(1) = c(1)*u(1) + d(1)*u(2) + e(1)*u(3) 
      x(2) = b(2)*u(1) + c(2)*u(2) + d(2)*u(3) + e(2)*u(4)
      x(3) = a(3)*u(1) + b(3)*u(2) + c(3)*u(3) + d(3)*u(4) + e(3)*u(5)
      x(4) = a(4)*u(2) + b(4)*u(3) + c(4)*u(4) + d(4)*u(5) + e(4)*u(6)
      x(5) = a(5)*u(3) + b(5)*u(4) + c(5)*u(5) + d(5)*u(6) + e(5)*u(7)
      x(6) = a(6)*u(4) + b(6)*u(5) + c(6)*u(6) + d(6)*u(7) 
      x(7) = a(7)*u(5) + b(7)*u(6) + c(7)*u(7) 

      write(6,*) 
      write(6,*) 'residuals in x-rhs column should be small:'
      write(6,*) '   i   x         rhs       x-rhs'
      write(6,03) (i, x(i), rhs(i), x(i)-rhs(i), i=1,np)





c..now test the cyclic pentadiagonal solver
      write(6,*)
      write(6,*)
      write(6,*) 'cyclic pentdag matrix:'
      write(6,01) c(1),d(1),e(1),0.0,0.0,cp1,cp2
      write(6,01) b(2),c(2),d(2),e(2),0.0,0.0,cp3
      write(6,01) a(3),b(3),c(3),d(3),e(3),0.0,0.0
      write(6,01) 0.0,a(4),b(4),c(4),d(4),e(4),0.0
      write(6,01) 0.0,0.0,a(5),b(5),c(5),d(5),e(5)
      write(6,01) cp4,0.0,0.0,a(6),b(6),c(6),d(6)
      write(6,01) cp5,cp6,0.0,0.0,a(7),b(7),c(7)


c..solve the cyclic pentadiagonal matrix
      call cypent(a,b,c,d,e,rhs,cp1,cp2,cp3,cp4,cp5,cp6,u,np)


c..write the solution
      write(6,*) 
      write(6,*) 'solution'
      write(6,02) (i, u(i), i=1,np)

c..test the solution
      x(1) = c(1)*u(1) + d(1)*u(2) + e(1)*u(3) + cp1*u(6)  + cp2*u(7)
      x(2) = b(2)*u(1) + c(2)*u(2) + d(2)*u(3) + e(2)*u(4) + cp3*u(7)
      x(3) = a(3)*u(1) + b(3)*u(2) + c(3)*u(3) + d(3)*u(4) + e(3)*u(5)
      x(4) = a(4)*u(2) + b(4)*u(3) + c(4)*u(4) + d(4)*u(5) + e(4)*u(6)
      x(5) = a(5)*u(3) + b(5)*u(4) + c(5)*u(5) + d(5)*u(6) + e(5)*u(7)
      x(6) = cp4*u(1)  + a(6)*u(4) + b(6)*u(5) + c(6)*u(6) + d(6)*u(7) 
      x(7) = cp5*u(1)  + cp6*u(2)  + a(7)*u(5) + b(7)*u(6) + c(7)*u(7) 

      write(6,*) 
      write(6,*) 'residuals in x-rhs column should be small:'
      write(6,*) '   i   x         rhs       x-rhs'
      write(6,03) (i, x(i), rhs(i), x(i)-rhs(i), i=1,np)

      end









c..pentadiagonal 
c..routine pentdag solves pentadiagonal systems 
c..routine cypent solves pentdiagonal + corners via woodbury formula



      subroutine pentdag(a,b,c,d,e,f,u,n) 
      implicit none
      save

c..solves for a vector u of length n in the pentadiagonal linear system 
c.. a_i u_(i-2) + b_i u_(i-1) + c_i u_i + d_i u_(i+1) + e_i u_(i+2) = f_i 
c..input are the a, b, c, d, e, and f and they are not modified 

c..in its clearest incarnation, this algorithm uses three storage arrays 
c..called p, q and r. here, the solution vector u is used for r, cutting 
c..the extra storage down to two arrays. 

c..declare the pass
      integer          n
      double precision a(n),b(n),c(n),d(n),e(n),f(n),u(n)

c..local variables
      integer          nmax,i 
      parameter        (nmax=500) 
      double precision p(nmax),q(nmax),bet,den 


c..initialize elimination and backsubstitution arrays 
      if (c(1) .eq. 0.0)  stop 'eliminate u2 trivially' 
      bet  = 1.0d0/c(1) 
      p(1) = -d(1) * bet 
      q(1) = -e(1) * bet 
      u(1) = f(1)  * bet 

      bet = c(2) + b(2)*p(1) 
      if (bet .eq. 0.0) stop 'singular 1 in pentdag' 
      bet = -1.0d0/bet 
      p(2) = (d(2) + b(2)*q(1)) * bet 
      q(2) = e(2) * bet 
      u(2) = (b(2)*u(1) - f(2)) * bet 


c..reduce to upper triangular 
      do i=3,n 
       bet = b(i) + a(i) * p(i-2) 
       den = c(i) + a(i)*q(i-2) + bet*p(i-1) 
       if (den .eq. 0.0) stop 'singular 2 in pentdag' 
       den = -1.0d0/den 
       p(i) = (d(i) + bet*q(i-1)) * den 
       q(i) = e(i) * den 
       u(i) = (a(i)*u(i-2) + bet*u(i-1) - f(i)) * den 
      enddo

c..backsubstitution 
      u(n-1) = u(n-1) + p(n-1) * u(n) 
      do i=n-2,1,-1 
       u(i) = u(i) + p(i) * u(i+1) + q(i) * u(i+2) 
      enddo
      return 
      end 






      subroutine cypent(a,b,c,d,e,f,cp1,cp2,cp3,cp4,cp5,cp6,x,n)
      implicit none
      save

c..solves for x(1:n) a pentadiagonal matrix with nonzero entries in the 
c..lower left and upper right corners of the matrix:

c..  x    x    x    0    0  ... 0  cp1  cp2
c..  x    x    x    x    0  ... 0  0    cp3
c..  .    .    .    .    .  .   .  .    . 
c..  cp4  0    0    ...  0  0   x  x    x
c..  cp5  cp6  0    ...  0  x   x  x    x

c..the woodbury formula is applied to the pentadiagonal matrix. 


c..declare the pass
      integer          n
      double precision a(n),b(n),c(n),d(n),e(n),f(n),
     1                 cp1,cp2,cp3,cp4,cp5,cp6,x(n) 


c..local variables
      integer          i,j,k,nmax 
      parameter        (nmax=500) 
      integer          indx(nmax)
      double precision u(nmax,4),v(nmax,4),z(nmax,4),
     1                 r(nmax),s(nmax),y(nmax),
     2                 h(4,4),p(4,4),sum


c..popular formats
 01   format(1x,1p7e10.2)



c..initialize 
      if (n .le. 2) stop 'n < 2 in routine cypent' 
      if (n .gt. nmax) stop 'n > nmax in routine cypent' 

      do j=1,4
       do i=1,n
        u(i,j) = 0.0d0
        v(i,j) = 0.0d0
        z(i,j) = 0.0d0
       enddo
      enddo

      u(1,1)   = 1.0d0
      u(2,2)   = 1.0d0
      u(n-1,3) = 1.0d0
      u(n,4)   = 1.0d0

      v(n-1,1) = cp1
      v(n,1)   = cp2
      v(n,2)   = cp3
      v(1,3)   = cp4
      v(1,4)   = cp5
      v(2,4)   = cp6


c..solve the auxillary systems
c..recipies equation 2.7.17 and 2.7.20
      call pentdag(a,b,c,d,e,u(1,1),z(1,1),n) 
      call pentdag(a,b,c,d,e,u(1,2),z(1,2),n) 
      call pentdag(a,b,c,d,e,u(1,3),z(1,3),n) 
      call pentdag(a,b,c,d,e,u(1,4),z(1,4),n) 
      call pentdag(a,b,c,d,e,f,y,n) 


c..form the 4x4 matrix h
c..recipies equation 2.7.19

      do j=1,4
       do i=1,4
        sum = 0.0d0
        do k=1,n
         sum = sum + v(k,j) * z(k,i)
        end do
        p(j,i) = sum
       end do
      enddo
      do i=1,4
       p(i,i) = p(i,i) + 1.0d0
      enddo
      call luinv(p,4,4,indx,h) 

c..form the solution
c..recipe equation 2.7.21
      
      do j=1,4
       r(j) = 0.0d0
       do k=1,n
        r(j) = r(j) + v(k,j) * y(k)
       enddo
      enddo

      do j=1,4
       s(j) = 0.0d0
       do k=1,4
        s(j) = s(j) + h(j,k) * r(k)
       enddo
      enddo

      do j=1,n
       sum = 0.0d0
       do k=1,4
        sum = sum + z(j,k) * s(k)
       enddo
       x(j) = y(j) - sum
      enddo

      return 
      end 









      subroutine ludcmp(a,n,np,indx,d) 
      implicit none
      save

c..given the matrix a(n,n), with physical dimsnsions a(np,ap) this routine
c..replaces a by the lu decompostion of a row-wise permutation of itself. 
c..input are a,n,np. output is a, indx which records the row 
c..permutations effected by the partial pivoting, and d which is 1 if 
c..the number of interchanges is even, -1 if odd. 
c..use routine lubksb to solve a system of linear equations.
c.. 
c..nmax is the largest expected value of n 
c.. 
c..declare 
      integer          n,np,indx(np),nmax,i,j,k,imax 
      parameter        (nmax=500) 
      double precision a(np,np),d,tiny,vv(nmax),aamax,sum,dum 
      parameter        (tiny=1.0d-20) 


c..vv stores the implicit scaling of each row 
c..loop over the rows to get the scaling information 
      d = 1.0d0 
      do i=1,n 
       aamax = 0.0d0 
       do j=1,n 
        if (abs(a(i,j)) .gt. aamax) aamax = abs(a(i,j)) 
       enddo
       if (aamax .eq. 0.0) stop 'singular matrix in ludcmp' 
       vv(i) = 1.0d0/aamax 
      enddo

c..for each column apply crouts method; see equation 2.3.12 
      do j=1,n 
       do i=1,j-1 
        sum = a(i,j) 
        do k=1,i-1 
         sum = sum - a(i,k)*a(k,j) 
        enddo
        a(i,j) = sum 
       enddo

c..find the largest pivot element 
       aamax = 0.0d0 
       do i=j,n 
        sum = a(i,j) 
        do k=1,j-1 
         sum = sum - a(i,k)*a(k,j) 
        enddo
        a(i,j) = sum 
        dum = vv(i)*abs(sum) 
        if (dum .ge. aamax) then 
         imax  = i 
         aamax = dum 
        end if 
       enddo

c..if we need to interchange rows 
       if (j .ne. imax) then 
        do k=1,n 
         dum       = a(imax,k) 
         a(imax,k) = a(j,k) 
         a(j,k)    = dum 
        enddo
        d          = -d 
        vv(imax)   = vv(j) 
       end if 

c..divide by the pivot element 
       indx(j) = imax 
       if (a(j,j) .eq. 0.0) a(j,j) = tiny 
       if (j .ne. n) then 
        dum = 1.0d0/a(j,j) 
        do i=j+1,n 
         a(i,j) = a(i,j)*dum 
        enddo
       end if 

c..and go back for another column of crouts method
      enddo
      return 
      end 




      subroutine lubksb(a,n,np,indx,b) 
      implicit none
      save

c..solves a set of n linear equations ax=b. a is input in its lu decomposition 
c..form, determined by the routine above ludcmp. indx is input as the 
c..permutation vector also returned by ludcmp. b is input as the right hand 
c..side vector and returns with the solution vector x. 
c..a,n ans np are not modified by this routine and thus can be left in place 
c..for successive calls (i.e matrix inversion) 
c.. 
c..declare 
      integer           n,np,indx(np),i,ii,j,ll 
      double precision  a(np,np),b(np),sum 

c..when ii is > 0, ii becomes the index of the first nonzero element of b 
c..this is forward substitution of equation 2.3.6, and unscamble in place
      ii = 0 
      do i=1,n 
       ll = indx(i) 
       sum = b(ll) 
       b(ll) = b(i) 
       if (ii .ne. 0) then 
        do j=ii,i-1 
         sum = sum - a(i,j) * b(j) 
        enddo

c..nonzero element was found, so dos the sums in the loop above 
       else if (sum .ne. 0.0) then 
        ii  = i 
       end if 
       b(i) = sum 
      enddo

c..back substitution equation 2.3.7 
      do i = n,1,-1 
       sum = b(i) 
       if (i .lt. n) then 
        do j=i+1,n 
         sum = sum - a(i,j) * b(j) 
        enddo
       end if 
       b(i) = sum/a(i,i) 
      enddo
      return 
      end 




      subroutine luinv(a,n,np,indx,y) 
      implicit none
      save

c..this routine takes as input the n by n matrix a, of physical dimension 
c..np by np and on output fills y with the inverse of a 
c.. 
c..declare 
      integer           n,np,i,j,indx(np) 
      double precision  a(np,np),y(np,np),d 

c..set y to the identity matrix
      do j=1,n 
       do i=1,n 
        y(i,j) = 0.0d0 
       enddo
      enddo
      do i=1,n
       y(i,i) = 1.0d0 
      enddo

c..decomp and backsubstitute each column
      call ludcmp(a,n,np,indx,d) 
      do j=1,n 
       call lubksb(a,n,np,indx,y(1,j)) 
      enddo
      return 
      end