module msi2d9
  implicit none
  !
  ! Goals:
  !    Given the matrix of coefficients B of a linear system Bx = rhs 
  !    1) Find the lower L and upper U triangular matrix of the factorization
  !       B = LU - P. (P matrix is not important) 
  !       B must have only nine non-null diagonals.
  !    2) Solve the linear system Bx = rhs iteratively using the LU factorization (MSI method)
  !
  !    Latest version: 28 Apr 2011
  !
  ! Subroutines given by this module
  !
  !    1) fb2d9 
  !       (ASSUMES THE COEFFICIENTS OF B MATRIX CORRESPONDING TO
  !        BOUNDARY NEIGHBOORS OF FICTITIOUS VOLUMES ARE ZERO)
  !
  !    2) lu2d9 
  !       (SETS ALPHA=0 AT FICTITIOUS VOLUMES INDEPENDENTLY OF ALPHA ELSEWHERE)
  !       (the parameter alpha must be set inside this subroutine)
contains
  !
  ! fb2d9 ASSUMES THE COEFFICIENTS OF B MATRIX CORRESPONDING TO
  ! BOUNDARY NEIGHBOORS OF FICTITIOUS VOLUMES ARE ZERO
  !
  ! fb2d9 solves the linear system b.var = rhs iteratively
  !
  ! b     - matrix  of coefficients of the original system 
  ! dl    - lower triangular matrix 
  ! du    - upper triangular matrix
  ! nj    - number of volumes in the j direction
  ! ni    - number of volumes in the i direction
  ! nij   - nij=ni*nj
  ! var   - vector of unkowns
  ! gamma - L2( r_n ) < L2( r_0 ) * gamma
  ! L2    - L2 norm
  ! r_n   - residual vector: r_n = b var_n - rhs
  ! nitm  - maximum number of iteractions
  ! rhs   - vector with the source term of the linear system
  !
  subroutine fb2d9(b,dl,du,nj,ni,nij,var,nitm,rhs,r,w,z, inv_dl1)
    implicit none
    integer, intent(in)    ::   ni, nj, nij, nitm   ! 
    real(8), intent(in)    ::   b(nij,9) ! matrix of coefficients
    real(8), intent(in)    ::  dl(nij,5) ! lower triangular matrix
    real(8), intent(in)    ::  du(nij,4) ! upper triangular matrix
    real(8), intent(in)    :: rhs(nij)   ! source vector of the linear system
    real(8), intent(inout), dimension(nij) :: r,w,z,inv_dl1
    real(8), intent(inout) :: var(nij)   ! vector of unknowns
    !
    integer :: nit
    integer :: isw, is, ise, iw, ip, ie, inw, in, ine
    real(8) :: gammarp
    real*8 :: temp1, temp2, temp3, temp4
    real*8 :: res
    integer :: i, j, aux_i
       
    
    call residuod9(ni,nj,nij,b,rhs,var,r)

    !--------------------------------------------------------------------------------
    !gammarp = gamma * dsqrt(dot_product(r,r))
    !--------------------------------------------------------------------------------
    do nit=1, nitm

       !
       ! Solving linear system Lw = -r
       !------------------------------
       ip = 1
       w(ip) = -r(ip) * inv_dl1(ip) !/ dl(ip,1)
       !------------------------------
       do ip = 2, ni-1
          w(ip) = -( r(ip) + dl(ip,2)*w(ip-1) ) * inv_dl1(ip) !/ dl(ip,1)
       end do
       !--------------------------------------------------
       ip = ni
       w(ip) = -( r(ip) + dl(ip,2)*w(ip-1) + dl(ip,3)*w(ip-ni+1) ) * inv_dl1(ip) !/ dl(ip,1)
       !---------------------------------------------------------------------
       ip = ni+1
       w(ip) = -( r(ip) + dl(ip,2)*w(ip-1) + dl(ip,3)*w(ip-ni+1) + dl(ip,4)*w(ip-ni) ) * inv_dl1(ip) !/ dl(ip,1)
       !-----------------------------------------------------------------------------------------
       do ip = ni+2, nij
          temp1 = -(dl(ip,3)*w(ip-ni+1) + dl(ip,5)*w(ip-ni-1) + dl(ip,4)*w(ip-ni))
          temp2 = -( r(ip) + dl(ip,2)*w(ip-1) )
          w(ip) = (temp1 + temp2) * inv_dl1(ip)
       end do
       !-------------------------------------------------------------------------------------------------------------------
       !
       ! Solving linear system Uz = w
       !-----------------------------
       z = w
       !ip = nij
       !z(ip) = z(ip)
       !------------
       do ip = nij-1, nij-ni+2, -1
          z(ip) = z(ip) - du(ip,1)*z(ip+1)
       end do
       !----------------------------------
       ip = nij-ni+1
       z(ip) = z(ip) - du(ip,1)*z(ip+1) - &
                       du(ip,2)*z(ip+ni-1)
       !-----------------------------------------------------
       ip = nij-ni
       in = ip + ni
       z(ip) = z(ip) - du(ip,1)*z(ip+1) - &
                       du(ip,2)*z(in-1) - du(ip,3)*z(in)
       !-------------------------------------------------------------------------
       do ip = nij-ni-1, 1, -1
          temp1 = - du(ip,2)*z(ip+ni-1) - du(ip,4)*z(ip+ni+1) - du(ip,3)*z(ip+ni) 
          z(ip) = temp1 + z(ip) - du(ip,1)*z(ip+1)  
       end do
       !--------------------------------------------------------------------------------------------------
       !
       ! Updating solution
       !------------------
       var = z + var        
 
       call residuod9(ni,nj,nij,b,rhs,var,r)
        
       !if ( sqrt(dot_product(r,r)) < gammarp ) exit
       !
    end do
       
  end subroutine fb2d9
  ! 
  ! ld2d9 SETS ALPHA=0 AT FICTITIOUS VOLUMES INDEPENDENTLY OF ALPHA OTHERWISE
  !
  ! lu2d9 calculates the entries of the L and U matrix in the decomposition LU = B + P
  ! where B is the matrix of the coefficients of the linear system to be solved and P is a
  ! matrix whose entries are not necessary.
  !
  ! b     - matrix  of coefficients of the original system (has 9 diagonals)
  ! dl    - lower triangular matrix 
  ! du    - upper triangular matrix
  ! nj    - number of volumes in the j direction
  ! ni    - number of volumes in the i direction
  ! nij   - nij=ni*nj
  !
  subroutine lu2d9(b,dl,du,nij,ni,nj,inv_dl1)
    implicit none
    integer, intent(in)  :: ni, nj, nij
    real(8), intent(in)  ::  b(nij,9)  ! matrix of coefficients
    real(8), intent(out) :: dl(nij,5)  ! lower diagonal matrix
    real(8), intent(out)  :: inv_dl1(nij)  ! matrix of coefficients
    real(8), intent(out) :: du(nij,4)  ! upper diagonal matrix

    integer :: isw, is, ise, iw, ip
    real(8) :: phi1, phi2, phi3, phi4, fat

    integer :: i, j, aux_i

    !du = 0.d0
    !dl = 0.d0

    !------------------------------------------------------------------------------------------------------------------------
    !
    ip = 1
    !
    dl(ip,1) = b(ip,5)
    fat = 1.d0/ dl(ip,1)
    du(ip,1) = b(ip,6) * fat !/ dl(ip,1)
    du(ip,2) = b(ip,7) * fat !/ dl(ip,1)
    du(ip,3) = b(ip,8) * fat !/ dl(ip,1)
    du(ip,4) = b(ip,9) * fat !/ dl(ip,1)
    inv_dl1(ip) = fat
    !------------------------------------------------------------------------------------------------------------------------
    !
    do ip = 2, ni-1
       iw  = ip - 1
       dl(ip,2) = b(ip,4)
       dl(ip,1) = b(ip,5) - dl(ip,2)*du(iw,1)
       fat = 1.d0/ dl(ip,1)
       du(ip,1) = b(ip,6) * fat !/ dl(ip,1)
       du(ip,2) = ( b(ip,7) - dl(ip,2)*du(iw,3) ) * fat !/ dl(ip,1)
       du(ip,3) = ( b(ip,8) - dl(ip,2)*du(iw,4) ) * fat !/ dl(ip,1)
       du(ip,4) = b(ip,9) * fat !/ dl(ip,1)
       inv_dl1(ip) = fat
    end do
    !------------------------------------------------------------------------------------------------------------------------
    !
    ip = ni
    !
    iw  = ip - 1
    is  = ip - ni
    ise = is + 1
    dl(ip,3) = b(ip,3)
    dl(ip,2) = b(ip,4)
    dl(ip,1) = b(ip,5)-dl(ip,2)*du(iw,1)-dl(ip,3)*du(ise,2)
    fat = 1.d0/ dl(ip,1)
    du(ip,1) = ( b(ip,6) - dl(ip,3)*du(ise,3) )* fat !/ dl(ip,1)
    du(ip,2) = ( b(ip,7) - dl(ip,2)*du(iw,3) ) * fat !/ dl(ip,1)
    du(ip,3) = ( b(ip,8) - dl(ip,2)*du(iw,4) ) * fat !/ dl(ip,1)
    du(ip,4) = b(ip,9) * fat
    inv_dl1(ip) = fat
    !------------------------------------------------------------------------------------------------------------------------
    !
    do j=2, nj - 1
        aux_i = ni*(j-1)
        
        ip = aux_i + 1
        iw  = ip - 1
        is  = ip - ni
        ise = is + 1
        dl(ip,4) = b(ip,2)
        dl(ip,3) = b(ip,3) - dl(ip,4) * du(is,1)
        dl(ip,2) = b(ip,4) - dl(ip,4)*du(is,2) 
        dl(ip,1) = b(ip,5)-dl(ip,2)*du(iw,1)-dl(ip,3)*du(ise,2)-dl(ip,4)*du(is,3)
        fat = 1.d0/ dl(ip,1)
        du(ip,1) = ( b(ip,6) - dl(ip,3)*du(ise,3) - dl(ip,4)*du(is,4) ) * fat !/ dl(ip,1)
        du(ip,2) = ( b(ip,7) - dl(ip,2)*du(iw,3) ) * fat !/ dl(ip,1)
        du(ip,3) = ( b(ip,8) - dl(ip,2)*du(iw,4) ) * fat !/ dl(ip,1)
        du(ip,4) = b(ip,9) * fat !/ dl(ip,1)  
        inv_dl1(ip) = fat     
        
        do i = 2, ni - 1
            ip = aux_i + i
            iw  = ip - 1
            is  = ip - ni
            isw = is - 1
            ise = is + 1
            dl(ip,5) = b(ip,1)
            dl(ip,4) = ( b(ip,2) -dl(ip,5)*du(isw,1) -0.5d0*b(ip,3)*du(ise,1) ) / ( 1.d0 - 0.5d0*du(is,1)*du(ise,1) )
            dl(ip,3) = b(ip,3) - dl(ip,4) * du(is,1)
            dl(ip,2) = (b(ip,4) - dl(ip,4)*du(is,2) -dl(ip,5)*du(isw,3) -2.d0*0.5d0*dl(ip,5)*du(isw,2) ) / ( 1.d0 + du(iw,2) )
            phi1 = dl(ip,3) * du(ise,1)
            phi2 = dl(ip,5) * du(isw,2)
            phi3 = dl(ip,3) * du(ise,4)
            phi4 = dl(ip,2) * du(iw ,2)
            dl(ip,1) = b(ip,5)-dl(ip,2)*du(iw,1)-dl(ip,3)*du(ise,2)-dl(ip,4)*du(is,3)-dl(ip,5)*du(isw,4) &
                 +     0.5d0 * ( 2.d0*phi1 + phi2 + phi3 + 2.d0*phi4 )
            fat = 1.d0/ dl(ip,1)
            du(ip,1) = ( b(ip,6) - dl(ip,3)*du(ise,3) - dl(ip,4)*du(is,4) - 1.d0*(phi1+phi3) ) * fat !/ dl(ip,1)
            du(ip,2) = ( b(ip,7) - dl(ip,2)*du(iw,3) ) * fat !/ dl(ip,1)
            du(ip,3) = ( b(ip,8) - dl(ip,2)*du(iw,4) - 0.5d0*phi4 ) * fat !/ dl(ip,1)
            du(ip,4) = b(ip,9) * fat !/ dl(ip,1)
            inv_dl1(ip) = fat
                        
        enddo
        ip = aux_i + ni
        iw  = ip - 1
        is  = ip - ni
        ise = is + 1
        dl(ip,4) = b(ip,2)
        dl(ip,3) = b(ip,3) - dl(ip,4) * du(is,1)
        dl(ip,2) = b(ip,4) - dl(ip,4)*du(is,2) 
        dl(ip,1) = b(ip,5)-dl(ip,2)*du(iw,1)-dl(ip,3)*du(ise,2)-dl(ip,4)*du(is,3)
        fat = 1.d0/ dl(ip,1)
        du(ip,1) = ( b(ip,6) - dl(ip,3)*du(ise,3) - dl(ip,4)*du(is,4) ) * fat !/ dl(ip,1)
        du(ip,2) = ( b(ip,7) - dl(ip,2)*du(iw,3) ) * fat !/ dl(ip,1)
        du(ip,3) = ( b(ip,8) - dl(ip,2)*du(iw,4) ) * fat !/ dl(ip,1)
        du(ip,4) = b(ip,9) * fat !/ dl(ip,1)
        inv_dl1(ip) = fat
    enddo
    !------------------------------------------------------------------------------------------------------------------------
    !
    ip = nij-ni+1
    !
    iw  = ip - 1
    is  = ip - ni
    isw = is - 1
    ise = is + 1
    dl(ip,5) = b(ip,1)
    dl(ip,4) = b(ip,2) -dl(ip,5)*du(isw,1)
    dl(ip,3) = b(ip,3) - dl(ip,4) * du(is,1)
    dl(ip,2) = b(ip,4) - dl(ip,4)*du(is,2) -dl(ip,5)*du(isw,3)
    dl(ip,1) = b(ip,5)-dl(ip,2)*du(iw,1)-dl(ip,3)*du(ise,2)-dl(ip,4)*du(is,3)-dl(ip,5)*du(isw,4) 
    fat = 1.d0/ dl(ip,1)
    du(ip,1) = ( b(ip,6) - dl(ip,3)*du(ise,3) - dl(ip,4)*du(is,4) ) * fat !/ dl(ip,1)
    du(ip,2) = ( b(ip,7) - dl(ip,2)*du(iw,3) ) * fat !/ dl(ip,1)
    inv_dl1(ip) = fat
    !------------------------------------------------------------------------------------------------------------------------
    !
    do ip = nij-ni+2, nij-1
       !
       iw  = ip - 1
       is  = ip - ni
       isw = is - 1
       ise = is + 1
       dl(ip,5) = b(ip,1)
       dl(ip,4) = b(ip,2) -dl(ip,5)*du(isw,1)
       dl(ip,3) = b(ip,3) - dl(ip,4) * du(is,1)
       dl(ip,2) = b(ip,4) - dl(ip,4)*du(is,2) -dl(ip,5)*du(isw,3)
       dl(ip,1) = b(ip,5)-dl(ip,2)*du(iw,1)-dl(ip,3)*du(ise,2)-dl(ip,4)*du(is,3)-dl(ip,5)*du(isw,4) 
       du(ip,1) = ( b(ip,6) - dl(ip,3)*du(ise,3) - dl(ip,4)*du(is,4) ) / dl(ip,1)
       inv_dl1(ip) = 1.d0/dl(ip,1)
    end do
    !------------------------------------------------------------------------------------------------------------------------
    !
    ip = nij
    !
    iw  = ip - 1
    is  = ip - ni
    isw = is - 1
    ise = is + 1
    dl(ip,5) = b(ip,1)
    dl(ip,4) = b(ip,2) -dl(ip,5)*du(isw,1)
    dl(ip,3) = b(ip,3) - dl(ip,4) * du(is,1)
    dl(ip,2) = b(ip,4) - dl(ip,4)*du(is,2) -dl(ip,5)*du(isw,3)
    dl(ip,1) = b(ip,5)-dl(ip,2)*du(iw,1)-dl(ip,3)*du(ise,2)-dl(ip,4)*du(is,3)-dl(ip,5)*du(isw,4)
    inv_dl1(ip) = 1.d0/dl(ip,1)
    !------------------------------------------------------------------------------------------------------------------------
    !
  end subroutine lu2d9
  
