Module gaussFMM

! Procedury Decomp,Solve,Svd 
! "Computer Methods for Mathematical  Computations",
! by George E. Forsythe, Michael A. Malcolm, and Cleve B. Moler
! Prentice-Hall, 1977.
!
! Prepis pro Fortran90:
! Last updated: 28 February 2005 by Ralph Carmichael ralph@pdas.com 
! Public Domain Aeronautical Software 
! P.O. Box 1438 Santa Cruz CA 95061 USA 
!
! Nutne upravy pro jazyk F
! Jan Cely, UFKL,Prirodovedecka fakulta MU, prosinec 2005
! Opravy:
! V procedurach DecompSingle, DecompDouble 
! byla doplnena chybejici inicializace
! ivpn(n)=1

!!=============================================================================
!!	V procedurach REAL(SP|DP) znaci single|double precision
!!-----------------------------------------------------------------------------
!!	SUBROUTINE Decomp(a, ipvt, errCode, cond)
!!		LU rozklad matice a (Gaussova eliminace)
!!	VSTUP:
!!	a		= 	REAL(SP|DP),DIMENSION(n,n) - ctvercova matice pro dekompozici
!!	ipvt 	= 	INTEGER,DIMENSION(n) - vektor pro ukladani informace
!!				o transpozici radku, ipvt(k) = index radku s k-tym pivotem
!!				ipvt(n) = (-1)**(pocet transpozic)
!!	VYSTUP
!!	a		=	matice a prepsana LU rozkladem, U je horni trojuhelnik
!!				L je pod diagonalou (na diagonale ma 1)
!!	errCode	=	INTEGER, 0 = OK (matice je regularni), 1 = matice je singularni
!!	cond	=	REAL(SP|DP),OPTIONAL - je-li pritomne, ulozi se do nej odhad
!!				cisla podminenosti cond(a)
!!-----------------------------------------------------------------------------
!!	SUBROUTINE Solve(a, b, ipvt)
!!		Reseni ax=b zpetnou substituci po Decomp
!!	VSTUP
!!	a		=	matice z vystupu Decomp
!!	ipvt	=	ipvt z vystupu Decomp
!!	b		=	REAL(SP|DP),DIMENSION(n) - vektor prave strany
!!	VYSTUP
!!	b		=	vektor b prepsany resenim x
!!=============================================================================



IMPLICIT NONE

private

! zapis pouzitelny v kompilatoru g95
integer,parameter :: SP = selected_real_kind(4)
integer,parameter :: DP = selected_real_kind(8)

PUBLIC :: Decomp,Solve
private :: DecompSingle,DecompDouble,SolveSingle,SolveDouble
  
INTERFACE Decomp
	MODULE PROCEDURE DecompSingle,DecompDouble
END INTERFACE
  
INTERFACE Solve
    MODULE PROCEDURE SolveSingle,SolveDouble
END INTERFACE
  

CONTAINS
! ----------------------------------------------
!

SUBROUTINE DecompSingle(a, ipvt, errCode, cond)
! ---------------------------------------------------------------------------
! PURPOSE - Matrix triangularization by Gaussian elimination and estimation 
!  of the condition of the matrix. The matrix a is replaced by the LU 
!  decomposition of a rowwise permutation of itself. The array ipvt records
!  the row permutations of the partial pivoting. ipvt(k) is the index of the
!  kth pivot row and ipvt(n) is (-1)**(no. of interchanges). The optional
!  variable cond is an estimate of the conditioning of a. For the linear
!  system Ax=B, changes in A and B may cause changes cond times as large
!  in x. If cond+1.0 == cond, then a is singular to the working precision.
!  The determinant of a can be obtained on output as
!    det = ipvt(n)*a(1,1)*a(2,2)*a(3,3)*...*a(n,n)
! NOTES - If errCode is not zero, the remaining arguments are not to be
!   believed.

  REAL(KIND=SP),INTENT(IN OUT),DIMENSION(:,:):: a   ! matrix to be decomposed
  INTEGER,INTENT(OUT),DIMENSION(:):: ipvt   		! index of pivot rows
  INTEGER,INTENT(OUT):: errCode   					! =0 OK; =1 singular
  REAL(KIND=SP),INTENT(OUT),OPTIONAL:: cond  		! condition number

  REAL(KIND=SP):: anorm
  REAL(KIND=SP):: ek
  INTEGER:: j,k,m
  INTEGER,DIMENSION(1) :: mloc  ! receives the result of MAXLOC
  INTEGER:: n
  REAL(KIND=SP):: t
  REAL(KIND=SP),DIMENSION(SIZE(a,1)) :: work  ! scratch array
  REAL(KIND=SP):: ynorm,znorm
  REAL(KIND=SP),PARAMETER:: ZERO=0.0, ONE=1.0
