; SINGULAR VALUE DECOMPOSITION OF A MATRIX
;
; Stuart Schmitt
; Department of Geology and Geophysics
; University of Wisconsin-Madison
;
; 24 March 2004
;
; This procedure decomposes an input matrix A into three
; output matrices:
;
;   A = U ## diag(S) ## transpose(V)
;
; Procedure parameters:
;   matrix   m x n matrix to be decomposed
;   s        1-D output array of n singular values
;   u        m x n output matrix
;   v        n x n output matrix
;
; If matrix is a double-precision array, the calculation will
; be done fully in double precision.  Otherwise, it will be
; done in single-precision.
;
; The algorithm for this procedure is adapted from:
;
;   Press, William H., Saul A. Teukolsky, William T. Vetterling,
;     Brian P. Flannery, "Numerical recipes in C: The art of
;     Scientific computing," ISBN 0521431085, Cambridge
;     University Press, 1992, http://lib-www.lanl.gov/numerical/
;
; Questions or comments: e-mail stuart@geology.wisc.edu
;

pro svdecomp,matrix,s,u,v

parameters=size(matrix)
m=parameters[2]              ;rows
n=parameters[1]              ;columns
if parameters[1] gt parameters[2] then begin
  print,'ERROR: matrix dimensions not compatible with procedure SVDECOMP'
  s=fltarr(n)+1
  u=matrix
  v=diag(s)
  return
endif

;--EXIT IF INPUT MATRIX IS NOT 2D----------------------------
if parameters[0] ne 2 then begin
  u=0.0
  s=0.0
  v=0.0
  print,'SVDECOMP ERROR: Input matrix not two-dimensional.'
  return
endif
;------------------------------------------------------------


;--DEFINE VARIABLES------------------------------------------
maxits=100

u=matrix
machinelimits=machar()
zero=machinelimits.epsneg
s=fltarr(n)
v=fltarr(n,n)
rv1=fltarr(n)
anorm=0.0
c=0.0
f=0.0
g=0.0
h=0.0
ss=0.0
scale=0.0
x=0.0
y=0.0
z=0.0
if parameters[3] eq 5 then begin
  machinelimits=machar(/double)
  zero=machinelimits.epsneg
  s=double(s)
  v=double(v)
  rv1=double(rv1)
  anorm=0d
  c=0d
  f=0d
  g=0d
  h=0d
  ss=0d
  scale=0d
  x=0d
  y=0d
  z=0d
endif
;------------------------------------------------------------


;--HOUSEHOLDER REDUCTION-------------------------------------
for i=0,n-1 do begin
  l=i+1
  rv1[i]=scale*g
  g=0*g
  ss=0*ss
  scale=0*scale
  if i le m-1 then begin
    for k=i,m-1 do scale=scale+abs(u[i,k])
    if abs(scale) gt zero then begin                               ;zero
      for k=i,m-1 do begin
        u[i,k]=u[i,k]/scale
        ss=ss+u[i,k]^2
      endfor
      f=u[i,i]
      g=-sign(sqrt(ss),f)
      h=f*g-ss
      u[i,i]=f-g
      for j=l,n-1 do begin
        ss=0*ss
        for k=i,m-1 do ss=ss+u[i,k]*u[j,k]
        f=ss/h
        for k=i,m-1 do u[j,k]=u[j,k]+f*u[i,k]
      endfor
      for k=i,m-1 do u[i,k]=u[i,k]*scale
    endif
  endif
  s[i]=scale*g
  g=0*g
  ss=0*ss
  scale=0*scale
  if (i le m-1) and (i ne n-1) then begin
    for k=l,n-1 do scale=scale+abs(u[k,i])
    if abs(scale) gt zero then begin                            ;zero
      for k=l,n-1 do begin
        u[k,i]=u[k,i]/scale
        ss=ss+u[k,i]*u[k,i]
      endfor
      f=u[l,i]
      g=-sign(sqrt(ss),f)
      h=f*g-ss
      u[l,i]=f-g
      for k=l,n-1 do rv1[k]=u[k,i]/h
      for j=l,m-1 do begin
        ss=0*ss
        for k=l,n-1 do ss=ss+u[k,j]*u[k,i]
        for k=l,n-1 do u[k,j]=u[k,j]+ss*rv1[k]
      endfor
      for k=l,n-1 do u[k,i]=u[k,i]*scale
    endif
  endif
  anorm=max([anorm,abs(s[i])+abs(rv1[i])])
endfor
math_debug,check_math(),'Householder debug:'
;------------------------------------------------------------


