program test_chol_ortho

  implicit none
  integer, parameter    :: n = 4     ! number of rows/cols in matrix
  integer               :: i,j       ! loop counter
  real, dimension(n,n)  :: A, V, B   ! input matrix ...
  real, dimension(n,n)  :: L, LI     ! matrices L and L^-1
  real, dimension(n)    :: p         ! diagonal elements of Cholesky factor L 
  real choldet, det

  data B / &
       +5.000000,  -1.000000,  -1.000000,  -1.000000, &
       -1.000000,   5.000000,  -1.000000,  -1.000000, &
       -1.000000,  -1.000000,   5.000000,  -1.000000, &
       -1.000000,  -1.000000,  -1.000000,   5.000000/
  ! Det(A) = 432.000000

  ! ------------------------------------------------------------------
  ! Chcemy znaleźć układ normalny wektorów umieszczonych w kolumnach A
  ! W tym celu dokonujemy faktoryzacji Cholesky'ego i następnie
  ! obliczamy U = A.(L^-1)'. Kolumny U są ortonormalnymi wektorami
  ! rozpinającymi tę samą przestrzeń co wektory w A.
  ! ------------------------------------------------------------------

  ! .. execution statements ..

  ! save data
  V = B
  print *, 'V - macierz wektorów do ortonormalizacji'
  print 10, ((V(i,j), j=1,n),i=1,n)

  ! Oblicz V'V
  A = matmul(transpose(V), V)
  print *, "V'V"
  print 10, ((A(i,j), j=1,n),i=1,n)

  ! wykonaj rozkład Cholesky'ego macierzy A = V'V => LL'
  ! (wynik w A i p)
  !call choldcmp(A,n,p)
  call choldc(A,n,p)

  ! Wypisz wynik rozkladu LL' jako L
  print *,'A() i p() po choldcmp, czyli L'
  ! oraz zapisz L
  L = 0d0
  do i = 1,n
     L(i,1:i-1) = A(i,1:i-1)
     L(i,i) = p(i)
     print 10, A(i,1:i-1), p(i)
  end do

  ! Oblicz odwrotność L^-1 czynnika Cholesky'ego L
  ! Wynik zapisany w  A(Lower triangle)
  call cholinv(a,p,n)

  ! Wypisz L^-1 (L^-1 jest również macierzą trójkątną dolną)
  LI = 0d0
  do i=1,n
     do j=i,n
        LI(j,i) = A(j,i)
     end do
  end do
  print *, "L^-1"
  print 10, ((LI(i,j), j=1,n),i=1,n)

  ! Sprawdź, czy L*LI == I? (wynik => A)
  A = matmul(L, LI)
  print *, "L.L^-1"
  print 10, ((A(i,j), j=1,n),i=1,n)

  ! Oblicz U = V.L^-1' => A
  A = matmul(V, transpose(LI))
  print *, "U = V.L^-1'"
  print 10, ((A(i,j), j=1,n),i=1,n)

  ! Sprawdź czy A ma ortonormalne kolumny, tzn, czy A'A = I?
  A = matmul(transpose(A), A)
  print *, "V'V"
  print 10, ((A(i,j), j=1,n),i=1,n)

10 format(4f10.4)  
end program test_chol_ortho


! ======================================================================
subroutine choldc (a,n,p)
  implicit none
  integer                    :: i,j,k,n
  real, dimension(n,n)       :: a
  real, dimension(n)         :: p
  real                       :: asum
  !
  ! Given a positive definite symmetric matrix A(1:n,1:n) this routine
  ! constructs its Cholesky decomposition, A=L.L' where L' is
  ! transposed to L. On input, only the upper triangle of A need to be
  ! given; it is not modified. The Cholesky factor L is returned in
  ! the lower triangle of a, except for its diagonal elements which
  ! are returned in p(1:n).  (see: Numerical recipes)
  !
  do i = 1,n
     do j = i,n
        asum = a(i,j)
        do k = i-1,1,-1
           asum = asum-a(i,k)*a(j,k)
        end do
        if(i.eq.j) then
           if (asum.le.0.) stop 'choldc failed'
           p(i) = sqrt(asum)
        else
           a(j,i) = asum / p(i)
        end if
     end do
  end do
  return
end subroutine choldc


! ======================================================================
subroutine choldcmp(A,n,p)
  implicit none
  integer :: n      ! liczba kolumn/wierszy
  real    :: A(n,n) ! macierz
  real    :: p(n)   ! diagonalne elementy faktora Cholesky'ego L
  integer :: j
  !
  ! Given a positive-definite symmetric matrix a(1:n,1:n),
  ! this routine constructs its Cholesky decomposition, A = L·L'.
  ! The Cholesky factor L is returned in the lower triangle of
  ! a, except for its diagonal elements which are returned in p(1:n).
  !
  do j = 1,n
     ! składowe diagonalne
     p(j) = sqrt(A(j,j) - dot_product(A(j,1:j-1),A(j,1:j-1)))
     if (p(j) .eq. 0) stop 'choldcmp failed.'
     ! skladowe pozadiagonalne
     if (j < n) then
        A(j+1:n,j) = (A(j+1:n,j) - &
             matmul(A(j+1:n,1:j-1),A(j,1:j-1))) / p(j)
     end if
  end do
  return
end subroutine choldcmp


! ======================================================================
function choldet(a,n)
  integer n
  real a(n,n)
  real, pointer :: p(:)
  real, pointer :: b(:,:)
  real d
  real choldet
  !
  ! Determinant of the matrix from Cholesky decomposition
  ! Use: choldc(mat,int)
  !
  allocate( p(n), b(n,n) )
  d=1.0
  b = a
  call choldc(b,n,p)
  do i = 1, n
     d = d * p(i)
  end do
  choldet = d * d
  deallocate(p, b)
  return
end Function choldet


! ======================================================================

subroutine cholinv(a,p,n)
  implicit none
  real :: a(n,n), p(n), sum
  integer :: n, i, j, k
  !
  ! Inverse L^-1 of the triangular matrix L (Cholesky factor L)
  !
  ! A(n,n) and p(n) are matrices resulting from choldc routine.
  !
  ! Output:
  !
  ! The lower triangle of A contains the inverse of L.
  !
  ! comment:
  ! Forward substitution method is used to solve the systems AX=I_k
  ! for k = 1,...,n, I_k = (0,...,1_k,0...,0)';
  ! (see: Numerical Recipes)
  !
  do i=1,n
     a(i,i)=1./p(i)
     do j=i+1,n
        sum=0.
        do k=i,j-1
           sum=sum-a(j,k)*a(k,i)
        end do
        a(j,i)=sum/p(j)
     end do
  end do
end subroutine cholinv