!-----------------------------------------------------------------------
  errCode=0
  n=SIZE(a,1)

  IF (n <= 1) THEN
    IF (a(1,1) == ZERO) THEN
      errCode=1
    ELSE
      cond=ONE
    END IF
    RETURN        ! note abnormal RETURN
   END IF

  IF (Present(cond)) THEN
    anorm=ZERO   ! compute 1-norm of a
    DO j=1,n
      anorm=MAX(anorm, SUM(ABS(a(1:n,j))))
    END DO   
  END IF  
  
  ipvt(n)=1     !!!  doplnil JC 
  DO k=1,n-1   ! Gaussian elimination with partial pivoting
    mloc= MAXLOC(ABS(a(k:n,k)))   ! pivot row
    m=k-1+mloc(1)
    ipvt(k)=m
    IF (m /= k) ipvt(n)=-ipvt(n)	!formuje se exponent pro (-1)**ipvt(n)
    t=a(m,k)
    a(m,k)=a(k,k)
    a(k,k)=t
    IF (t /= ZERO) THEN
      t=ONE/t
      a(k+1:n,k)=-t*a(k+1:n,k)
      DO j=k+1,n   ! interchange and eliminate by columns.......
        t=a(m,j)
        a(m,j)=a(k,j)
        a(k,j)=t
        IF (t /= ZERO) a(k+1:n,j)=a(k+1:n,j) + t*a(k+1:n,k)
      END DO
    END IF
  END DO

  DO k=1,n   ! solve (a-transpose)*y=e
    t=ZERO
    IF (k > 1) t=DOT_PRODUCT(a(1:k-1,k), work(1:k-1))
    ek=ONE
    IF (t < ZERO) ek=-ONE
    IF (a(k,k) == ZERO) THEN    ! singular matrix
      errCode=1
      RETURN                     ! note abnormal RETURN
    END IF
    work(k)=-(ek+t)/a(k,k)
  END DO

  DO k=n-1,1,-1 
    t=ZERO
    t=work(k)*SUM(a(k+1:n,k))
    work(k)=t
    m=ipvt(k)
    IF (m /= k) THEN
      t=work(m)
      work(m)=work(k)
      work(k)=t
    END IF
  END DO

  IF (Present(cond)) THEN
    ynorm=SUM(ABS(work(1:n)))
    CALL Solve(a,work,ipvt)   ! solve a*z=y
    znorm=SUM(ABS(work(1:n)))
    cond=anorm*znorm/ynorm   ! estimate condition
    IF (cond < ONE) cond=ONE
  END IF
  
  RETURN
END Subroutine DecompSingle 
!------------------------------------------------------------------------------

!+
SUBROUTINE SolveSingle(a, b, ipvt)
! -----------------------------------------------------------------------------
	! PURPOSE - Solve the linear system a*x=b
	! the decomposed matrix from Decomp
  REAL(KIND=SP),INTENT(IN),DIMENSION(:,:):: a 
	! in:right-hand side; out:solution
  REAL(KIND=SP),INTENT(IN OUT),DIMENSION(:):: b
	! index of pivot rows (from Decomp)  
  INTEGER,INTENT(IN),DIMENSION(:):: ipvt  

  INTEGER:: i,k,m
  INTEGER:: n
  REAL(KIND=SP):: t
!----------------------------------------------------------------------------
  n=SIZE(a,1)
  DO k=1,n-1   ! forward elimination
    m=ipvt(k)
    t=b(m)
    b(m)=b(k)
    b(k)=t
!    DO i=k+1,n
!      b(i)=b(i)+a(i,k)*t
!    END DO
    b(k+1:n)=b(k+1:n) + t*a(k+1:n,k)
  END DO       

  DO k=n,1,-1      ! back substitution
    b(k)=b(k)/a(k,k)
    t=-b(k)
    DO i=1,k-1
      b(i)=b(i)+a(i,k)*t
    END DO       
  END DO
  RETURN
END Subroutine SolveSingle
!------------------------------------------------------------------------------

!------------------------------ Double ---------------------------------------


SUBROUTINE DecompDouble(a, ipvt, errCode, cond)
! ---------------------------------------------------------------------------
! PURPOSE - Matrix triangularization by Gaussian elimination and estimation 
!  of the condition of the matrix. The matrix a is replaced by the LU 
!  decomposition of a rowwise permutation of itself. The array ipvt records
!  the row permutations of the partial pivoting. ipvt(k) is the index of the
!  kth pivot row and ipvt(n) is (-1)**(no. of interchanges). The optional
!  variable cond is an estimate of the conditioning of a. For the linear
!  system Ax=B, changes in A and B may cause changes cond times as large
!  in x. If cond+1.0 == cond, then a is singular to the working precision.
!  The determinant of a can be obtained on output as
!    det = ipvt(n)*a(1,1)*a(2,2)*a(3,3)*...*a(n,n)
! NOTES - If errCode is not zero, the remaining arguments are not to be
!   believed.

  REAL(KIND=DP),INTENT(IN OUT),DIMENSION(:,:):: a   ! matrix to be decomposed
  INTEGER,INTENT(OUT),DIMENSION(:):: ipvt   !    index of pivot rows
  INTEGER,INTENT(OUT):: errCode   ! =0 OK; =1 singular
  REAL(KIND=DP),INTENT(OUT),OPTIONAL:: cond  ! condition number

  REAL(KIND=DP):: anorm
  REAL(KIND=DP):: ek
  INTEGER:: j,k,m
  INTEGER,DIMENSION(1) :: mloc  ! receives the result of MAXLOC
  INTEGER:: n
  REAL(KIND=DP):: t
  REAL(KIND=DP),DIMENSION(SIZE(a,1)) :: work  ! scratch array
  REAL(KIND=DP):: ynorm,znorm
  REAL(KIND=DP),PARAMETER:: ZERO=0.0, ONE=1.0
