module LinEqu

! Modul odpovidajici pascalske UNITe tehoz jmena

use std_type

implicit none
private

public :: Decomp, Solve, Chodec, Chosol, Svd, Tridig
private :: swap
intrinsic :: abs,epsilon,max,sqrt


contains

subroutine swap(a,b)
    Real(kind=DP),intent(inout) :: a,b
    real(kind=DP) :: t
    t = a
    a = b
    b = t
end subroutine swap


! =================== Solve ================================

subroutine Solve(dimA, A, b, IndxPvt)
    integer, intent(in) :: dimA
    real(kind=DP),dimension(:,:),intent(inout) :: A
    real(kind=DP),dimension(:),intent(inout) :: b
    integer,dimension(:),intent(IN) :: IndxPvt
    
    integer :: i,k,m
    
    if (size(b)/=dimA .or. size(IndxPvt)/=dimA & 
        .or. size(A,1)/=dimA .or. size(A,2)/=dimA) then
        print *,"V Solve je chyba v dimenzich matic nebo vektoru"
        return
    end if
    
    if (dimA == 1) then                 ! dimA = 1
        b(1) = b(1)/A(1,1)
    else                                ! prepocitani prave strany
        do k = 1,dimA-1
            m = IndxPvt(k)
            call swap(b(m),b(k))
            do i = k+1,dimA
                b(i) = b(i) + A(i,k)*b(k)
            end do
        end do
         
        do k = dimA,1,-1                ! zpetna substituce
            b(k) = b(k)/A(k,k)
            do i = 1,k-1
                b(i) = b(i)-A(i,k)*b(k)
            end do
        end do
    end if
    
end subroutine Solve

!========================= Decomp ============================

