Module LinAlgFMM

! 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

use std_type   !!! nas modul

IMPLICIT NONE

Private

PUBLIC:: Decomp,Solve
private :: DecompSingle,DecompDouble,SolveSingle,SolveDouble
PUBLIC:: SVD
private :: SVDSingle,SVDDouble
  
INTERFACE Decomp
	MODULE PROCEDURE DecompSingle,DecompDouble
END INTERFACE
  
INTERFACE Solve
    MODULE PROCEDURE SolveSingle,SolveDouble
END INTERFACE
  
INTERFACE SVD
	MODULE PROCEDURE SVDSingle,SVDDouble
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
  REAL(KIND=SP),INTENT(IN),DIMENSION(:,:):: a  ! the decomposed matrix from Decomp
  REAL(KIND=SP),INTENT(IN OUT),DIMENSION(:):: b ! in:right-hand side; out:solution
  INTEGER,INTENT(IN),DIMENSION(:):: ipvt  ! index of pivot rows (from Decomp)

  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   ! ----------------------------------------------------



SUBROUTINE SvdSingle(a,w,matu,u,matv,v,ierr)
! ---------------------------------------------------------------------------
! PURPOSE - Determine the singular value decomposition of a real m by n 
!  rectangular matrix.  Householder bidiagonalization and a variant of the 
!  QR algorithm are used. This subroutine is a translation of the Algol 
!  procedure SVD, Num. Math. 14, 403-420(1970) by Golub and Reinsch.
!  Handbook for Auto. Comp., vol II-Linear algebra, 134-151(1971).
!  This is a modified version of a routine from the Eispack collection by 
!  the Nats project. Modified to eliminate machep.

  REAL(KIND=SP), INTENT(IN),DIMENSION(:,:) :: a  ! matrix to be decomposed. 
      ! on output, a is unaltered (unless overwritten by u or v).
  REAL(KIND=SP),INTENT(OUT),DIMENSION(:):: w ! w contains the n (non-negative) 
      ! singular values of a (the diagonal elements of s).  They are
      ! unordered. If an error exit is made, the singular values should be 
      ! correct for indices ierr+1,ierr+2,...,n.
  LOGICAL,INTENT(IN):: matu   ! matu should be set to .TRUE. if the u matrix
      ! in the decomposition is desired, and to .FALSE. otherwise.
  REAL(KIND=SP),INTENT(OUT),DIMENSION(:,:):: u   ! u contains the matrix u of 
      ! orthogonal column vectors of the decomposition if matu has been set
      ! to .TRUE. Otherwise, u is used as a temporary array.  
      ! u may coincide with a.  If an error exit is made, the columns of u 
      ! corresponding to indices of correct singular values should be correct.
  LOGICAL,INTENT(IN):: matv   ! matv should be set to .TRUE. if the v matrix
      ! in the decomposition is desired, and to .FALSE. otherwise.
  REAL(KIND=SP),INTENT(OUT),DIMENSION(:,:):: v   ! v contains the matrix v (orthogonal)
      ! of the decomposition if matv has been set to .TRUE.  Otherwise v is 
      ! not referenced. v may also coincide with a if u is not needed.  If an
      ! error exit is made, the columns of v corresponding to indices of 
      ! correct singular values should be correct.
  INTEGER,INTENT(OUT):: ierr   ! zero for normal return, k if the k-th 
      ! singular value has not been determined after 30 iterations.

  REAL(KIND=SP) :: c,f,g,h
  INTEGER :: i,j,k,l, i1,k1
  INTEGER:: its
  INTEGER:: m,n   ! dimensions of a
 
  REAL(KIND=SP),DIMENSION(SIZE(a,2)):: rv1    ! rv1 (n) is a temporary storage array.
  REAL(KIND=SP):: s,x,y,z,scale,anorm

  REAL(KIND=SP),PARAMETER:: ZERO = 0.0, ONE=1.0, TWO=2.0
!----------------------------------------------------------------------------
  m=SIZE(a,1)
  n=SIZE(a,2)
  IF (SIZE(w) < n) THEN
    ierr=-1
    RETURN
  END IF
  
  IF (matu .AND. (SIZE(u,1) < m .OR. SIZE(u,2) < n) ) THEN
    ierr=-2
    RETURN
  END IF
  
  IF (matv .AND. (SIZE(v,1) < n .OR. SIZE(v,2) < n) ) THEN
    ierr=-3
    RETURN
  END IF
    
  ierr = 0

