!                                               *****************
!                                               *    MAT2.FOR   *
!                                               *  PURSER 1993  *
!                                               *****************
!    Routines for linear inversions etc.

!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE LUFM                                            C
!  perform l-u decomposition of square matrix a in place with                  C
!  partial pivoting                                                            C
!  For DOUBLE PRECISION version see DLUFM                                      C
!                                                                              C
!  --> a    square matrix to be factorized                                     C
!  <-- ipiv array encoding the pivoting sequence                               C
!  <-- d    indicator for possible sign change of determinant                  C
!  --> m    degree of (active part of) a                                       C
!  --> na   first fortran dimension of a                                       C
!                                                                              C
!------------------------------------------------------------------------------C
      subroutine LUFM(a,ipiv,d,m,na)
      dimension a(na,*),ipiv(*)
      d=1.
      ipiv(m)=m
      do j=1,m-1
       jp=j+1
       abig=abs(a(j,j))
       ibig=j
       do i=jp,m
        aa=abs(a(i,j))
        if(aa.gt.abig)then
         ibig=i
         abig=aa
        endif
       enddo
!  swap rows, recording changed sign of determinant
       ipiv(j)=ibig
       if(ibig.ne.j)then
        d=-d
        do k=1,m
         t=a(j,k)
         a(j,k)=a(ibig,k)
         a(ibig,k)=t
        enddo
       endif
       ajj=a(j,j)
       if(ajj.eq.0.)then
         jm=j-1
	      print *, ' failure in lufact: matrix singular, rank=', jm
                   stop
       endif
       ajji=1./ajj
       do i=jp,m
        aij=ajji*a(i,j)
        a(i,j)=aij
        do k=jp,m
         a(i,k)=a(i,k)-aij*a(j,k)
        enddo
       enddo
      enddo
      return
      end

!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE INVMM                                           C
!  invert matrix, possibly in place (a=b), using the l-u decomposition method  C
!  For DOUBLE PRECISION version see DINVMM                                     C
!                                                                              C
!  --> b    square matrix to be inverted                                       C
!  <-- a    inverse of b                                                       C
!  --> m    degree of (active part of) b and a                                 C
!  --> nb   first fortran dimension of b                                       C
!  --> na   first fortran dimension of a                                       C
!                                                                              C
!   LIMITATION:                                                                C
!    ipiv is an index array, internal to this array, encoding the              C
!    pivoting sequence used. It is given a fortran dimension of NN=500         C
!    in the parameter statement below. If the order of the linear system       C
!    exceeds 500, increase this parameter accordingly                          C
!                                                                              C
!------------------------------------------------------------------------------C
      subroutine INVMM(b,a,m,nb,na)
      PARAMETER (NN=500)
      DIMENSION IPIV(NN)
      dimension a(na,*),b(nb,*)
      do j=1,m
      do i=1,m
       a(i,j)=b(i,j)
      enddo
      enddo
      call lufm(a,ipiv,d,m,na)
!  invert u in place:
      do i=1,m
       a(i,i)=1./a(i,i)
      enddo
      do i=1,m-1
       do j=i+1,m
        s=0.
        do k=i,j-1
         s=s-a(i,k)*a(k,j)
        enddo
        a(i,j)=a(j,j)*s
       enddo
      enddo
!  invert l in place assuming implicitly diagonal elements of unity
      do j=1,m-1
       do i=j+1,m
        s=-a(i,j)
        do k=j+1,i-1
         s=s-a(i,k)*a(k,j)
        enddo
        a(i,j)=s
       enddo
      enddo
!  form the product of u**-1 and l**-1 in place
      do j=1,m-1
       do i=1,j
        s=a(i,j)
        do k=j+1,m
         s=s+a(i,k)*a(k,j)
        enddo
        a(i,j)=s
       enddo
       do i=j+1,m
        s=0.
        do k=i,m
         s=s+a(i,k)*a(k,j)
        enddo
        a(i,j)=s
       enddo
      enddo
