MODULE M_PENTA
    IMPLICIT NONE
    PRIVATE
!
    PUBLIC PENTA
!
    INTERFACE PENTA
        MODULE PROCEDURE PENTA1, PENTA2
    END INTERFACE    
!    
! PARAMETERS---
    REAL(8), PARAMETER :: ONE = 1.0D0
    REAL(8), PARAMETER :: ZERO = 0.0D0
!    REAL(8), PARAMETER :: PIV_TOL = EPSILON(ONE)
!
    REAL(8), PARAMETER :: UNDERFLOW = TINY(ONE)
    REAL(8), PARAMETER :: OVERFLOW  = HUGE(ONE)
!
!   ERROR CODES
!
    INTEGER, PUBLIC, PARAMETER :: ERR_NOERROR =  0
    INTEGER, PUBLIC, PARAMETER :: ERR_DATA    = -1
    INTEGER, PUBLIC, PARAMETER :: ERR_MATINV  = -2
    INTEGER, PUBLIC, PARAMETER :: ERR_UNFLO   = -3    
!    
! VARIABLES---
    REAL(8) :: PIV_MIN
    REAL(8) :: PIV_TOL
!        
CONTAINS    
!
!   ***************************************************************************!
!
    SUBROUTINE PENTA1( N, A, B, C, D, E, F, IER, PAR)
    !
    ! PURPOSE---
    !   SOLUTION OF PENTA-DIAGONAL SYSTEM BY THOMAS ALGORITHM
    !
    ! ARGUMENTS---
        INTEGER, INTENT(IN) :: N
        REAL(8), INTENT(INOUT) :: A(*), B(*), C(*), D(*), E(*) ! Destroyed
        REAL(8), INTENT(INOUT) :: F(*)  ! RHS / Solution 
        INTEGER, INTENT(OUT) :: IER     ! 0=OK, -1=Data, -2=Fact, -3=Back
    ! 
    ! OPTIONAL ARGUMENTS--- 
        REAL(8), OPTIONAL, INTENT(IN) :: PAR(4) ! 1=Alpha, 2=Beta, 3=Gamma, 4=Delta
    !
    ! LOCAL SCALARS---
        INTEGER :: K
        REAL(8) :: ALP, BET, GAM, DEL
        REAL(8) :: A11, A12, A21, A22, F1, F2, B1, T, U, V
    !
    ! START-OF-CODE
    !        
    !   INITIALIZATION
    !
        IER = 0
        IF (N <= 4) THEN
            IER = ERR_DATA
            RETURN
        ENDIF    
        PIV_TOL = UNDERFLOW           
        B1 = B(1)
    !
        IF (PRESENT(PAR)) THEN
            ALP = PAR(1)
            BET = PAR(2)
            GAM = PAR(3)
            DEL = PAR(4)
        ELSE
            ALP = ONE
            BET = -ONE
            GAM = ONE
            DEL = -ONE
        ENDIF 
    !
        C(1)   = C(1)   - BET*D(N)/ALP
        D(1)   = D(1)   - BET*E(N)/ALP
        B(2)   = B(2)   - DEL*E(N-1)/GAM
        D(N-1) = D(N-1) - GAM*A(2)/DEL
        B(N)   = B(N)   - ALP*A(1)/BET
        C(N)   = C(N)   - ALP*B(1)/BET
    !
    !   FACTORIZATION
    !
        C(1) = ONE/C(1)
        D(1) = C(1)*D(1)
        E(1) = C(1)*E(1)
        T = (C(2) - B(2)*D(1))
        IF (ABS(T) <= PIV_TOL) THEN
            IER = ERR_MATINV
            RETURN
        ENDIF
        C(2) = ONE/T    
        D(2) = C(2)*(D(2) - B(2)*E(1))
        E(2) = C(2)*E(2)
        DO K = 3, N
            B(K) = B(K) - A(K)*D(K-2)
            T = C(K) - B(K)*D(K-1) - A(K)*E(K-2)
            IF (ABS(T) <= PIV_TOL) THEN
                IER = ERR_MATINV
                RETURN
            ENDIF
            C(K) = ONE/T
            IF (K == N) EXIT
            D(K) = C(K)*(D(K) - B(K)*E(K-1))
            IF (K == N - 1) CYCLE
            E(K) = C(K)*E(K)                
        ENDDO
    !            
    !   INTERMEDIATE SOLUTION
    !
        F(1) = C(1)*F(1)
        C(1) = C(1)/ALP
        B(1) = ZERO
        T = C(2)
        F(2) =  T*(F(2) - B(2)*F(1))
        C(2) = -T*B(2)*C(1)
        B(2) =  T/GAM
        DO K = 3, N
            T = C(K)
            F(K) =  T*(F(K) - B(K) *F(K-1) - A(K)*F(K-2))
            C(K) = -T*(B(K)*C(K-1) + A(K)*C(K-2))
            B(K) = -T*(B(K)*B(K-1) + A(K)*B(K-2))
            IF ( K == N)     C(K) = C(K) + T/BET
            IF ( K == N - 1) B(K) = B(K) + T/DEL
        ENDDO
    !
    !   BACK SUBSTITUTION
    !    
        F(N-1) = F(N-1) - D(N-1)*F(N)
        C(N-1) = C(N-1) - D(N-1)*C(N)
        B(N-1) = B(N-1) - D(N-1)*B(N)
        DO K = N-2, 1, -1
            F(K) = F(K) - D(K)*F(K+1) - E(K)*F(K+2)
            C(K) = C(K) - D(K)*C(K+1) - E(K)*C(K+2)
            B(K) = B(K) - D(K)*B(K+1) - E(K)*B(K+2)            
        ENDDO
    !
    !   FINAL SOLUTION
    !
        A11 = ONE + ALP*(A(1)*C(N-1) + B1*C(N)) + BET*(D(N)*C(1) + E(N)*C(2))
        A12 =       ALP*(A(1)*B(N-1) + B1*B(N)) + BET*(D(N)*B(1) + E(N)*B(2))
        A21 =       GAM*A(2)*C(N) + DEL*E(N)*C(1)
        A22 = ONE + DEL*E(N-1)*B(1) + GAM*A(2)*B(N)
        F1  = ALP*(A(1)*F(N-1) + B1*F(N)) + BET*(D(N)*F(1) + E(N)*F(2))
        F2  = GAM*A(2)*F(N) + DEL*E(N-1)*F(1)
    !
        T = A11*A22 - A21*A21
        IF (ABS(T) <= PIV_TOL) THEN
            IER = ERR_MATINV
            RETURN
        ENDIF
        U = (F1*A22 - F2*A12)/T
        V = (F2*A11 - F1*A21)/T  
    !          
        DO K = 1, N
            F(K) = F(K) - C(K)*U - B(K)*V
        ENDDO    
    !        
    END SUBROUTINE PENTA1             
