!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! Singular Value Decomposition
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
module cubemain_svd_tool
  use cubetools_parameters
  use cubemain_messaging
  !
  public :: svd_t
  private
  !
  type svd_t
     integer(kind=chan_k),  private :: mp = 0
     integer(kind=inte_k),  private :: np = 0
     real(kind=4), pointer, private :: u(:,:) => null() ! mp x np
     real(kind=4), pointer, private :: v(:,:) => null() ! np x np
     real(kind=4), pointer, private :: w(:)   => null() ! np
   contains
     procedure, public  :: reallocate     => cubemain_svd_reallocate
     procedure, public  :: fit            => cubemain_svd_fit
     procedure, public  :: minloc_w_lt_np => cubemain_svd_minloc_w_lt_np
     procedure, private :: compute        => cubemain_svd_compute
     procedure, private :: solve_for      => cubemain_svd_solve_for
     procedure, private :: free           => cubemain_svd_free
     final :: cubemain_svd_final
  end type svd_t
  !
contains
  !
  subroutine cubemain_svd_reallocate(svd,mp,np,error)
    use gkernel_interfaces
    !----------------------------------------------------------------------
    ! 
    !----------------------------------------------------------------------
    class(svd_t),         intent(inout) :: svd
    integer(kind=chan_k), intent(in)    :: mp
    integer(kind=inte_k), intent(in)    :: np
    logical,              intent(inout) :: error
    !
    logical :: alloc
    integer(kind=4) :: ier
    character(len=mess_l) :: mess
    character(len=*), parameter :: rname='SVD>REALLOCATE'
    !
    call cubemain_message(mainseve%trace,rname,'Welcome')
    !
    ! Sanity check
    if (mp.le.0) then
       call cubemain_message(seve%e,rname,'Negative or zero number of channels')
       error = .true.
       return
    endif
    if (np.le.0) then
       call cubemain_message(seve%e,rname,'Negative or zero number of coefficients')
       error = .true.
       return
    endif
    alloc = .true.
    if (associated(svd%u)) then
       if ((svd%mp.eq.mp).and.(svd%np.eq.np)) then
          write(mess,'(a,i0,a,i0)')  &
               'svd pointers already associated at the right size: ',mp,' x ',np
          call cubemain_message(mainseve%alloc,rname,mess)
          alloc = .false.
       else
          if (svd%np.ne.np) then
             ! At least np changed => Reallocate everybody
             write(mess,'(a)') &
                  'svd pointers already associated but with a different size => Freeing them first'
             call cubemain_message(mainseve%alloc,rname,mess)
             call svd%free(error)
             if (error) return
             alloc = .true.
          else 
             ! Only mp has changed => Reallocate only U
             write(mess,'(a)') &
                  'svd%u pointer already associated but with a different size => Freeing it first'
             call cubemain_message(mainseve%alloc,rname,mess)
             if (associated(svd%u)) deallocate(svd%u)
             allocate(svd%u(mp,np),stat=ier)
             if (failed_allocate(rname,' svd%u',ier,error)) return
             alloc = .false.
          endif
       endif
    endif
    if (alloc) then
       allocate(svd%u(mp,np),svd%v(np,np),svd%w(np),stat=ier)
       if (failed_allocate(rname,'svd pointers',ier,error)) return
    endif
    ! Allocation success => svd%mp and svd%np may be updated
    svd%mp = mp
    svd%np = np
  end subroutine cubemain_svd_reallocate
  !
  subroutine cubemain_svd_free(svd,error)
    !----------------------------------------------------------------------
    ! 
    !----------------------------------------------------------------------
    class(svd_t), intent(inout) :: svd
    logical,      intent(inout) :: error
    !
    character(len=*), parameter :: rname='SVD>FREE'
    !
    call cubemain_message(mainseve%trace,rname,'Welcome')
    !
    svd%mp = 0
    svd%np = 0
    if (associated(svd%u)) deallocate(svd%u)
    if (associated(svd%v)) deallocate(svd%v)
    if (associated(svd%w)) deallocate(svd%w)
  end subroutine cubemain_svd_free
  !
  subroutine cubemain_svd_final(svd)
    !----------------------------------------------------------------------
    ! 
    !----------------------------------------------------------------------
    type(svd_t), intent(inout) :: svd
    !
    logical :: error
    character(len=*), parameter :: rname='SVD>FINAL'
    !
    call cubemain_message(mainseve%trace,rname,'Welcome')
    !
    call svd%free(error)
  end subroutine cubemain_svd_final
  !
  !------------------------------------------------------------------------
  !
  subroutine cubemain_svd_fit(svd,data,a,ma,chisq,funcs,error)
    use cubeadm_spectrum_types
    !----------------------------------------------------------------------
    ! Given a set of data%n points data%x(id), data%y(id) with weights
    ! data%w(id), use Chi2 minimization to determine the MA coefficients A
    ! of the fitting function. Here we solve the fitting equations using
    ! singular value decomposition of the data%n by MA matrix.
    !
    ! The user supplies a subroutine FUNCS(X,AFUNC,MA) that returns the MA
    ! basis functions evaluated at x=X in the array AFUNC.
    !
    ! The programs returns values for the MA fit parameters and Chi2, CHISQ. 
    !
    ! Arrays U,V,W provide workspace on input. On output they define the
    ! singular value decomposition and can be used to obtain the covariance
    ! matrix.
    !
    ! It is necessary that svd%mp >= data%n, svd%np >= ma.
    !----------------------------------------------------------------------
    class(svd_t),         intent(inout) :: svd
    type(spectrum_t),     intent(in)    :: data
    real(kind=4),         intent(out)   :: a(ma)
    integer(kind=4),      intent(in)    :: ma
    real(kind=sign_k),    intent(out)   :: chisq
    external                            :: funcs
    logical,              intent(inout) :: error
    !
    real(kind=4), parameter :: tol=1.e-5
    real(kind=4) :: b(data%n)
    real(kind=4) :: afunc(ma)
    integer(kind=4) :: id,j
    real(kind=4) :: wmax,thresh,sum,tmp
    character(len=*), parameter :: rname='SVD>FIT'
    !
    ! Initialize the svd%u matrix
    do id=1,data%n
       call funcs(data%x%val(id),afunc,ma)
       tmp = sqrt(data%w%val(id))
       do j=1,ma
          svd%u(id,j) = afunc(j)*tmp
       enddo
       b(id) = data%y%val(id)*tmp
    enddo ! id
    call svd%compute(data%n,ma,error)
    if (error) goto 10
    ! Edit out the (nearly) singular values
    wmax = 0.
    do j=1,ma
       if (svd%w(j).gt.wmax) wmax=svd%w(j)
    enddo
    thresh = tol*wmax
    do j=1,ma
       if (svd%w(j).lt.thresh) svd%w(j) = 0
    enddo
    ! Solve the equations
    call svd%solve_for(data%n,ma,b,a,error)
    if (error) goto 10
    ! Evaluate chi-square
    chisq = 0.
    do id=1,data%n
       call funcs(data%x%val(id),afunc,ma)
       sum=0.
       do j=1,ma
          sum = sum+a(j)*afunc(j)
       enddo
       chisq = chisq+(data%y%val(id)-sum)**2*data%w%val(id)
    enddo ! id
    return
    ! Exception handler