!  permute columns according to ipiv
      do j=m-1,1,-1
       l=ipiv(j)
       do i=1,m
        t=a(i,j)
        a(i,j)=a(i,l)
        a(i,l)=t
       enddo
      enddo
      return

!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE LINVMM                                          C
!  invert linear systems sharing the same square system matrix                 C
!  For DOUBLE PRECISION version see DLINVMM                                    C
!                                                                              C
!  <-> a    square system matrix on entry, its L-D-U factorization on return   C
!  <-> b    right-hand-sides on entry, corresponding matrix of solution        C
!           vectors on return                                                  C
!  --> m    degree of (active part of) b and a                                 C
!  --> mm   number of right-hand-side vectors (active columns of b)            C
!  --> na   first fortran dimension of a                                       C
!  --> nb   first fortran dimension of b                                       C
!                                                                              C
!------------------------------------------------------------------------------C
      entry                            LINVMM(a,b,m,mm,na,nb)
      call lufm(a,ipiv,d,m,na)
      call lubmm(a,b,ipiv,m,mm,na,nb)
      return
      end


!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE LUBMM                                           C
!  use l-u factors in a to back-substitute for mm rhs in b, using ipiv to      C
!  define the pivoting permutation used in the l-u decomposition.              C
!  For DOUBLE PRECISION version see DLUBMM                                     C
!                                                                              C
!  --> a    L-D-U factorization of linear system matrux                        C
!  <-> b    right-hand-sides on entry, corresponding matrix of solution        C
!           vectors on return                                                  C
!  --> ipiv array encoding the pivoting sequence                               C
!  --> m    degree of (active part of) b and a                                 C
!  --> mm   number of right-hand-side vectors (active columns of b)            C
!  --> na   first fortran dimension of a                                       C
!  --> nb   first fortran dimension of b                                       C
!                                                                              C
!------------------------------------------------------------------------------C
      subroutine LUBMM(a,b,ipiv,m,mm,na,nb)
      dimension a(na,*),b(nb,*),ipiv(*)
      do k=1,mm !loop over columns of b
       do i=1,m
        l=ipiv(i)
        s=b(l,k)
        b(l,k)=b(i,k)
        do j=1,i-1
         s=s-a(i,j)*b(j,k)
        enddo
        b(i,k)=s
       enddo
       do i=m,1,-1
        s=b(i,k)
        do j=i+1,m
         s=s-a(i,j)*b(j,k)
        enddo
        b(i,k)=s/a(i,i)
       enddo
      enddo
      return
      end

!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE INVHH                                           C
!  Invert, possibly in place, a symmetric matrix                               C
!  For DOUBLE PRECISION version see DINVHH                                     C
!                                                                              C
!  --> B    symmetric square input matrix                                      C
!  <-- A    inverse of B                                                       C
!  --> M    degree of (active part of) A and B                                 C
!  --> NB   first fortran dimension of B                                       C
!  --> NA   first fortran dimension of A                                       C
!                                                                              C
!  LIMITATION                                                                  C
!     This routine incorporates no pivoting - it is intended for matrices      C
!     that are already diagonally dominant                                     C
!                                                                              C
!------------------------------------------------------------------------------C
      SUBROUTINE INVHH(B,A,M,NB,NA)
      DIMENSION B(NB,*),A(NA,*)
      DO I=1,M
      DO J=1,M
      A(I,J)=B(I,J)
      ENDDO
      ENDDO
!-----------------------------------------------------------------------
!  PERFORM L.D.U DECOMPOSITION OF SYMMETRIC MATRIX ( U = L* )
!-----------------------------------------------------------------------
      DO J=1,M
       JM=J-1
       AJJ=A(J,J)
       DO K=1,JM
        AJJ=AJJ-A(K,K)*A(J,K)**2
       ENDDO
       A(J,J)=AJJ
       DO I=J+1,M
        AIJ=A(I,J)
        DO K=1,JM
         AIJ=AIJ-A(I,K)*A(K,K)*A(J,K)
        ENDDO
        A(I,J)=AIJ/AJJ
        A(J,I)=0.
       ENDDO
      ENDDO