!DO  i = 1, m
!  DO  j = 1, n
!    u(i,j) = a(i,j)
!  END DO
!END DO
  u(1:m,1:n)=a

  g = ZERO
  scale = ZERO
  anorm = ZERO

DO  i = 1, n   ! Householder reduction to bidiagonal form 
!!!  l = i + 1
  rv1(i) = scale * g
  g = ZERO
  s = ZERO
  scale = ZERO
  IF (i > m) GO TO 210
  
  DO  k = i, m
    scale = scale + ABS(u(k,i))
  END DO
  
  IF (scale == ZERO) GO TO 210
  
  DO  k = i, m
    u(k,i) = u(k,i) / scale
    s = s + u(k,i)**2
  END DO
  
  f = u(i,i)
  g = -SIGN(SQRT(s),f)
  h = f * g - s
  u(i,i) = f - g
!!!  IF (i == n) GO TO 190
  
  DO  j = i+1, n
    s = ZERO   
    DO  k = i, m
      s = s + u(k,i) * u(k,j)
    END DO
    
    f = s / h
    
    DO  k = i, m
      u(k,j) = u(k,j) + f * u(k,i)
    END DO
  END DO
  u(i:m,i)=scale*u(i:m,i)
!!!190 DO  k = i, m
!!!      u(k,i) = scale * u(k,i)
!!!    END DO
  
210 continue
	w(i) = scale * g
    g = ZERO
    s = ZERO
    scale = ZERO
    IF (i > m .OR. i == n) GO TO 290
  
    DO  k = i+1, n
      scale = scale + ABS(u(i,k))
    END DO
  
    IF (scale == ZERO) GO TO 290
  
    DO  k = i+1, n
      u(i,k) = u(i,k) / scale
      s = s + u(i,k)**2
    END DO
  
    f = u(i,i+1)
    g = -SIGN(SQRT(s),f)
    h = f * g - s
    u(i,i+1) = f - g
  
    DO  k = i+1, n
      rv1(k) = u(i,k) / h
    END DO
  
    IF (i == m) GO TO 270
  
    DO  j = i+1, m
      s = ZERO
      DO  k = i+1, n
        s = s + u(j,k) * u(i,k)
      END DO
      DO  k = i+1, n
        u(j,k) = u(j,k) + s * rv1(k)
      END DO
    END DO
  
270 Continue
	DO  k = i+1, n
      u(i,k) = scale * u(i,k)
    END DO
  
290 Continue
	anorm = MAX(anorm,ABS(w(i))+ABS(rv1(i)))
  END DO
!     .......... accumulation of right-hand transformations ..........
  IF (.NOT. matv) GO TO 410
!     .......... for i=n step -1 until 1 do -- ..........
  DO i=n,1,-1
    IF (i == n) GO TO 390
    IF (g == ZERO) GO TO 360
  
    DO  j = i+1, n
      v(j,i) = (u(i,j) / u(i,i+1)) / g   ! double division, YES!
    END DO
  
    DO  j = i+1, n
      s = ZERO
      DO  k = i+1, n
        s = s + u(i,k) * v(k,j)
      END DO
      DO  k = i+1, n
        v(k,j) = v(k,j) + s * v(k,i)
      END DO
    END DO
  
360 Continue
	v(i,i+1:n)=ZERO
    v(i+1:n,i)=ZERO

!    DO  j = i+1, n
!      v(i,j) = ZERO
!      v(j,i) = ZERO
!    END DO
  
390 Continue
	v(i,i) = ONE
    g = rv1(i)
!!!    l = i
  END DO
  
!     .......... accumulation of left-hand transformations ..........
410 Continue
	IF (.NOT. matu) GO TO 510
!     ..........for i=min(m,n) step -1 until 1 do -- ..........
!    mn = n
!    IF (m < n) mn = m
    
    DO i=MIN(m,n),1,-1
!      i = mn + 1 - ii
!      l = i + 1
      g = w(i)
      IF (i == n) GO TO 430
  
      DO  j = i+1, n
        u(i,j) = ZERO
      END DO
  
