module linear
! lineaarialgebran rutiineja:
! - matriisin LU-hajotelman laskeminen
! - lineaarisen yhtälöryhmän ratkaisu
! - käänteismatriisin laskeminen

implicit none

contains

subroutine lu (A, order, n)
! Lasketaan matriisin A(n,n) LU-hajotelma.
! Hajotelma talletetaan alkuperäiseen matriisin tilalle.
! Ulostuloparametri order(n) on vektori, joka kertoo, mihin
! järjestykseen matriisin rivit ovat  vaihtuneet pivotoinnissa
! Huom: hajotelman tulo on alkuperäisen matriisin permutoitu muoto

  integer, intent(in) :: n
  real, dimension(n,n), intent(inout) :: A
  integer, dimension(n), intent(out) :: order

  real, dimension(n) :: tmp
  real :: s, maxv
  integer i,j,k,l, m, maxind, t

  ! rivien alkuperäinen järjestys
  do i=1,n
     order(i) = i
  end do

  ! ensimmäinen vaakarivi
  ! pivotointi, etsitään suurin alkio 1. sarakkeelta
  maxind = 1
  maxv = A(1,1)
  do l=2,n
     if (A(l, 1) > maxv) then
        maxv = A(l, 1) ; maxind = l
     end if
  end do
  ! vaihdetaan ko. rivi ensimmäiseksi
  tmp=A(maxind,:)
  A(maxind,:) = A(1,:)
  A(1,:)=tmp
  ! pidetään kirjaa vaihdettujen rivien järjestyksestä
  t = order(maxind)
  order(maxind) = 1
  order(1) = t

  do j=2,n 
     A(1,j)=A(1,j)/A(1,1)
  end do

  ! muut rivit
  do m=2,n
     ! ensin pystyrivi
     ! pivotointi, haetaan suurin luku alkion A(m,m) alapuolelta
     maxind = m
     maxv = A(m,m)
     do l=m+1,n
       if (A(l, m) > maxv) then
          maxv = A(l, m) ; maxind = l
       end if
     end do
     ! vaihdetaan vaakarivejä 
     tmp=A(maxind,:)
     A(maxind,:) = A(m,:)
     A(m,:)=tmp
     t = order(maxind)
     order(maxind) = m
     order(m) = t

     ! käsitellään A(m,m):n alapuolella oleva sarake
     do i=m,n
        s=0.0
        do k=1,m-1 
           s=s+A(i,k)*A(k,m)
        end do
        A(i,m)=A(i,m)-s
     end do

     ! sitten A(m,m):n oikealla puolella oleva vaakarivi
     do j=m+1,n
        s=0.0
        do k=1,m-1 
           s=s+A(m,k)*A(k,j)
        end do
        A(m,j)=(A(m,j)-s)/A(m,m)
     end do
   end do
end subroutine

subroutine solve (A, b, order, n, x)
! ratkaistaan yhtälö Ax=b
! A(n, n) sisältää kerroinmatriisin LU-hajotelman 
! b on yhtälön oikea puoli
! order on aliohjelman lu palauttama vektori, joka kertoo,
! mihin järjestykseen matriisin rivit on permutoitu
! ratkaisu palautetaan vektorissa x(n), ratkaisuvektorin järjestys
! vastaa alkuperäistä kerroinmatriisia

  integer, intent(in) :: n
  real, dimension(n,n), intent(in) :: A
  integer, dimension(n), intent(in) :: order
  real, dimension(n), intent(in) :: b
  real, dimension(n) :: x
  integer i,k
  real s    

  ! oikea puoli permutoitaan vastaamaan kerroinmatriisia
  do i=1,n
     x(order(i)) = b(i)
  end do

  x(1) = x(1)/A(1,1)

  do i=2,n
     s=0.0
     do k=1,i-1
        s=s+A(i,k)*x(k)
     end do
     x(i) = (x(i)-s) / A(i,i)
  end do  

  do i=n-1,1,-1
     s=x(i)
     do k=i+1,n
        s=s-A(i,k)*x(k)
     end do
     x(i)=s
  end do

end subroutine

function inverse (A, order, n)
! Lasketaan matriisin A(n,n) käänteismatriisi.
! Ennen funktion kutsua A on korvattava LU-hajotelmallaan.
! order on aliohjelman lu palauttama vektori, joka kertoo,
! mihin järjestykseen matriisin rivit on permutoitu

  integer, intent(in) :: n
  real, dimension(n,n), intent(in) :: A
  integer, dimension(n), intent(in) :: order
  real, dimension(n,n) :: inverse, C, S
  integer i

  C=0.0
  do i=1,n
     C(i,i)=1.0
  end do

  do i=1,n
     call solve(A, C(:,i), order, n, S(:,i))
  end do

  inverse = S

end function

function reorder (A, order, n)
! permutoidaan matriisin A(n,n) vaakarivit vektorin order
! mukaiseen järjestykseen

  real, dimension(n,n) :: reorder
  integer, intent(in) :: n
  real, dimension(n,n), intent(in) :: A
  integer, dimension(n), intent(in) :: order
  real, dimension(n,n) :: C
  integer i

  do i=1,n
     C(order(i),:)=A(i,:)
  end do

  reorder = C
end function

subroutine dump(A, n, s)
! tulostetaan matriisi A(n,n)
! merkkijono s on otsikkoteksti

  integer, intent(in) :: n
  real, intent(in), dimension(n,n) :: A
  character(len=*) :: s
  integer i

  write(*,'(/A)') s
  do i=1,n
     write(*,'(10F6.2)') A(i,:)
  end do

end subroutine

end module