;--ACCUMULATION OF RIGHT-HAND TRANSFORMATIONS----------------
;
;carried over: g=0, l=10, rv1=[0, nonzero, nonzero, ...]
;
;print,[g,l]
;print,rv1
for i=n-1,0,-1 do begin
  if i lt n-1 then begin
    if abs(g) gt zero then begin                                       ;zero
      for j=l,n-1 do v[i,j]=(u[j,i]/u[l,i])/g
      for j=l,n-1 do begin
        ss=0*ss
        for k=l,n-1 do ss=ss+u[k,i]*v[j,k]
        for k=l,n-1 do v[j,k]=v[j,k]+ss*v[i,k]
      endfor
    endif
    for j=l,n-1 do begin
      v[j,i]=0
      v[i,j]=0
    endfor
  endif
  v[i,i]=1
  g=rv1[i]
  l=i
endfor
math_debug,check_math(),'Right hand debug: '
;------------------------------------------------------------


;--ACCUMULATION OF LEFT-HAND TRANSFORMATIONS-----------------
for i=min([m,n])-1,0,-1 do begin
  l=i+1
  g=s[i]
  for j=l,n-1 do u[j,i]=0 
  if abs(g) gt zero then begin                                          ;zero
    g=1/g
    for j=l,n-1 do begin
      ss=0*ss
      for k=l,m-1 do ss=ss+u[i,k]*u[j,k]
      f=(ss/u[i,i])*g
      for k=i,m-1 do u[j,k]=u[j,k]+f*u[i,k]
    endfor
    for j=i,m-1 do u[i,j]=u[i,j]*g
  endif else for j=i,m-1 do u[i,j]=0
  u[i,i]=u[i,i]+1
endfor
math_debug,check_math(),'Left hand debug: '
;------------------------------------------------------------


;--DIAGONALIZATION OF THE BIDIAGONAL FORM--------------------
for k=n-1,0,-1 do begin
  for its=1,maxits do begin
    flag=1b
    for l=k,0,-1 do begin
      nm=l-1
      if abs(rv1[l]) lt zero then begin
        flag=0b
        break
      endif
      if abs(s[nm]) lt zero then break
    endfor
    if flag ne 0 then begin
      c=0*c
      ss=0*ss+1
      for i=l,k do begin
        f=ss*rv1[i]
        rv1[i]=c*rv1[i]
        if abs(f) lt zero then break
        g=s[i]
        h=mag([f,g])
        s[i]=h
        h=1/h
        c=g*h
        ss=-f*h
        for j=0,m-1 do begin
          y=u[nm,j]
          z=u[i,j]
          u[nm,j]=y*c+z*ss
          u[i,j]=z*c-y*ss
        endfor
      endfor
    endif
    z=s[k]
    if l eq k then begin
      if z lt 0 then begin
        s[k]=-z
        for j=0,n-1 do v[k,j]=-v[k,j]
      endif
      break
    endif
    if its eq maxits then begin
      for j=0,m*n-1 do u[j]=0.0
      for j=0,n-1 do s[j]=0.0
      for j=0,n*n-1 do v[j]=0.0
      math_debug,check_math(),'SVDECOMP ERROR: Failure to converge.  Debug:'
      return
    endif
    x=s[l]
    nm=k-1
    y=s[nm]
    g=rv1[nm]
    h=rv1[k]
    f=((y-z)*(y+z)+(g-h)*(g+h))/(2*h*y)
    g=mag([f,1])
    f=((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x
    c=0*c+1
    ss=0*ss+1
    for j=l,nm do begin
      i=j+1
      g=rv1[i]
      y=s[i]
      h=ss*g
      g=c*g
      z=mag([f,h])
      rv1[j]=z
      c=f/z
      ss=h/z
      f=x*c+g*ss
      g=g*c-x*ss
      h=y*ss
      y=y*c
      for jj=0,n-1 do begin
        x=v[j,jj]
        z=v[i,jj]
        v[j,jj]=x*c+z*ss
        v[i,jj]=z*c-x*ss
      endfor
      z=mag([f,h])
      s[j]=z
      if z gt zero then begin                               ;zero
        z=1/z
        c=f*z
        ss=h*z
      endif
      f=c*g+ss*y
      x=c*y-ss*g
      for jj=0,m-1 do begin
        y=u[j,jj]
        z=u[i,jj]
        u[j,jj]=y*c+z*ss
        u[i,jj]=z*c-y*ss
      endfor
    endfor
    rv1[l]=0
    rv1[k]=f
    s[k]=x
  endfor
;  rv1=0*rv1
;  rv1[n-k-1]=0
endfor
math_debug,check_math(),'Diagonalize debug: '
;------------------------------------------------------------


print,'SVD RMS residual: '+strcompress(string(mag(abs(matrix-u##diag(s)##transpose(v)))),/remove_all)

end