!-----------------------------------------------------------------------
!  INVERT (IN PLACE) THE LOWER TRIANGULAR PART OF A, (ASSUMING UNIT
!  DIAGONAL ELEMENTS), AND INVERT THE DIAGONAL PART OF A (ASSUMING
!  ZERO OFF-DIAGONAL ELEMNTS)
!-----------------------------------------------------------------------
      DO K=1,M
       KP=K+1
       A(K,K)=1./A(K,K)
       IM=K
       DO I=KP,M
        AIK=A(I,K)
        DO J=KP,IM
         AIK=AIK+A(I,J)*A(J,K)
        ENDDO
        A(I,K)=-AIK
        IM=I
       ENDDO
      ENDDO
!-----------------------------------------------------------------------
!  MULTIPLY: THE TRANSPOSE OF THE LOWER PART OF A (ASSUMING UNIT DIAGS),
!  TIMES THE DIAGONAL PART (ASSUMING ZERO OFF-DIAGS), TIMES THE LOWER
!  PART. THIS PRODUCT IS THE SYMMETRIC INVERSE OF THE ORIGINAL B.
!-----------------------------------------------------------------------
      DO I=1,M
       AII=A(I,I)
       DO J=1,I-1
        AIJ=AII*A(I,J)
        DO K=I+1,M
         AIJ=AIJ+A(K,I)*A(K,K)*A(K,J)
        ENDDO
        A(I,J)=AIJ
        A(J,I)=AIJ
       ENDDO
       DO K=I+1,M
        AII=AII+A(K,I)**2*A(K,K)
       ENDDO
       A(I,I)=AII
      ENDDO
      RETURN
      END

!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE DLUFM                                           C
!  perform l-u decomposition of square matrix a in place with                  C
!  partial pivoting IN DOUBLE PRECISION                                        C
!  For SINGLE PRECISION version see LUFM                                       C
!                                                                              C
!  --> a    square matrix to be factorized                                     C
!  <-- ipiv array encoding the pivoting sequence                               C
!  <-- d    indicator for possible sign change of determinant                  C
!  --> m    degree of (active part of) a                                       C
!  --> na   first fortran dimension of a                                       C
!                                                                              C
!------------------------------------------------------------------------------C
      subroutine DLUFM(a,ipiv,d,m,na)
      implicit double precision (a-h,o-z)
      dimension a(na,*),ipiv(*)
      d=1.
      ipiv(m)=m
      do j=1,m-1
       jp=j+1
       abig=dabs(a(j,j))
       ibig=j
       do i=jp,m
        aa=dabs(a(i,j))
        if(aa.gt.abig)then
         ibig=i
         abig=aa
        endif
       enddo
!  swap rows, recording changed sign of determinant
       ipiv(j)=ibig
       if(ibig.ne.j)then
        d=-d
        do k=1,m
         t=a(j,k)
         a(j,k)=a(ibig,k)
         a(ibig,k)=t
        enddo
       endif
       ajj=a(j,j)
       if(ajj.eq.0.d0)then
                           jm=j-1
	      print *, ' failure in lufact: matrix singular, rank=', jm
       endif
       ajji=1./ajj
       do i=jp,m
        aij=ajji*a(i,j)
        a(i,j)=aij
        do k=jp,m
         a(i,k)=a(i,k)-aij*a(j,k)
        enddo
       enddo
      enddo
      return
      end