!------------------------------------------------------------------------------------------------------------------------
subroutine residuod9(ni,nj,nij,b,rhs,var,r)

    integer, intent(in)    ::   ni, nj, nij 
    real(8), intent(in)    ::   b(nij,9) ! matrix of coefficients
    real(8), intent(in)    :: rhs(nij)   ! source vector of the linear system
    real(8), intent(in)    :: var(nij)   ! vector of unknowns
    real(8), intent(out)   :: r(nij)   ! vector of unknowns
    !
    integer :: isw, is, ise, iw, ip, ie, inw, in, ine
    real*8 :: temp1, temp2, temp3, temp4,temp5,temp6,temp7,temp8,temp9
    integer :: i, j, aux_i

    !
    ! Calculation of the residual vector r = A.var-rhs
    !--------------------------------------------------------------------------------
    ip = 1
    ie  = ip + 1
    in  = ip + ni
    ine = in + 1
    r(ip) =  -rhs(ip)+ b(ip,5) * var(ip ) + b(ip,6) * var(ie ) &
           + b(ip,8) * var(in ) + b(ip,9) * var(ine) 
    !--------------------------------------------------------------------------------
    do ip = 2, ni-1
       iw = ip - 1
       ie  = ip + 1
       in  = ip + ni
       inw = in - 1
       ine = in + 1
       r(ip) = -rhs(ip)+ b(ip,4) * var(iw ) + b(ip,5) * var(ip ) + b(ip,6) * var(ie ) &
              + b(ip,7) * var(inw) + b(ip,8) * var(in ) + b(ip,9) * var(ine) 
    end do
    !--------------------------------------------------------------------------------
    ip = ni
    iw  = ip - 1
    in  = ip + ni
    inw = in - 1
    r(ip) =  -rhs(ip)+ b(ip,4) * var(iw ) + b(ip,5) * var(ip ) &
           + b(ip,7) * var(inw) + b(ip,8) * var(in )                      
    !--------------------------------------------------------------------------------
    do j = 2, nj-1
        aux_i = ni*(j-1)
        ip = aux_i + 1
        iw  = aux_i 
        ie  = aux_i + 2
        is  = aux_i + 1 - ni
        in  = aux_i + 1 + ni
        ise = is + 1
        ine = in + 1
        r(ip) = -rhs(ip)+ b(ip,2) * var(is ) + b(ip,3) * var(ise) &
              + b(ip,5) * var(ip ) + b(ip,6) * var(ie ) &
              + b(ip,8) * var(in ) + b(ip,9) * var(ine)
    
        do i = 2, ni-1, 2
            ip = aux_i + i
            iw  = aux_i + i - 1
            ie  = aux_i + i + 1
            is  = aux_i + i - ni
            in  = aux_i + i + ni
