!*****************************************
!*  Rasmus Handberg
!*  Department of Physics and Astronomy
!*  University of Aarhus
!*****************************************

module matrix
	interface
		function backsub(T, c)
			implicit none
			real, intent(in) :: T(:,:)
			real, intent(in) :: c(:)
			real :: backsub(size(c))
		end function backsub
		
		subroutine qrdec(matrix, Q, R)
			implicit none
			real, dimension(:,:) :: matrix
			real, dimension(size(matrix,1),size(matrix,2)) :: Q
			real, dimension(size(matrix,2),size(matrix,2)) :: R
		end subroutine qrdec
		
		real function determinant(R)
			implicit none
			real, dimension(:,:) :: R
		end function determinant
		
		function lineqnsystem(Q, R, b)
			implicit none
			real, dimension(:,:) :: Q
			real, dimension(:,:) :: R
			real, dimension(:) :: b
			real, dimension(size(b)) :: lineqnsystem
		end function lineqnsystem
		
		function inverse(Q, R)
			implicit none
			real, dimension(:,:) :: Q
			real, dimension(:,:) :: R
			real, dimension(size(Q,1),size(Q,2)) :: inverse
		end function inverse
		
		function unit(n)
			implicit none
			integer, intent(in) :: n
			real :: unit(n,n)
		end function unit
		
		subroutine MatOut(streng, A)
			implicit none
			character(len=*) :: streng
			real, dimension(:,:) :: A
		end subroutine MatOut
		
		subroutine VecOut(streng, A)
			implicit none
			character(len=*) :: streng
			real, dimension(:) :: A
		end subroutine VecOut
	end interface
end module matrix


! Back-substitution.
function backsub(T, c)
	implicit none
	real, intent(in) :: T(:,:)
	real, intent(in) :: c(:)
	real :: backsub(size(c))
	integer :: i, n
	
	n = size(c)
	do i = n,1,-1
		backsub(i) = (c(i) - dot_product(T(i,i+1:n),backsub(i+1:n)))/T(i,i)
	enddo
end function backsub

! QR-decomposition.
! Input:     matrix - n*m-matrix to be decomposed.
!            Q      - n*m-matrix
!            R      - m*m-matrix
subroutine qrdec(matrix, Q, R)
	implicit none
	real, intent(in) :: matrix(:,:)
	real, intent(out) :: Q(size(matrix,1),size(matrix,2))
	real, intent(out) :: R(size(matrix,2),size(matrix,2))
	real :: A(size(matrix,1),size(matrix,2))
	integer :: i, j, n, m
	
	n = size(matrix,1)
	m = size(matrix,2)
	A = matrix
	Q = 0.0
	R = 0.0
	do i = 1,m
		R(i,i) = sqrt(dot_product(A(:,i),A(:,i)))
		Q(:,i) = A(:,i)/R(i,i)
		do j = i+1,m
			R(i,j) = dot_product(Q(:,i),A(:,j))
			A(:,j) = A(:,j) - Q(:,i)*R(i,j)
		enddo
	enddo
end subroutine qrdec

! Calcultaes the determinant, given the R-matrix
! from the QR-decomposition.
real function determinant(R)
	implicit none
	real, intent(in) :: R(:,:)
	integer :: i
	
	determinant = 1.0
	do i = 1,size(R,1)
		determinant = determinant*R(i,i)
	enddo
end function determinant

! Solve linear equation system Ax=b, given the
! Q and R-matrices from the QR-decomposition and
! the vector b.
function lineqnsystem(Q, R, b)
	use matrix, only : backsub
	implicit none
	real, intent(in) :: Q(:,:)
	real, intent(in) ::	R(:,:)
	real, intent(in) :: b(:)
	real :: lineqnsystem(size(b))
	real :: c(size(b))

	c = matmul(transpose(Q), b)
	lineqnsystem = backsub(R, c)
end function lineqnsystem


! Calculate the inverse:
function inverse(Q, R)
	use matrix, only : lineqnsystem
	implicit none
	real, intent(in) :: Q(:,:)
	real, intent(in) :: R(:,:)
	real :: inverse(size(Q,1),size(Q,2))
	real :: z(size(Q,1))
	integer :: i

	inverse = 0.0
	do i = 1,size(Q,1)
		z = 0.0
		z(i) = 1.0
		inverse(:,i) = lineqnsystem(Q, R, z)
	enddo
end function inverse

! Create the n*n unit-matrix.
function unit(n)
	implicit none
	integer, intent(in) :: n
	real :: unit(n,n)
	integer :: i
	
	unit = 0.0
	do i = 1,n
		unit(i,i) = 1.0
	enddo
end function unit

! Routine for writing matrix to the screen.
! Input:     streng - text to be placed above matrix.
!            A      - matrix to print.
subroutine MatOut(streng, A)
	implicit none
	character(len=*), intent(in) :: streng
	real, intent(in) :: A(:,:)
	integer :: i, n, m
	
	n = size(A,1)
	m = size(A,2)
	
	print '(1x,a)', streng(1:len_trim(streng))
	do i = 1,n
		print '(10f10.3)', A(i,1:m)
	enddo
end subroutine MatOut

! Routine for writing vector to the screen:
! Input:     streng - text to be placed above vector.
!            A      - vector to print.
subroutine VecOut(streng, A)
	implicit none
	character(len=*), intent(in) :: streng
	real, intent(in) :: A(:)
	integer :: i, n
	n = size(A)
	
	print '(1x,a)', streng(1:len_trim(streng))
	do i = 1,n
		print '(f10.3)', A(i)
	enddo
end subroutine VecOut