!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE DINVMM                                          C
!  invert matrix, possibly in place (a=b), using the l-u decomposition method  C
!  in DOUBLE PRECISION (for single precision version see INVMM                 C
!                                                                              C
!  --> b    square matrix to be inverted                                       C
!  <-- a    inverse of b                                                       C
!  --> m    degree of (active part of) b and a                                 C
!  --> nb   first fortran dimension of b                                       C
!  --> na   first fortran dimension of a                                       C
!                                                                              C
!   LIMITATION:                                                                C
!    ipiv is an index array, internal to this array, encoding the              C
!    pivoting sequence used. It is given a fortran dimension of NN=500         C
!    in the parameter statement below. If the order of the linear system       C
!    exceeds 500, increase this parameter accordingly                          C
!                                                                              C
!------------------------------------------------------------------------------C
      subroutine DINVMM(b,a,m,nb,na)
      PARAMETER (NN=500)
      implicit double precision (a-h,o-z)
      DIMENSION IPIV(NN)
      dimension a(na,*),b(nb,*)
      do j=1,m
      do i=1,m
       a(i,j)=b(i,j)
      enddo
      enddo
      call DLUFM(a,ipiv,d,m,na)
!  invert u in place:
      do i=1,m
       a(i,i)=1./a(i,i)
      enddo
      do i=1,m-1
       do j=i+1,m
        s=0.
        do k=i,j-1
         s=s-a(i,k)*a(k,j)
        enddo
        a(i,j)=a(j,j)*s
       enddo
      enddo
!  invert l in place assuming implicitly diagonal elements of unity
      do j=1,m-1
       do i=j+1,m
        s=-a(i,j)
        do k=j+1,i-1
         s=s-a(i,k)*a(k,j)
        enddo
        a(i,j)=s
       enddo
      enddo
!  form the product of u**-1 and l**-1 in place
      do j=1,m-1
       do i=1,j
        s=a(i,j)
        do k=j+1,m
         s=s+a(i,k)*a(k,j)
        enddo
        a(i,j)=s
       enddo
       do i=j+1,m
        s=0.
        do k=i,m
         s=s+a(i,k)*a(k,j)
        enddo
        a(i,j)=s
       enddo
      enddo
!  permute columns according to ipiv
      do j=m-1,1,-1
       l=ipiv(j)
       do i=1,m
        t=a(i,j)
        a(i,j)=a(i,l)
        a(i,l)=t
       enddo
      enddo
      return

!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE DLINVMM                                         C
!  invert linear systems sharing the same square system matrix in              C
!  DOUBLE PRECISION. (For single precision version see LINVMM                  C
!                                                                              C
!  <-> a    square system matrix on entry, its L-D-U factorization on return   C
!  <-> b    right-hand-sides on entry, corresponding matrix of solution        C
!           vectors on return                                                  C
!  --> m    degree of (active part of) b and a                                 C
!  --> mm   number of right-hand-side vectors (active columns of b)            C
!  --> na   first fortran dimension of a                                       C
!  --> nb   first fortran dimension of b                                       C
!                                                                              C
!------------------------------------------------------------------------------C
      entry                            DLINVMM(a,b,m,mm,na,nb)
      call DLUFM(a,ipiv,d,m,na)
      call DLUBMM(a,b,ipiv,m,mm,na,nb)
      return
      end


!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE DUBMM                                           C
!  use l-u factors in a to back-substitute for mm rhs in b, using ipiv to      C
!  define the pivoting permutation used in the l-u decomposition               C
!  in DOUBLE PRECISION. (For single precision version see LUBMM                C
!                                                                              C
!  --> a    L-D-U factorization of linear system matrux                        C
!  <-> b    right-hand-sides on entry, corresponding matrix of solution        C
!           vectors on return                                                  C
!  --> ipiv array encoding the pivoting sequence                               C
!  --> m    degree of (active part of) b and a                                 C
!  --> mm   number of right-hand-side vectors (active columns of b)            C
!  --> na   first fortran dimension of a                                       C
!  --> nb   first fortran dimension of b                                       C
!                                                                              C
!------------------------------------------------------------------------------C
      subroutine DLUBMM(a,b,ipiv,m,mm,na,nb)
      implicit double precision (a-h,o-z)
      dimension a(na,*),b(nb,*),ipiv(*)
      do k=1,mm !loop over columns of b
       do i=1,m
        l=ipiv(i)
        s=b(l,k)
        b(l,k)=b(i,k)
        do j=1,i-1
         s=s-a(i,j)*b(j,k)
        enddo
        b(i,k)=s
       enddo
       do i=m,1,-1
        s=b(i,k)
        do j=i+1,m
         s=s-a(i,j)*b(j,k)
        enddo
        b(i,k)=s/a(i,i)
       enddo
      enddo
      return
      end

!------------------------------------------------------------------------------C
!   R.J.Purser, National Meteorological Center, Washington D.C.  1993          C
!                   SUBROUTINE DINVHH                                          C
!  Invert, possibly in place, a symmetric matrix in DOUBLE PRECISION.          C
!  (For single precision version see INVHH                                     C
!                                                                              C
!  --> B    symmetric square input matrix                                      C
!  <-- A    inverse of B                                                       C
!  --> M    degree of (active part of) A and B                                 C
!  --> NB   first fortran dimension of B                                       C
!  --> NA   first fortran dimension of A                                       C
!                                                                              C
!  LIMITATION                                                                  C
!     This routine incorporates no pivoting - it is intended for matrices      C
!     that are already diagonally dominant                                     C
!                                                                              C
!------------------------------------------------------------------------------C
      SUBROUTINE DINVHH(B,A,M,NB,NA)
      implicit double precision (a-h,o-z)
      DIMENSION B(NB,*),A(NA,*)
      DO I=1,M
      DO J=1,M
      A(I,J)=B(I,J)
      ENDDO
      ENDDO
!-----------------------------------------------------------------------
!  PERFORM L.D.U DECOMPOSITION OF SYMMETRIC MATRIX ( U = L* )
!-----------------------------------------------------------------------
      DO J=1,M
       JM=J-1
       AJJ=A(J,J)
       DO K=1,JM
        AJJ=AJJ-A(K,K)*A(J,K)**2
       ENDDO
       A(J,J)=AJJ
       DO I=J+1,M
        AIJ=A(I,J)
        DO K=1,JM
         AIJ=AIJ-A(I,K)*A(K,K)*A(J,K)
        ENDDO
        A(I,J)=AIJ/AJJ
        A(J,I)=0.
       ENDDO
      ENDDO
!-----------------------------------------------------------------------
!  INVERT (IN PLACE) THE LOWER TRIANGULAR PART OF A, (ASSUMING UNIT
!  DIAGONAL ELEMENTS), AND INVERT THE DIAGONAL PART OF A (ASSUMING
!  ZERO OFF-DIAGONAL ELEMNTS)
!-----------------------------------------------------------------------
      DO K=1,M
       KP=K+1
       A(K,K)=1./A(K,K)
       IM=K
       DO I=KP,M
        AIK=A(I,K)
        DO J=KP,IM
         AIK=AIK+A(I,J)*A(J,K)
        ENDDO
        A(I,K)=-AIK
        IM=I
       ENDDO
      ENDDO
!-----------------------------------------------------------------------
!  MULTIPLY: THE TRANSPOSE OF THE LOWER PART OF A (ASSUMING UNIT DIAGS),
!  TIMES THE DIAGONAL PART (ASSUMING ZERO OFF-DIAGS), TIMES THE LOWER
!  PART. THIS PRODUCT IS THE SYMMETRIC INVERSE OF THE ORIGINAL B.
!-----------------------------------------------------------------------
      DO I=1,M
       AII=A(I,I)
       DO J=1,I-1
        AIJ=AII*A(I,J)
        DO K=I+1,M
         AIJ=AIJ+A(K,I)*A(K,K)*A(K,J)
        ENDDO
        A(I,J)=AIJ
        A(J,I)=AIJ
       ENDDO
       DO K=I+1,M
        AII=AII+A(K,I)**2*A(K,K)
       ENDDO
       A(I,I)=AII
      ENDDO
      RETURN
      END