!
!   ***************************************************************************!
!
    SUBROUTINE PENTA2( M, N, A, B, C, D, E, F, IER, PAR)
    !
    ! PURPOSE---
    !   SOLUTION OF PENTA-DIAGONAL SYSTEM BY THOMAS ALGORITHM
    !
    ! ARGUMENTS---
        INTEGER, INTENT(IN) :: M, N
        REAL(8), INTENT(INOUT) :: A(M,M,*), B(M,M,*), C(M,M,*), D(M,M,*),&
        &   E(M,M,*) ! Destroyed
        REAL(8), INTENT(INOUT) :: F(M,*)  ! RHS / Solution 
        INTEGER, INTENT(OUT) :: IER       ! 0=OK, -1=Data, -2=Fact, -3=Back
    ! 
    ! OPTIONAL ARGUMENTS--- 
        REAL(8), OPTIONAL, INTENT(IN) :: PAR(4) ! 1=Alpha, 2=Beta, 3=Gamma, 4=Delta
    !
    ! FORTRAN FUNCTIONS CALLED---
        INTRINSIC   MATMUL
    !       
    ! SUBROUTINES CALLED---
    !    INTERFACE 
    !        SUBROUTINE MATINV( N, A, IER)
    !            INTEGER, INTENT(IN) :: N
    !            REAL(8), INTENT(INOUT) :: A(:,:)
    !            INTEGER, INTENT(OUT) :: IER
    !        END SUBROUTINE
    !    END INTERFACE        
    !         
    ! LOCAL SCALARS---
        INTEGER :: K
        REAL(8) :: ALP, BET, GAM, DEL
    !
    ! LOCAL ARRAYS---
        REAL(8) :: AA(2*M,2*M), FF(2*M), B1(M,M), T(M,M), U(M), V(M), &
        &   UM(M,M) ! Automatic arrays       
    !
    ! START-OF-CODE
    !        
    !   INITIALIZATION
    !
        IER = 0
        IF (N <= 4) THEN
            IER = -1
            RETURN
        ENDIF 
        PIV_TOL = UNDERFLOW           
        PIV_MIN = OVERFLOW                      
        B1 = B(:,:,1)
    !
        IF (PRESENT(PAR)) THEN
            ALP = PAR(1)
            BET = PAR(2)
            GAM = PAR(3)
            DEL = PAR(4)
        ELSE
            ALP = ONE
            BET = -ONE
            GAM = ONE
            DEL = -ONE
        ENDIF 
    !
        C(:,:,1)   = C(:,:,1)   - BET*D(:,:,N)/ALP
        D(:,:,1)   = D(:,:,1)   - BET*E(:,:,N)/ALP
        B(:,:,2)   = B(:,:,2)   - DEL*E(:,:,N-1)/GAM
        D(:,:,N-1) = D(:,:,N-1) - GAM*A(:,:,2)/DEL
        B(:,:,N)   = B(:,:,N)   - ALP*A(:,:,1)/BET
        C(:,:,N)   = C(:,:,N)   - ALP*B(:,:,1)/BET
    !
    !   FACTORIZATION
    !
        CALL MATINV( M, C(:,:,1), IER)
        IF (IER /= 0) RETURN
        D(:,:,1) = MATMUL( C(:,:,1), D(:,:,1))
        E(:,:,1) = MATMUL( C(:,:,1), E(:,:,1))
        T(:,:) = C(:,:,2) - MATMUL(B(:,:,2),D(:,:,1))
        CALL MATINV( M, T, IER)
        IF (IER /= 0) RETURN
        C(:,:,2) = T(:,:)    
        D(:,:,2) = MATMUL(C(:,:,2),(D(:,:,2) - MATMUL(B(:,:,2),E(:,:,1))))
        E(:,:,2) = MATMUL(C(:,:,2),E(:,:,2))
        DO K = 3, N
            B(:,:,K) = B(:,:,K) - MATMUL(A(:,:,K),D(:,:,K-2))
            T(:,:)   = C(:,:,K) - MATMUL(B(:,:,K),D(:,:,K-1)) &
            &   - MATMUL(A(:,:,K),E(:,:,K-2))
            CALL MATINV( M, T, IER)
            IF (IER /= 0) RETURN
            C(:,:,K) = T(:,:)
            IF (K == N) EXIT
            D(:,:,K) = MATMUL(C(:,:,K),(D(:,:,K) - MATMUL(B(:,:,K),E(:,:,K-1))))
            IF (K == N - 1) CYCLE
            E(:,:,K) = MATMUL(C(:,:,K),E(:,:,K))                
        ENDDO 
    !            
    !   INTERMEDIATE SOLUTION
    !
        F(:,1)   = MATMUL(C(:,:,1),F(:,1))
        C(:,:,1) = C(:,:,1)/ALP
        B(:,:,1) = ZERO
        T(:,:)   = C(:,:,2)
        F(:,2)   = MATMUL(T,(F(:,2) - MATMUL(B(:,:,2),F(:,1))))
        C(:,:,2) = -MATMUL(T(:,:),MATMUL(B(:,:,2),C(:,:,1)))
        B(:,:,2) =  T(:,:)/GAM
        DO K = 3, N
            T = C(:,:,K)
            F(:,K) =  MATMUL(T(:,:),(F(:,K) - MATMUL(B(:,:,K),F(:,K-1)) &
            &   - MATMUL(A(:,:,K),F(:,K-2))))
            C(:,:,K) = -MATMUL(T(:,:),(MATMUL(B(:,:,K),C(:,:,K-1)) &
            &   + MATMUL(A(:,:,K),C(:,:,K-2))))
            B(:,:,K) = -MATMUL(T(:,:),(MATMUL(B(:,:,K),B(:,:,K-1)) &
            &   + MATMUL(A(:,:,K),B(:,:,K-2))))
            IF ( K == N)     C(:,:,K) = C(:,:,K) + T(:,:)/BET
            IF ( K == N - 1) B(:,:,K) = B(:,:,K) + T(:,:)/DEL
        ENDDO
    !
    !   BACK SUBSTITUTION
    !    
        F(:,N-1)   = F(:,N-1) - MATMUL(D(:,:,N-1),F(:,N))
        C(:,:,N-1) = C(:,:,N-1) - MATMUL(D(:,:,N-1),C(:,:,N))
        B(:,:,N-1) = B(:,:,N-1) - MATMUL(D(:,:,N-1),B(:,:,N))
        DO K = N-2, 1, -1
            F(:,K) = F(:,K) - MATMUL(D(:,:,K),F(:,K+1)) &
            &   - MATMUL(E(:,:,K),F(:,K+2))
            C(:,:,K) = C(:,:,K) - MATMUL(D(:,:,K),C(:,:,K+1)) &
            &   - MATMUL(E(:,:,K),C(:,:,K+2))
            B(:,:,K) = B(:,:,K) - MATMUL(D(:,:,K),B(:,:,K+1)) &
            &   - MATMUL(E(:,:,K),B(:,:,K+2))            
        ENDDO                    
    !
    !   FINAL SOLUTION
    !
        UM(:,:) = ZERO
        DO K = 1, M
            UM(K,K) = ONE
        ENDDO    
        AA(1:M,1:M)         = UM + ALP*(MATMUL(A(:,:,1),C(:,:,N-1)) &
        &   + MATMUL(B1,C(:,:,N))) + BET*(MATMUL(D(:,:,N),C(:,:,1)) &
        &   + MATMUL(E(:,:,N),C(:,:,2)))
        AA(1:M,M+1:2*M)     = ALP*(MATMUL(A(:,:,1),B(:,:,N-1)) &
        &   + MATMUL(B1,B(:,:,N))) + BET*(MATMUL(D(:,:,N),B(:,:,1)) &
        &   + MATMUL(E(:,:,N),B(:,:,2)))
        AA(M+1:2*M,1:M)     = GAM*MATMUL(A(:,:,2),C(:,:,N)) &
        &   + DEL*MATMUL(E(:,:,N),C(:,:,1))
        AA(M+1:2*M,M+1:2*M) = UM + DEL*MATMUL(E(:,:,N-1),B(:,:,1)) &
        &   + GAM*MATMUL(A(:,:,2),B(:,:,N))
        FF(1:M)      = ALP*(MATMUL(A(:,:,1),F(:,N-1)) + MATMUL(B1,F(:,N))) &
        &   + BET*(MATMUL(D(:,:,N),F(:,1)) + MATMUL(E(:,:,N),F(:,2)))
        FF(M+1:2*M)  = GAM*MATMUL(A(:,:,2),F(:,N)) + &
        &   DEL*MATMUL(E(:,:,N-1),F(:,1))
    !
        CALL MATINV( 2*M, AA, IER)
        IF (IER /= 0) RETURN
        FF(:) = MATMUL( AA(:,:),FF(:))
        U(:) = FF(1:M)
        V(:) = FF(M+1:2*M)  
    !          
        DO K = 1, N
            F(:,K) = F(:,K) - MATMUL(C(:,:,K),U(:)) - MATMUL(B(:,:,K),V(:))
        ENDDO    
    !        
    END SUBROUTINE PENTA2        