430 Continue
	IF (g == ZERO) THEN
      u(i:m,i)=ZERO
    ELSE  
      IF (i == MIN(m,n)) GO TO 460
  
      DO  j = i+1, n
        s = ZERO
        DO  k = i+1, m
          s = s + u(k,i) * u(k,j)
        END DO
        f = (s / u(i,i)) / g   ! double division, YES! Avoid underflow
    
        DO  k = i, m
          u(k,j) = u(k,j) + f * u(k,i)
        END DO
      END DO
  
460 Continue
	DO  j = i, m
      u(j,i) = u(j,i) / g
    END DO
  
    END IF  

!475 DO  j = i, m
!      u(j,i) = ZERO
!    END DO
  
    u(i,i) = u(i,i) + ONE
  END DO
  
!     .......... diagonalization of the bidiagonal form ..........
!     .......... for k=n step -1 until 1 do -- ..........
510 Continue
	DO k=n,1,-1
      k1=k-1
!      k1 = n - kk
!      k = k1 + 1
      its = 0
!     .......... test for splitting.
!                for l=k step -1 until 1 do -- ..........
520 Continue
N520: DO
	DO L=k,1,-1
!      l1 = k - ll
!      l = l1 + 1
      IF (ABS(rv1(L)) + anorm <= anorm) GO TO 565
!     .......... rv1(1) is always zero, so there is no exit
!                through the bottom of the loop ..........
      IF (ABS(w(L-1)) + anorm <= anorm) EXIT
    END DO
!     .......... cancellation of rv1(l) if l greater than 1 ..........
    c = ZERO   ! was statement 540
    s = ONE
  
    DO  i = L, k
      f = s * rv1(i)
      rv1(i) = c * rv1(i)
      IF (ABS(f) + anorm <= anorm) EXIT
      g = w(i)
      h = SQRT(f*f+g*g)
      w(i) = h
      c = g / h
      s = -f / h
      IF (matu) THEN
        DO  j = 1, m
          y = u(j,L-1)
          z = u(j,i)
          u(j,L-1) = y * c + z * s
          u(j,i) = -y * s + z * c
        END DO
      END IF
    END DO
    
!     .......... test for convergence ..........
565 Continue
	z = w(k)
    IF (L == k) GO TO 650
!     .......... shift from bottom 2 by 2 minor ..........
    IF (its == 30) GO TO 1000
    its = its + 1
    x = w(L)
    y = w(k1)
    g = rv1(k1)
    h = rv1(k)
    f = ((y - z) * (y + z) + (g - h) * (g + h)) / (TWO * h * y)
    g = SQRT(f*f+ONE)
    f = ((x - z) * (x + z) + h * (y / (f + SIGN(g,f)) - h)) / x
!     .......... next qr transformation ..........
    c = ONE
    s = ONE
  
    DO  i1 = L, k1
      i = i1 + 1
      g = rv1(i)
      y = w(i)
      h = s * g
      g = c * g
      z = SQRT(f*f+h*h)
      rv1(i1) = z
      c = f / z
      s = h / z
      f = x * c + g * s
      g = -x * s + g * c
      h = y * s
      y = y * c
      IF (matv) THEN
        DO  j = 1, n
          x = v(j,i1)
          z = v(j,i)
          v(j,i1) = x * c + z * s
          v(j,i) = -x * s + z * c
        END DO
      END IF    
    z = SQRT(f*f+h*h)   ! was statement #575
    w(i1) = z

    IF (z /= ZERO) THEN
      c = f / z   ! rotation can be arbitrary if z is zero
      s = h / z
    END IF  
    f = c * g + s * y
    x = -s * g + c * y
    IF (matu) THEN
      DO  j = 1, m
        y = u(j,i1)
        z = u(j,i)
        u(j,i1) = y * c + z * s
        u(j,i) = -y * s + z * c
      END DO
    END IF
  END DO
  
  rv1(L) = ZERO
  rv1(k) = f
  w(k) = x
end do N520
!     .......... convergence ..........
650 Continue
	IF (z >= ZERO) CYCLE

    w(k) = -z   !  w(k) is made non-negative 
    IF (matv) v(1:n,k)=-v(1:n,k)
  