10  call cubemain_message(seve%e,rname,'Error in singular value decomposition')
  end subroutine cubemain_svd_fit
  !
  function cubemain_svd_minloc_w_lt_np(svd) result(test)
    !---------------------------------------------------------------------
    !
    !---------------------------------------------------------------------
    class(svd_t), intent(in)  :: svd
    logical                   :: test
    !
    test = minloc(svd%w,1).lt.svd%np
  end function cubemain_svd_minloc_w_lt_np
  !
  !------------------------------------------------------------------------
  !
  subroutine cubemain_svd_compute(svd,m,n,error)
    !---------------------------------------------------------------------
    ! Given a matrix A with logical dimensions M by N and physical dimensions
    ! MP by NP, this routine computes its singular value decomposition
    !	A = U.W.Vt
    !
    ! The matrix U is initialized to A on input and computed to its actual
    ! value in output. The diagonal matrix of singular values W is output
    ! as vector W. The matrix V (not the transpose Vt) is output as V.
    !
    ! M must be greater or equal to N. If it is smaller, then A should be
    ! filled up to square with zero rows.
    !---------------------------------------------------------------------
    class(svd_t),         intent(inout) :: svd
    integer(kind=chan_k), intent(in)    :: m
    integer(kind=4),      intent(in)    :: n
    logical,              intent(out)   :: error
    !
    integer(kind=4), parameter :: nmax=100 ! Maximum anticipated value of N
    real(kind=4) ::  rv1(nmax)
    integer(kind=4) :: i,j,k,l,nm,its,jj
    real(kind=4) :: scale,anorm,c,s,x,y,z
    real(kind=4) :: f,g,h
    character(len=*), parameter :: rname='SVD>COMPUTE'
    !
    if (n.gt.nmax) then
       call cubemain_message(seve%e,rname,'NMAX dimension too small => Recompilation needed')
       error = .true.
       return
    elseif (m.lt.n) then
       call cubemain_message(seve%e,rname,'You must add extra zero rows to A')
       error = .true.
       return
    endif
    ! Householder reduction to diagonal form
    g = 0.0
    scale = 0.0
    anorm = 0.0
    do i=1,n
       l=i+1
       rv1(i) = scale*g
       g = 0.0
       s = 0.0
       scale = 0.0
       if (i.le.m) then
          do k=i,m
             scale = scale+abs(svd%u(k,i))
          enddo
          if (scale.ne.0.0) then
             do k=i,m
                svd%u(k,i) = svd%u(k,i)/scale
                s = s+svd%u(k,i)*svd%u(k,i)
             enddo
             f = svd%u(i,i)
             g = -sign(sqrt(s),f)
             h = f*g-s
             svd%u(i,i) = f-g
             if (i.ne.n) then
                do j=l,n
                   s=0.0
                   do k=i,m
                      s = s+svd%u(k,i)*svd%u(k,j)
                   enddo
                   f = s/h
                   do k=i,m
                      svd%u(k,j) = svd%u(k,j)+f*svd%u(k,i)
                   enddo
                enddo
             endif
             do k=i,m
                svd%u(k,i) = scale*svd%u(k,i)
             enddo
          endif
       endif
       svd%w(i) = scale*g
       g = 0.0
       s = 0.0
       scale = 0.0
       if ((i.le.m).and.(i.ne.n)) then
          do k=l,n
             scale = scale+abs(svd%u(i,k))
          enddo
          if (scale.ne.0) then
             do k=l,n
                svd%u(i,k)=svd%u(i,k)/scale
                s=s+svd%u(i,k)*svd%u(i,k)
             enddo
             f=svd%u(i,l)
             g=-sign(sqrt(s),f)
             h=f*g-s
             svd%u(i,l)=f-g
             do k=l,n
                rv1(k)=svd%u(i,k)/h
             enddo
             if (i.ne.m) then
                do j=l,m
                   s=0.0
                   do k=l,n
                      s=s+svd%u(j,k)*svd%u(i,k)
                   enddo
                   do k=l,n
                      svd%u(j,k)=svd%u(j,k)+s*rv1(k)
                   enddo
                enddo
             endif
             do k=l,n
                svd%u(i,k)=scale*svd%u(i,k)
             enddo
          endif
       endif
       anorm = max(anorm,(abs(svd%w(i))+abs(rv1(i))))
    enddo
    ! Accumulation of right hand transformation
    do i=n,1,-1
       if (i.lt.n) then
          if (g.ne.0.0) then
             do j=l,n
                ! Double division avoids possible underflow
                svd%v(j,i) = (svd%u(i,j)/svd%u(i,l))/g
             enddo
             do j=l,n
                s=0.0
                do k=l,n
                   s=s+svd%u(i,k)*svd%v(k,j)
                enddo
                do k=l,n
                   svd%v(k,j)=svd%v(k,j)+s*svd%v(k,i)
                enddo
             enddo
          endif
          do j=l,n
             svd%v(i,j) = 0.0
             svd%v(j,i) = 0.0
          enddo
       endif
       svd%v(i,i) = 1.0
       g = rv1(i)
       l = i
    enddo
    ! Accumulation of left hand transformations
    do i=n,1,-1
       l = i+1
       g = svd%w(i)
       if (i.lt.n) then
          do j=l,n
             svd%u(i,j) = 0.0
          enddo
       endif
       if (g.ne.0.0) then
          g = 1.0/g
          if (i.ne.n) then
             do j=l,n
                s = 0.0
                do k=l,m
                   s = s+svd%u(k,i)*svd%u(k,j)
                enddo
                f = (s/svd%u(i,i))*g
                do k=i,m
                   svd%u(k,j)=svd%u(k,j)+f*svd%u(k,i)
                enddo
             enddo
          endif
          do j=i,m
             svd%u(j,i) = svd%u(j,i)*g
          enddo
       else
          do j=i,m
             svd%u(j,i) = 0.0
          enddo
       endif
       svd%u(i,i) = svd%u(i,i)+1.0
    enddo
    ! Diagonalization of the bidiagonal form
    do k=n,1,-1 ! Loop over singular values
       do its=1,30 ! Loop over allowed iterations
          do l=k,1,-1 ! Test for splitting:
             nm=l-1 ! Note that RV1(1) is always zero
             if ((abs(rv1(l))+anorm).eq.anorm) goto 2
             if ((abs(svd%w(nm))+anorm).eq.anorm) goto 1
          enddo