!
!
!   ==========================================================================!
!
    SUBROUTINE MATINV( N, A, IER)
    !
    ! PURPOSE---
    !   MATRIX INVERSION
    !
    ! ARGUMENTS---
        INTEGER, INTENT(IN)     :: N       ! MATRIX SIZE
        REAL(8), INTENT(INOUT)  :: A(N,N)  ! MATRIX
        INTEGER, INTENT(OUT)    :: IER     ! ERROR INDICATOR (0=OK)
    !
    !  FORTRAN FUNCTIONS CALLED---
        INTRINSIC   ABS
    !
    !  LOCAL VARIABLES---
        INTEGER  :: I, J, K
        REAL(8) :: DET, T1
    !
    !  LOCAL ARRAYS---
        REAL(8) :: B(3,3)
    !
    !  EXECUTABLE STATEMENTS---
    !
        IER = 0
    !
        SELECT CASE (N)
    !
        CASE (1)    ! 1X1 MATRIX   ----------------------------------------------!
    !
            PIV_MIN = MIN(PIV_MIN, ABS(A(1,1)))
            IF (ABS(A(1,1)) < PIV_TOL) THEN
                IER = ERR_MATINV
                RETURN
            ENDIF
            A(1,1) = ONE/A(1,1)
            RETURN             
    !
        CASE (2)  ! 2X2 MATRIX  -------------------------------------------------!
    !
            DET =  A(1,1)*A(2,2) - A(1,2)*A(2,1)
            PIV_MIN = MIN(PIV_MIN, ABS(DET))
            IF (ABS(DET) < PIV_TOL) THEN
                IER = ERR_MATINV
                RETURN
            ENDIF
            T1     =  A(1,1)
            A(1,1) =  A(2,2)/DET
            A(2,1) = -A(2,1)/DET
            A(1,2) = -A(1,2)/DET
            A(2,2) =  T1/DET
            RETURN
    !      
        CASE (3)    ! 3X3 MATRIX ------------------------------------------------!
    !
            B(:,:) = A(:,:)
    !
    !        CALCULATE DETERMINANT
    !
            DET =   B(1,1)*(B(2,2)*B(3,3) - B(3,2)*B(2,3)) &
            &     - B(1,2)*(B(2,1)*B(3,3) - B(3,1)*B(2,3)) &
            &     + B(1,3)*(B(2,1)*B(3,2) - B(3,1)*B(2,2))
    !
    !        CHECK DETERMINANT
    !
            PIV_MIN = MIN(PIV_MIN, ABS(DET))
            IF (ABS(DET) < PIV_TOL) THEN
                IER = ERR_MATINV
                RETURN
            ENDIF
            DET = ONE/DET
    !
    !        INVERT
    !
            A(1,1) = ( B(2,2)*B(3,3) - B(3,2)*B(2,3))*DET
            A(2,1) = (-B(2,1)*B(3,3) + B(3,1)*B(2,3))*DET
            A(3,1) = ( B(2,1)*B(3,2) - B(3,1)*B(2,2))*DET
            A(1,2) = (-B(1,2)*B(3,3) + B(3,2)*B(1,3))*DET
            A(2,2) = ( B(1,1)*B(3,3) - B(3,1)*B(1,3))*DET
            A(3,2) = (-B(1,1)*B(3,2) + B(3,1)*B(1,2))*DET
            A(1,3) = ( B(1,2)*B(2,3) - B(2,2)*B(1,3))*DET
            A(2,3) = (-B(1,1)*B(2,3) + B(2,1)*B(1,3))*DET
            A(3,3) = ( B(1,1)*B(2,2) - B(2,1)*B(1,2))*DET
            RETURN
    !
        CASE DEFAULT   ! N X N  MATRIX   ----------------------------------------!
    !
    !        FROM SMITH AND GRIFFITHS'S FEM
    !
    !        THE INVERSE OF A MATRIX USING GAUSS-JORDAN TRANSFORMATION
    !        FOR 'SMALL' MATRIX ONLY !!!
    !
            DO K = 1, N
                T1=A(K,K)
                PIV_MIN = MIN(PIV_MIN, ABS(T1))
                IF (ABS(T1) < PIV_TOL) THEN
                    IER = ERR_MATINV
                    RETURN
                ENDIF
                A(K,K) = ONE
                A(K,:) = A(K,:)/T1
                DO I = 1,N
                    IF (I == K) CYCLE
                    T1 = A(I,K)
                    A(I,K) = ZERO
                    A(I,:) = A(I,:) - A(K,:)*T1
                ENDDO
            ENDDO
            RETURN
    !
        END SELECT
    !
    END SUBROUTINE MATINV
!          
END MODULE M_PENTA