!  DO  j = 1, n
!    v(j,k) = -v(j,k)
!  END DO
  
  END DO
  RETURN
  
!  set error -- no convergence to a singular value after 30 iterations ......
1000 Continue 
	ierr = k
  RETURN
END SUBROUTINE SvdSingle


!---------------- 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
  REAL(KIND=DP),INTENT(IN),DIMENSION(:,:):: a  ! the decomposed matrix from Decomp
  REAL(KIND=DP),INTENT(IN OUT),DIMENSION(:):: b ! in:right-hand side; out:solution
  INTEGER,INTENT(IN),DIMENSION(:):: ipvt  ! index of pivot rows (from Decomp)

  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   ! ----------------------------------------------------

SUBROUTINE SvdDouble(a,w,matu,u,matv,v,ierr)
! ---------------------------------------------------------------------------
! PURPOSE - Determine the singular value decomposition of a real m by n 
!  rectangular matrix.  Householder bidiagonalization and a variant of the 
!  QR algorithm are used. This subroutine is a translation of the Algol 
!  procedure SVD, Num. Math. 14, 403-420(1970) by Golub and Reinsch.
!  Handbook for Auto. Comp., vol II-Linear algebra, 134-151(1971).
!  This is a modified version of a routine from the Eispack collection by 
!  the Nats project. Modified to eliminate machep.

  REAL(KIND=DP), INTENT(IN),DIMENSION(:,:) :: a  ! matrix to be decomposed. 
      ! on output, a is unaltered (unless overwritten by u or v).
  REAL(KIND=DP),INTENT(OUT),DIMENSION(:):: w ! w contains the n (non-negative) 
      ! singular values of a (the diagonal elements of s).  They are
      ! unordered. If an error exit is made, the singular values should be 
      ! correct for indices ierr+1,ierr+2,...,n.
  LOGICAL,INTENT(IN):: matu   ! matu should be set to .TRUE. if the u matrix
      ! in the decomposition is desired, and to .FALSE. otherwise.
  REAL(KIND=DP),INTENT(OUT),DIMENSION(:,:):: u   ! u contains the matrix u of 
      ! orthogonal column vectors of the decomposition if matu has been set
      ! to .TRUE. Otherwise, u is used as a temporary array.  
      ! u may coincide with a.  If an error exit is made, the columns of u 
      ! corresponding to indices of correct singular values should be correct.
  LOGICAL,INTENT(IN):: matv   ! matv should be set to .TRUE. if the v matrix
      ! in the decomposition is desired, and to .FALSE. otherwise.
  REAL(KIND=DP),INTENT(OUT),DIMENSION(:,:):: v   ! v contains the matrix v (orthogonal)
      ! of the decomposition if matv has been set to .TRUE.  Otherwise v is 
      ! not referenced. v may also coincide with a if u is not needed.  If an
      ! error exit is made, the columns of v corresponding to indices of 
      ! correct singular values should be correct.
  INTEGER,INTENT(OUT):: ierr   ! zero for normal return, k if the k-th 
      ! singular value has not been determined after 30 iterations.

  REAL(KIND=DP) :: c,f,g,h
  INTEGER :: i,j,k,l, i1,k1
  INTEGER:: its
  INTEGER:: m,n   ! dimensions of a
 
  REAL(KIND=DP),DIMENSION(SIZE(a,2)):: rv1    ! rv1 (n) is a temporary storage array.
  REAL(KIND=DP):: s,x,y,z,scale,anorm

  REAL(KIND=DP),PARAMETER:: ZERO = 0.0, ONE=1.0, TWO=2.0
!----------------------------------------------------------------------------
  m=SIZE(a,1)
  n=SIZE(a,2)
  IF (SIZE(w) < n) THEN
    ierr=-1
    RETURN
  END IF
  
  IF (matu .AND. (SIZE(u,1) < m .OR. SIZE(u,2) < n) ) THEN
    ierr=-2
    RETURN
  END IF
  
  IF (matv .AND. (SIZE(v,1) < n .OR. SIZE(v,2) < n) ) THEN
    ierr=-3
    RETURN
  END IF
    
  ierr = 0