!-----------------------------------------------------------------------
  errCode=0
  n=SIZE(a,1)

  IF (n <= 1) THEN
    IF (a(1,1) == ZERO) THEN
      errCode=1
    ELSE
      cond=ONE
    END IF
    RETURN        ! note abnormal RETURN
   END IF

  IF (Present(cond)) THEN
    anorm=ZERO   ! compute 1-norm of a
    DO j=1,n
      anorm=MAX(anorm, SUM(ABS(a(1:n,j))))
    END DO   
  END IF  

  ipvt(n)=1    !!! doplnil JC
  DO k=1,n-1   ! Gaussian elimination with partial pivoting
    mloc= MAXLOC(ABS(a(k:n,k)))   ! pivot row
    m=k-1+mloc(1)
    ipvt(k)=m
    IF (m /= k) ipvt(n)=-ipvt(n)
    t=a(m,k)
    a(m,k)=a(k,k)
    a(k,k)=t
    IF (t /= ZERO) THEN
      t=ONE/t
      a(k+1:n,k)=-t*a(k+1:n,k)
      DO j=k+1,n   ! interchange and eliminate by columns.......
        t=a(m,j)
        a(m,j)=a(k,j)
        a(k,j)=t
        IF (t /= ZERO) a(k+1:n,j)=a(k+1:n,j) + t*a(k+1:n,k)
      END DO
    END IF
  END DO

  DO k=1,n   ! solve (a-transpose)*y=e
    t=ZERO
    IF (k > 1) t=DOT_PRODUCT(a(1:k-1,k), work(1:k-1))
    ek=ONE
    IF (t < ZERO) ek=-ONE
    IF (a(k,k) == ZERO) THEN    ! singular matrix
      errCode=1
      RETURN                            ! note abnormal RETURN
    END IF
    work(k)=-(ek+t)/a(k,k)
  END DO

  DO k=n-1,1,-1 
    t=ZERO
    t=work(k)*SUM(a(k+1:n,k))
    work(k)=t
    m=ipvt(k)
    IF (m /= k) THEN
      t=work(m)
      work(m)=work(k)
      work(k)=t
    END IF
  END DO

  IF (Present(cond)) THEN
    ynorm=SUM(ABS(work(1:n)))
    CALL Solve(a,work,ipvt)   ! solve a*z=y
    znorm=SUM(ABS(work(1:n)))
    cond=anorm*znorm/ynorm   ! estimate condition
    IF (cond < ONE) cond=ONE
  END IF
  
  RETURN
END Subroutine DecompDouble
!-----------------------------------------------------------------------------

!+
SUBROUTINE SolveDouble(a, b, ipvt)
! ---------------------------------------------------------------------------
	! PURPOSE - Solve the linear system a*x=b
	! the decomposed matrix from Decomp 
 REAL(KIND=DP),INTENT(IN),DIMENSION(:,:):: a 
 	! in:right-hand side; out:solution
  REAL(KIND=DP),INTENT(IN OUT),DIMENSION(:):: b 
 	! index of pivot rows (from Decomp)
  INTEGER,INTENT(IN),DIMENSION(:):: ipvt 

  INTEGER:: i,k,m
  INTEGER:: n
  REAL(KIND=DP):: t
!----------------------------------------------------------------------------
  n=SIZE(a,1)
  DO k=1,n-1   ! forward elimination
    m=ipvt(k)
    t=b(m)
    b(m)=b(k)
    b(k)=t
!    DO i=k+1,n
!      b(i)=b(i)+a(i,k)*t
!    END DO
    b(k+1:n)=b(k+1:n) + t*a(k+1:n,k)
  END DO       

  DO k=n,1,-1      ! back substitution
    b(k)=b(k)/a(k,k)
    t=-b(k)
    DO i=1,k-1
      b(i)=b(i)+a(i,k)*t
    END DO       
  END DO
  RETURN
END Subroutine SolveDouble 
!-----------------------------------------------------------------------------


end module gaussFMM