subroutine Decomp(dimA, A, IndxPvt, JeReg,condA, MinPivot)
    integer, intent(in) :: dimA
    real(kind=DP),dimension(:,:),intent(inout) :: A
    integer,dimension(:),intent(out) :: IndxPvt
    logical,intent(out) :: JeReg
    real(kind=DP),Optional,intent(out) :: condA
    real(kind=DP),optional,intent(in) :: MinPivot
    
    real(kind=DP) :: e,t,normaA,normaY,normaZ,MinPiv
    Integer :: i,j,k,m
    Real(kind=DP),dimension(dimA) :: work           !pracovni vektor
    real(KIND=DP),parameter :: MinP = 1.0E-12_DP    !default MinPivot
    
    if (size(IndxPvt)/=dimA .or. size(A,1)/=dimA &
        .or. size(A,2)/=dimA) then
        print *,"V Decomp je chyba v dimenzich matic nebo vektoru"
        return
    end if    
    if (present(MinPivot)) then
        MinPiv = MinPivot
    else
        MinPiv = MinP
    end if
        
    IndxPvt(dimA) = 1
    JeReg = .true.
    
    if (dimA == 1) then
        if (A(1,1)==0) then
            JeReg =.false.
        else
            condA = 1.0
        end if
        return
    end if
    
    normaA = 0
    do j=1,dimA
        t = 0
        do i=1,dimA
            t = t+abs(A(i,j))
        end do
        normaA = max(t,normaA)
    end do
    
    ! s vyuzitim funkci F95   
    !normaA = max(sum(abs(A(i,:)),dim=1)
    
    ! Gaussova eliminace s castecnym vyberem pivota
    do k=1,dimA-1
        m = k
        do i = k+1,dimA
            if (abs(A(i,k)) > abs(A(m,k))) m = i
        end do
        IndxPvt(k) = m
        if (m /= k) IndxPvt(dimA) = - IndxPvt(dimA)
        call swap(A(m,k),A(k,k))
        if (A(k,k) == 0) cycle            ! vynechat krok, dalsi k
        do i = k+1,dimA
            A(i,k) = - A(i,k)/A(k,k)
        end do
        do j = k+1,dimA
            call swap(A(m,j),A(k,j))
            if (A(k,j) == 0) cycle          ! dalsi krok j
            do i = k+1,dimA
                A(i,j) = A(i,j)+A(i,k)*A(k,j)
            end do
        end do
    end do
    
      
    ! Vypocet odhadu cisla podminenosti
    do k = 1,dimA
        t = 0
        if (k > 1) then 
            do i = 1,k-1
                t = t+A(i,k)*work(i)
            end do
        end if
        e = 1.0_DP
        if (t<0) e = -e
        if (A(k,k) == 0) then       ! singularni A
            JeReg = .false.
            return
        end if
        work(k) = -(e+t)/A(k,k)
    end do
    
    If (present(condA)) then    
        do k = dimA-1,1,-1
            t = 0
            do i = k+1,dimA
                t = t+A(i,k)*work(k)
            end do
            work(k) = t
            m = IndxPvt(k)
            if (m /= k) call swap(work(m),work(k))
        end do
        
        normaY = sum(abs(work))
        call Solve(dimA,A,work,IndxPvt)
        normaZ = sum(abs(work))
        
        condA = max(1.0_DP,normaA*normaZ/normaY)
    end if
      

end subroutine Decomp


!======================= Chodec ==================================

subroutine Chodec (dimA, A1, JePosDef)
    integer,intent(in) :: dimA
    real(kind=DP),dimension(:),intent(inout) :: A1
    logical,intent(out) :: JePosDef
    
    integer :: i,j,k,m,q
    real(kind=DP) :: s
    
    if (size(A1)/=dimA*(dimA+1)/2) then
        print *,"V Chodec je spatna dimenze pole A1"
        return
    end if
    
    JePosDef = .true.
    do j = 1,dimA
        q=j*(j+1)/2
        if (j>1) then
            do i = j,dimA
                m = i*(i-1)/2+j
                s = A1(m)
                do k = 1,j-1
                    s = s-A1(m-k)*A1(q-k)
                end do
                A1(m) = s
            end do
        end if
        if (A1(q)<=0.0_DP) then
            A1(q) = 0.0_DP
            JePosDef = .false.      ! A1 neni positivne definitni
            exit
        else
            s = sqrt(A1(q))
            do i = j,dimA
                m = i*(i-1)/2 + j
                if (s==0.0_DP) then
                    A1(m) = 0.0_DP
                else
                    A1(m) = A1(m)/s
                end if
            end do
        end if
    end do
end subroutine Chodec

!======================== Chosol ==============================

subroutine Chosol (dimA, A1, b)
    integer,intent(in) :: dimA
    real(kind=DP),dimension(:),intent(in) :: A1
    real(kind=DP),dimension(:),intent(inout) :: b
    
    integer :: i,j,pom,q
    
    if (size(A1)/=dimA*(dimA+1)/2 .or. size(b)/=dimA) then
        print *,"V Chosol je spatna dimenze pole A1 nebo vektoru b"
        return
    end if
    
    ! Reseni Ly=b ; prima substituce ; y do b
    if (A1(1)==0) then
        b(1) = 0
    else
        b(1) = b(1)/A1(1)
    end if
    if (dimA>1) then
        q = 1
        do i = 2,dimA
            do j = 1,i-1
                q = q+1
                b(i) = b(i)-A1(q)*b(j)
            end do
            q = q+1
            if (A1(q)==0) then
                b(i) = 0
            else
                b(i) = b(i)/A1(q)
            end if
        end do
    end if
    
    ! Reseni Ux=y; zpetna substituce, x do b
    pom = dimA*(dimA+1)/2
    if (A1(pom)==0) then
        b(dimA) = 0
    else
        b(dimA) = b(dimA)/A1(pom)
    end if
    if (dimA>1) then
        do i = dimA,2,-1
            q = i*(i-1)/2
            do j = 1,i-1
                b(j) = b(j)-b(i)*A1(q+j)
            end do
            if (A1(q)==0) then
                b(i-1) = 0
            else
                b(i-1) = b(i-1)/A1(q)
            end if
        end do
    end if
end subroutine Chosol

    
! ==================== svd =============================

subroutine Svd (radku,sloupcu, A, V, z)
    integer,intent(in) :: radku,sloupcu
    real(kind=DP),dimension(:,:),intent(INOUT) :: A
    real(kind=DP),dimension(:,:),intent(out) :: V
    real(kind=DP),dimension(:),intent(out) :: z
    real(kind=DP),parameter :: minel = 1.0E-17
    
    
    integer :: i,j,k,pocrot
    real(kind=DP) :: p,q,r,c,s,vs,tol,h1,h2
    
    if (size(A,1)/=radku .or. size(A,2)/=sloupcu .or. size(z)/=sloupcu &
        .or. size(V,1)/=sloupcu .or. size(V,2)/=sloupcu) then
        print *,"V Svd je nejaka chyba v dimenzich matic a vektoru"
        return
    end if
    
    ! tolerance pouzivana pri rozhodovani o konvergenci,lze zvolit jinou
    tol = (epsilon(tol)**2)*(sloupcu**2)
    
    ! do V jednotkovou matici
    do i = 1,sloupcu
        do j = 1,sloupcu
            if (i==j) then
                V(i,j) = 1
            else
                V(i,j) = 0
            end if
        end do
    end do
  
    pocrot = sloupcu*(sloupcu-1)/2      ! nastaveni pocitadla rotaci
   
    do      ! cyklus probiha dokud pocrot > 0
        do j = 1,sloupcu-1
            do k = j+1,sloupcu
                p = 0
                q = 0
                r = 0
                do i = 1,radku
                    h1 = A(i,j)
                    if (abs(h1)<minel) h1 = 0
                    h2 = A(i,k)
                    if (abs(h2)<minel) h2 = 0
                    p = p+h1*h2
                    q = q+h1*h1
                    r = r+h2*h2
                end do
                if (q<r) then
                    c = 0           ! zajisti vymenu sloupcu
                    s = 1
                else
                    if ((q*r==0).or.((p/q)*(p/r)<tol)) then
                        pocrot = pocrot-1
                        cycle   ! dalsi k, prvky sloupcu jsou tak male, ze by 
                                !pri soucinu doslo k podteceni nebo rotace 
                                !neni nutna, sloupce j,k jsou ortogonalni
                    end if
                    q = q-r
                    vs = sqrt(4*p*p+q*q)
                    c = sqrt((vs+q)/(vs+vs)) !cos a sin rot. uhlu
                    s = p/(c*vs)
                end if
                do i = 1,radku      ! aplikace rotace na A
                    r = A(i,j)
                    A(i,j) = r*c+s*A(i,k)
                    A(i,k) = -r*s+c*A(i,k)
                end do
                do i = 1,sloupcu    ! aplikace rotace na V
                    r = V(i,j)
                    V(i,j) = r*c+s*V(i,k)
                    V(i,k) = -r*s+c*V(i,k)
                end do
            end do      ! cyklus k
        end do      ! cyklus j
        if (pocrot<=0) exit     !ukonceni hlavniho cyklu
    end do
    
    ! Vypocet singularnich cisel
    do j = 1,sloupcu
        q = 0
        do i = 1,radku
            q = q+A(i,j)**2
        end do
        q = sqrt(q)
        z(j) = q
        if (q>=tol) then
            do i = 1,radku
                A(i,j) = A(i,j)/q
            end do
        end if
    end do
    
end subroutine Svd


!=================== Tridig ============================

subroutine Tridig (n, a, c, b, f, res)
    integer, intent(in) :: n
    real(kind=DP),dimension(:),intent(in) :: a,b
    real(kind=DP),dimension(:),intent(inout) :: c,f
    logical,intent(out) :: res
    
    integer :: i
    real(kind=DP) :: w
    
    ! eliminace p(i) do c(i), q(i) do f(i)
    res = .true.
    w = c(1)
    if (w==0) then
        res = .false.
        return
    else
        f(1) = f(1)/w
    end if
    do i = 2,n
        c(i-1) = b(i-1)/w
        w = c(i)-a(i)*c(i-1)
        if (w==0) then
            res = .false.
            return
        else
            f(i) = (f(i)+a(i)*f(i-1))/w
        end if
    end do
    
    ! zpetna substituce, reseni do f
    do i = n-1,1,-1
        f(i) = c(i)*f(i+1)+f(i)
    end do
end subroutine Tridig


end module LinEqu