!DO  i = 1, m
!  DO  j = 1, n
!    u(i,j) = a(i,j)
!  END DO
!END DO
  u(1:m,1:n)=a

  g = ZERO
  scale = ZERO
  anorm = ZERO

DO  i = 1, n   ! Householder reduction to bidiagonal form 
!!!  l = i + 1
  rv1(i) = scale * g
  g = ZERO
  s = ZERO
  scale = ZERO
  IF (i > m) GO TO 210
  
  DO  k = i, m
    scale = scale + ABS(u(k,i))
  END DO
  
  IF (scale == ZERO) GO TO 210
  
  DO  k = i, m
    u(k,i) = u(k,i) / scale
    s = s + u(k,i)**2
  END DO
  
  f = u(i,i)
  g = -SIGN(SQRT(s),f)
  h = f * g - s
  u(i,i) = f - g
!!!  IF (i == n) GO TO 190
  
  DO  j = i+1, n
    s = ZERO   
    DO  k = i, m
      s = s + u(k,i) * u(k,j)
    END DO
    
    f = s / h
    
    DO  k = i, m
      u(k,j) = u(k,j) + f * u(k,i)
    END DO
  END DO
  u(i:m,i)=scale*u(i:m,i)
!!!190 DO  k = i, m
!!!      u(k,i) = scale * u(k,i)
!!!    END DO
  
210 continue
	w(i) = scale * g
    g = ZERO
    s = ZERO
    scale = ZERO
    IF (i > m .OR. i == n) GO TO 290
  
    DO  k = i+1, n
      scale = scale + ABS(u(i,k))
    END DO
  
    IF (scale == ZERO) GO TO 290
  
    DO  k = i+1, n
      u(i,k) = u(i,k) / scale
      s = s + u(i,k)**2
    END DO
  
    f = u(i,i+1)
    g = -SIGN(SQRT(s),f)
    h = f * g - s
    u(i,i+1) = f - g
  
    DO  k = i+1, n
      rv1(k) = u(i,k) / h
    END DO
  
    IF (i == m) GO TO 270
  
    DO  j = i+1, m
      s = ZERO
      DO  k = i+1, n
        s = s + u(j,k) * u(i,k)
      END DO
      DO  k = i+1, n
        u(j,k) = u(j,k) + s * rv1(k)
      END DO
    END DO
  
270 continue
	DO  k = i+1, n
      u(i,k) = scale * u(i,k)
    END DO
  
290 continue
	anorm = MAX(anorm,ABS(w(i))+ABS(rv1(i)))
  END DO
!     .......... accumulation of right-hand transformations ..........
  IF (.NOT. matv) GO TO 410
!     .......... for i=n step -1 until 1 do -- ..........
  DO i=n,1,-1
    IF (i == n) GO TO 390
    IF (g == ZERO) GO TO 360
  
    DO  j = i+1, n
      v(j,i) = (u(i,j) / u(i,i+1)) / g   ! double division, YES!
    END DO
  
    DO  j = i+1, n
      s = ZERO
      DO  k = i+1, n
        s = s + u(i,k) * v(k,j)
      END DO
      DO  k = i+1, n
        v(k,j) = v(k,j) + s * v(k,i)
      END DO
    END DO
  
360 continue
	v(i,i+1:n)=ZERO
    v(i+1:n,i)=ZERO

!    DO  j = i+1, n
!      v(i,j) = ZERO
!      v(j,i) = ZERO
!    END DO
  
390 Continue
	v(i,i) = ONE
    g = rv1(i)
!!!    l = i
  END DO
  
!     .......... accumulation of left-hand transformations ..........
410 Continue
	IF (.NOT. matu) GO TO 510
!     ..........for i=min(m,n) step -1 until 1 do -- ..........
!    mn = n
!    IF (m < n) mn = m
    
    DO i=MIN(m,n),1,-1
!      i = mn + 1 - ii
!      l = i + 1
      g = w(i)
      IF (i == n) GO TO 430
  
      DO  j = i+1, n
        u(i,j) = ZERO
      END DO
  
430 Continue
	IF (g == ZERO) THEN
      u(i:m,i)=ZERO
    ELSE  
      IF (i == MIN(m,n)) GO TO 460
  
      DO  j = i+1, n
        s = ZERO
        DO  k = i+1, m
          s = s + u(k,i) * u(k,j)
        END DO
        f = (s / u(i,i)) / g   ! double division, YES! Avoid underflow
    
        DO  k = i, m
          u(k,j) = u(k,j) + f * u(k,i)
        END DO
      END DO
  
