MODULE lu_dcmps
  USE precisions  ! define working precision (wp) e higher precision (hp) 
  IMPLICIT NONE

! Abaixo seguem os procedimentos associados a eliminacao de Gauss e LU.
! Estao no mesmo modulo pois sao pequenos e as interfaces sao automaticamente
! explicitas, evitando precaucoes adicionais com parametros opcionais e
! "assumed shape". Ha tres decomposicoes: LU por eliminacao de Gauss, LU
! pelo metodo de Doolittle e LU pelo metodo de Crout. A rotina
! lu_crout_solve é diferente pois nessa decomposicao a matriz U tem 1
! na diagonal.

  CONTAINS
    SUBROUTINE lu_gauss(A, vp)
      INTEGER, OPTIONAL :: vp(:)
      REAL(KIND=wp) :: A(:,:)
    ! Recebe uma matriz quadrada A e armazena nela propria a decomposicao
    ! LU usando o metodo de eliminacao de Gauss. Se o parametro OPCIONAL
    ! vp estiver presente, a matriz é balanceada (implicitamente) e
    ! usa-se condensacao pivotal. O vetor vp entao retorna a ordenacao
    ! final das linhas.

      INTEGER :: n, k, j, itmp, jj(1)  ! jj is for the "weird" MAXLOC
      REAL(KIND=wp) :: tmp
      REAL(KIND=wp), DIMENSION(SIZE(A,1)) :: scal, work

      n = SIZE(A,1)
      IF (PRESENT(vp)) THEN
        vp = (/(j, j=1,n)/)
        scal = 1.0_wp/MAXVAL(ABS(A),2)  ! Scaling factors for implicit
                                        ! equilibration
        DO k=1, n-1
          jj = MAXLOC(ABS(A(k:n,k))*scal(k:n)) + k - 1 ! MAXLOC does not care
          j = jj(1)                 ! for bounds and does not return a scalar
          IF (j .NE. k) THEN  ! swap lines k and j
            work = A(k,:); A(k,:) = A(j,:); A(j,:) = work
            tmp = scal(k); scal(k) = scal(j); scal(j) = tmp
            itmp = vp(k); vp(k) = vp(j); vp(j) = itmp
          END IF
          A((k+1):n,k) = A((k+1):n,k)/A(k,k)  ! multiplyers
          DO j = k+1, n                      
            A(j,(k+1):n) = A(j,(k+1):n) - A(j,k)*A(k,(k+1):n)
          END DO
        END DO
      ELSE  ! no scaling and no pivoting
        DO k=1, n-1
          A((k+1):n,k) = A((k+1):n,k)/A(k,k)  ! multiplyers
          DO j = k+1, n
            A(j,(k+1):n) = A(j,(k+1):n) - A(j,k)*A(k,(k+1):n)
          END DO
        END DO
      END IF
    END SUBROUTINE lu_gauss

    SUBROUTINE lu_doolittle(A,vp)
      INTEGER, OPTIONAL :: vp(:)
      REAL(KIND=wp) :: A(:,:)
    ! Decomposicao LU da matriz A usando o algoritmo de Doolittle. A
    ! decomposicao e retornada na propria matriz. Se vp estiver presente,
    ! balanceamento implicito e condensacao pivotal sao usados.

      INTEGER :: n, k, j, itmp, jj(1)  ! jj is for the "weird" MAXLOC
      REAL(KIND=wp) :: tmp
      REAL(KIND=wp), DIMENSION(SIZE(A,1)) :: scal, work
      REAL(KIND=hp) :: tmphp
      REAL(KIND=hp), DIMENSION(SIZE(A,1)) :: sd  ! stores higher precision
                                                 ! computations

      n = SIZE(A,1)
      IF (PRESENT(vp)) THEN
        vp = (/(j, j=1,n)/)
        scal = 1.0_wp/MAXVAL(ABS(A),2)  ! Scaling factors for implicit
                                        ! equilibration
        DO k = 1, n
          sd(k:n) = A(k:n,k) - & ! higher precision computation
                    MATMUL(A(k:n,1:(k-1)),A(1:(k-1),k)+0.0_hp)
          jj = MAXLOC(ABS(sd(k:n))*scal(k:n)) + k - 1
          j = jj(1)
          IF(j .NE. k) THEN  !swap lines
            work = A(k,:); A(k,:) = A(j,:); A(j,:) = work
            tmp = scal(k); scal(k) = scal(j); scal(j) = tmp
            itmp = vp(k); vp(k) = vp(j); vp(j) = itmp
            tmphp = sd(k); sd(k) = sd(j); sd(j) = tmphp
          END IF
          ! Compute row k of U
          A(k,k) = sd(k)
          A(k,(k+1):n) = A(k,(k+1):n) - & ! higher precision computation
                       MATMUL(A(k,1:(k-1))+0.0_hp,A(1:(k-1),(k+1):n))
          ! Compute column k of L
          A((k+1):n,k) = sd((k+1):n)/sd(k)
        END DO
      ELSE  ! no scaling and no pivoting
        DO k = 1, n
          sd(k:n) = A(k:n,k) - & ! higher precision below
                    MATMUL(A(k:n,1:(k-1)),A(1:(k-1),k)+0.0_hp)
          ! Compute row k of U
          A(k,k) = sd(k)
          A(k,(k+1):n) = A(k,(k+1):n) - & ! higher precision computation
                       MATMUL(A(k,1:(k-1))+0.0_hp,A(1:(k-1),(k+1):n))
          ! Compute column k of L
          A((k+1):n,k) = sd((k+1):n)/sd(k)
        END DO
      END IF
    END SUBROUTINE lu_doolittle

    SUBROUTINE lu_solve(A,x,vp)
      INTEGER, OPTIONAL :: vp(:)
      REAL(KIND=wp) :: A(:,:), x(:)
    ! A matriz A recebe a decomposicao LU e o vetor x o lado direito.
    ! Se o argumento OPCIONAL vp estiver presente, as linhas de x sao
    ! permutadas. Os sistemas triangulares inferior e superior sao
    ! resolvidos por substituticoes progressiva e regressiva,
    ! respectivamente. A solucao e retornada em x.

      INTEGER :: i, n

      n = size(x)
      IF (PRESENT(vp)) x = x(vp)  ! Permute the rows of the vector x
      DO i = 2, n  ! forward substitution
        x(i) = x(i) - DOT_PRODUCT(A(i,1:(i-1)),x(1:(i-1)))
      END DO
      x(n) = x(n)/A(n,n)
      DO i = n-1, 1, -1  ! backward substitution
        x(i) = (x(i) - DOT_PRODUCT(A(i,(i+1):n),x((i+1):n)))/A(i,i)
      END DO
    END SUBROUTINE lu_solve

   ! Decomposicao de Crout com ou sem condensacao pivotal e equilibrio
   ! implicito. Resolucao dos sistemas triangulares.

    SUBROUTINE lu_crout(A,vp)
      INTEGER, OPTIONAL :: vp(:)
      REAL(KIND=wp) :: A(:,:)
      ! Recebe a matriz A e devolve nela mesma a decomposicao de Crout.
      ! Se o argumento OPCIONAL vp estiver presente, usa-se condensacao
      ! pivotal e balanceamento implicito, devolvendo em vp a ordenacao
      ! final das linhas.

      INTEGER :: j, k, n, itmp, jj(1) ! jj is for the "weird" MAXLOC
      REAL(KIND=wp), DIMENSION(SIZE(A,1)) :: scal, work
      REAL(KIND=wp) :: tmp

      n = SIZE(A,1)
      IF (PRESENT(vp)) THEN ! use pivoting and implicit scaling
        vp = (/(j, j=1,n)/)
        scal = 1.0_wp/MAXVAL(ABS(A),2) ! Scaling factors
        DO k = 1, n
          A(k:n,k) = A(k:n,k) -& ! higher precision computation
              MATMUL(A(k:n,1:(k-1)),A(1:(k-1),k)+0.0_hp)
          jj = MAXLOC(ABS(A(k:n,k))*scal(k:n)) + k - 1
          j = jj(1)
          IF (j .NE. k) THEN  ! swap lines k and j
            work = A(k,:); A(k,:) = A(j,:); A(j,:) = work
            tmp = scal(k); scal(k) = scal(j); scal(j) = tmp
            itmp = vp(k); vp(k) = vp(j); vp(j) = itmp
          END IF
          A(k,(k+1):n) = (A(k,(k+1):n) -& ! higher precision computation
               MATMUL(A(k,1:(k-1))+0.0_hp,A(1:(k-1),(k+1):n)))/A(k,k)
          END DO
      ELSE  ! no pivoting and equilibration
        DO k = 1, n
          A(k:n,k) = A(k:n,k) -& ! higher precision computation
              MATMUL(A(k:n,1:(k-1)),A(1:(k-1),k)+0.0_hp)
          A(k,(k+1):n) = (A(k,(k+1):n) -& ! higher precision below
               MATMUL(A(k,1:(k-1))+0.0_hp,A(1:(k-1),(k+1):n)))/A(k,k)
        END DO
      END IF
    END SUBROUTINE lu_crout

    SUBROUTINE lu_crout_solve(A,x,vp)
      INTEGER, OPTIONAL :: vp(:)
      REAL(KIND=WP) :: A(:,:), x(:)
      ! Resolve os sistemas triangulares da decomposicao de Crout
      ! armazenada em A. O vetor x recebe o lado direito e devolve
      ! a solucao. Se o argumento OPCIONAL estiver presente, signi-
      ! fica que foi usada condensacao pivotal e entao as linhas de
      ! x sao permutadas de acordo com a ordenacao em vp.

      INTEGER :: i, n

      n = size(x)
      IF (PRESENT(vp)) x = x(vp)  ! Permutacao das linhas de x
      ! substituicao progressiva
      x(1) = x(1)/A(1,1)
      DO i = 2, n
        x(i) = (x(i) - DOT_PRODUCT(A(i,1:(i-1)),x(1:(i-1))))/A(i,i)
      END DO
      ! substituicao regressiva
      DO i = n-1, 1, -1
        x(i) = x(i) - DOT_PRODUCT(A(i,(i+1):n),x((i+1):n))
      END DO
    END SUBROUTINE lu_crout_solve
END MODULE lu_dcmps