!            isw = is - 1
!            ise = is + 1
!            inw = in - 1
!            ine = in + 1
            
            temp1 = b(ip,1) * var(is - 1) + b(ip,2) * var(is    ) + b(ip,3) * var(is + 1)
            temp4 = b(ie,1) * var(is    ) + b(ie,2) * var(is + 1) + b(ie,3) * var(is + 2)
            temp2 = b(ip,4) * var(iw    ) + b(ip,5) * var(ip    ) + b(ip,6) * var(ie    )
            temp5 = b(ie,4) * var(ip    ) + b(ie,5) * var(ie    ) + b(ie,6) * var(ie + 1)
            temp3 = b(ip,7) * var(in - 1) + b(ip,8) * var(in    ) + b(ip,9) * var(in + 1)
            temp6 = b(ie,7) * var(in    ) + b(ie,8) * var(in + 1) + b(ie,9) * var(in + 2)
            
            r(ip) = -rhs(ip)+ temp1 + temp2 + temp3
            r(ie) = -rhs(ie)+ temp4 + temp5 + temp6
            
!            temp1 = b(ip,1) * var(isw) + b(ip,2) * var(is ) + b(ip,3) * var(ise)
!            temp3 = b(ie,1) * var(is) + b(ie,2) * var(ise) + b(ie,3) * var(ise+1)
!            
!            temp2 = b(ip,7) * var(inw) + b(ip,8) * var(in ) + b(ip,9) * var(ine)
!            temp4 = b(ie,7) * var(in) + b(ie,8) * var(ine) + b(ie,9) * var(ine+1)
!            
!            r(ip) =  -rhs(ip)+ temp1 &
!                  +  b(ip,4) * var(iw ) + b(ip,5) * var(ip ) + b(ip,6) * var(ie ) &
!                  +  temp2
!            r(ie) =  -rhs(ie)+ temp3 &
!                  +  b(ie,4) * var(ip) + b(ie,5) * var(ie ) + b(ie,6) * var(ie+1 ) &
!                  +  temp4
                 
        enddo
        ip = aux_i + ni
        iw  = aux_i + ni - 1
        ie  = aux_i + ni + 1
        is  = aux_i
        in  = aux_i + ni + ni
        isw = is - 1
        inw = in - 1
        r(ip) =  -rhs(ip)+ b(ip,1) * var(isw) + b(ip,2) * var(is ) &
               + b(ip,4) * var(iw ) + b(ip,5) * var(ip ) &
               + b(ip,7) * var(inw) + b(ip,8) * var(in ) 

    enddo
    !--------------------------------------------------------------------------------
    ip = nij-ni+1
    ie  = ip + 1
    is  = ip - ni
    ise = is + 1
    r(ip) = -rhs(ip)+ b(ip,2) * var(is ) + b(ip,3) * var(ise) &
          + b(ip,5) * var(ip ) + b(ip,6) * var(ie ) 
    !--------------------------------------------------------------------------------
    do ip = nij-ni+2, nij-1
       iw  = ip - 1
       ie  = ip + 1
       is  = ip - ni
       isw = is - 1
       ise = is + 1
       r(ip) = -rhs(ip)+ b(ip,1) * var(isw) + b(ip,2) * var(is ) + b(ip,3) * var(ise) &
             + b(ip,4) * var(iw ) + b(ip,5) * var(ip ) + b(ip,6) * var(ie ) 
    end do
    !--------------------------------------------------------------------------------
    ip = nij
    iw  = ip - 1
    is  = ip - ni
    isw = is - 1
    r(ip) = -rhs(ip)+ b(ip,1) * var(isw) + b(ip,2) * var(is ) &
          + b(ip,4) * var(iw ) + b(ip,5) * var(ip )
          
end subroutine residuod9
!------------------------------------------------------------------------------------------------------------------------

end module msi2d9