1         continue
          c=0.0 ! Cancellation of RV1(L), if L>1
          s=1.0
          do i=l,k
             f=s*rv1(i)
             if ((abs(f)+anorm).ne.anorm) then
                g = svd%w(i)
                h = sqrt(f*f+g*g)
                svd%w(i) = h
                h = 1.0/h
                c = (g*h)
                s = -f*h
                do j=1,m
                   y = svd%u(j,nm)
                   z = svd%u(j,i)
                   svd%u(j,nm)=(y*c)+(z*s)
                   svd%u(j,i)=-(y*s)+(z*c)
                enddo
             endif
          enddo
2         continue
          z=svd%w(k)
          if (l.eq.k) then
             ! Convergence
             if (z.lt.0.0) then
                ! Singular value is made non negative
                svd%w(k)=-z
                do j=1,n
                   svd%v(j,k)=-svd%v(j,k)
                enddo
             endif
             goto 3
          endif
          if (its.eq.30) then
             call cubemain_message(seve%e,rname,'No convergence in 30 iterations')
             error = .true.
             return
          endif
          x=svd%w(l) ! Shift from bottom 2-by-2 minor
          nm = k-1
          y = svd%w(nm)
          g = rv1(nm)
          h = rv1(k)
          f = ((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y)
          g = sqrt(f*f+1.0)
          f = ((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x
          ! Next QR transformation
          c=1.0
          s=1.0
          do j=l,nm
             i=j+1
             g=rv1(i)
             y=svd%w(i)
             h=s*g
             g=c*g
             z=sqrt(f*f+h*h)
             rv1(j) = z
             c=f/z
             s=h/z
             f= (x*c)+(g*s)
             g=-(x*s)+(g*c)
             h=y*s
             y=y*c
             do jj=1,n
                x=svd%v(jj,j)
                z=svd%v(jj,i)
                svd%v(jj,j)= (x*c)+(z*s)
                svd%v(jj,i)=-(x*s)+(z*c)
             enddo
             z=sqrt(f*f+h*h)
             svd%w(j) = z
             if (z.ne.0) then
                ! Rotation can be arbitrary if Z=0
                z=1.0/z
                c=f*z
                s=h*z
             endif
             f= (c*g)+(s*y)
             x=-(s*g)+(c*y)
             do jj=1,m
                y=svd%u(jj,j)
                z=svd%u(jj,i)
                svd%u(jj,j) = (y*c)+(z*s)
                svd%u(jj,i) =-(y*s)+(z*c)
             enddo
          enddo
          rv1(l)=0.0
          rv1(k)=f
          svd%w(k)=x
       enddo
3      continue
    enddo
  end subroutine cubemain_svd_compute
  !
  subroutine cubemain_svd_solve_for(svd,m,n,b,x,error)
    !---------------------------------------------------------------------
    ! Solves A.X = B for a vector X, where A is specified by the arrays U,W,V
    ! as returned by svd%compute.
    !
    ! M and N are the logical dimensions of A, and will be equal for square
    ! matrices. MP and NP are the physical dimensions of A.
    !
    ! B is the input right hand side. X is the output solution vector. No
    ! input quantities are destroyed, so the routine may be called
    ! sequentially with different B's.
    !---------------------------------------------------------------------
    class(svd_t),         intent(in)  :: svd
    integer(kind=chan_k), intent(in)  :: m
    integer(kind=4),      intent(in)  :: n
    real(kind=4),         intent(in)  :: b(svd%mp)
    real(kind=4),         intent(out) :: x(svd%np)
    logical,              intent(out) :: error
    !
    integer(kind=4), parameter :: nmax=100
    real(kind=4) :: tmp(nmax),s
    integer(kind=4) :: i,j,jj
    character(len=*), parameter :: rname='SVD>SOLVE>FOR'
    !
    if (n.gt.nmax) then
       call cubemain_message(seve%e,rname,'NMAX dimension too small => Will need to recompile')
       error = .true.
       return
    endif
    ! Calculate UtB
    do j=1,n
       s = 0.
       if (svd%w(j).ne.0.) then
          do i=1,m
             s=s+svd%u(i,j)*b(i)
          enddo
          s = s/svd%w(j)
       endif
       tmp(j) = s
    enddo
    ! Matrix multiply by V to get answer
    do j=1,n
       s = 0.0
       do jj=1,n
          s = s+svd%v(j,jj)*tmp(jj)
       enddo
       x(j) = s
    enddo
  end subroutine cubemain_svd_solve_for
end module cubemain_svd_tool
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