460 continue
	DO  j = i, m
      u(j,i) = u(j,i) / g
    END DO
  
    END IF  

!475 DO  j = i, m
!      u(j,i) = ZERO
!    END DO
  
    u(i,i) = u(i,i) + ONE
  END DO
  
!     .......... diagonalization of the bidiagonal form ..........
!     .......... for k=n step -1 until 1 do -- ..........
510 Continue
	DO k=n,1,-1
      k1=k-1
!      k1 = n - kk
!      k = k1 + 1
      its = 0
!     .......... test for splitting.
!                for l=k step -1 until 1 do -- ..........
520 Continue
N520: DO
	DO L=k,1,-1
!      l1 = k - ll
!      l = l1 + 1
      IF (ABS(rv1(L)) + anorm <= anorm) GO TO 565
!     .......... rv1(1) is always zero, so there is no exit
!                through the bottom of the loop ..........
      IF (ABS(w(L-1)) + anorm <= anorm) EXIT
    END DO
!     .......... cancellation of rv1(l) if l greater than 1 ..........
    c = ZERO   ! was statement 540
    s = ONE
  
    DO  i = L, k
      f = s * rv1(i)
      rv1(i) = c * rv1(i)
      IF (ABS(f) + anorm <= anorm) EXIT
      g = w(i)
      h = SQRT(f*f+g*g)
      w(i) = h
      c = g / h
      s = -f / h
      IF (matu) THEN
        DO  j = 1, m
          y = u(j,L-1)
          z = u(j,i)
          u(j,L-1) = y * c + z * s
          u(j,i) = -y * s + z * c
        END DO
      END IF
    END DO
    
!     .......... test for convergence ..........
565 Continue
	z = w(k)
    IF (L == k) GO TO 650
!     .......... shift from bottom 2 by 2 minor ..........
    IF (its == 30) GO TO 1000
    its = its + 1
    x = w(L)
    y = w(k1)
    g = rv1(k1)
    h = rv1(k)
    f = ((y - z) * (y + z) + (g - h) * (g + h)) / (TWO * h * y)
    g = SQRT(f*f+ONE)
    f = ((x - z) * (x + z) + h * (y / (f + SIGN(g,f)) - h)) / x
!     .......... next qr transformation ..........
    c = ONE
    s = ONE
  
    DO  i1 = L, k1
      i = i1 + 1
      g = rv1(i)
      y = w(i)
      h = s * g
      g = c * g
      z = SQRT(f*f+h*h)
      rv1(i1) = z
      c = f / z
      s = h / z
      f = x * c + g * s
      g = -x * s + g * c
      h = y * s
      y = y * c
      IF (matv) THEN
        DO  j = 1, n
          x = v(j,i1)
          z = v(j,i)
          v(j,i1) = x * c + z * s
          v(j,i) = -x * s + z * c
        END DO
      END IF    
    z = SQRT(f*f+h*h)   ! was statement #575
    w(i1) = z

    IF (z /= ZERO) THEN
      c = f / z   ! rotation can be arbitrary if z is zero
      s = h / z
    END IF  
    f = c * g + s * y
    x = -s * g + c * y
    IF (matu) THEN
      DO  j = 1, m
        y = u(j,i1)
        z = u(j,i)
        u(j,i1) = y * c + z * s
        u(j,i) = -y * s + z * c
      END DO
    END IF
  END DO
  
  rv1(L) = ZERO
  rv1(k) = f
  w(k) = x
end do N520
!     .......... convergence ..........
650 Continue
	IF (z >= ZERO) CYCLE

    w(k) = -z   !  w(k) is made non-negative 
    IF (matv) v(1:n,k)=-v(1:n,k)
  
!  DO  j = 1, n
!    v(j,k) = -v(j,k)
!  END DO
  
  END DO
  RETURN
  
!  set error -- no convergence to a singular value after 30 iterations ......
1000 continue 
	ierr = k
  RETURN
END SUBROUTINE SvdDouble

end module LinAlgFMM

