! Matrix-diagonalisation
! Rasmus Handberg
! Department of Physics and Astronomy
! University of Aarhus

	module jacobi
		interface
			subroutine Cyclic(A, acc, rot_done)
				implicit none
				real, intent(inout) :: A(:,:)
				real, intent(in) :: acc
				integer, optional, intent(out) :: rot_done
			end subroutine Cyclic

			subroutine Classical(A, acc, rot_done)
				implicit none
				real, intent(inout) :: A(:,:)
				real, intent(in) :: acc
				integer, optional, intent(out) :: rot_done
			end subroutine Classical

			subroutine JacobiRot(A, p, q)
				implicit none
				real, intent(inout) :: A(:,:)
				integer, intent(in) :: p, q
			end subroutine JacobiRot
		end interface
	end module jacobi

	! Cyclic routine for Jacobi
	! Input: 	A        - The matrix to make diagonal. Will be overwritten.
	!			acc      - The desired accuracy.
	!			rot_done - Optional. Returns the number of rotations performed.
	subroutine Cyclic(A, acc, rot_done)
		use Jacobi, only : JacobiRot
		implicit none
		real, intent(inout) :: A(:,:)
		real, intent(in) :: acc
		integer, optional, intent(out) :: rot_done
		logical :: rotated
		integer :: i, j, n, max_rot, rotations
		
		n = size(A,1)
		max_rot = 5*n**2
		rotations = 0
		do
			rotated = .false.
			do i = 1,n
				do j = i+1,n
					if (abs(A(i,j)) > acc) then
						rotated = .true.
						call JacobiRot(A, i, j)
						rotations = rotations+1
						if (rotations>=max_rot) stop "Maximum rotation reached."
					endif
				enddo
			enddo
			if (.not. rotated) exit
		enddo
		! Return the number of rotations done:
		if (present(rot_done)) rot_done = rotations
	end subroutine Cyclic

	! Classical routine for Jacobi
	! Input: 	A        - The matrix to make diagonal. Will be overwritten.
	!			acc      - The desired accuracy.
	!			rot_done - Optional. Returns the number of rotations performed.
	subroutine Classical(A, acc, rot_done)
		use Jacobi, only : JacobiRot
		implicit none
		real, intent(inout) :: A(:,:)
		real, intent(in) :: acc
		integer, optional, intent(out) :: rot_done
		integer :: n, i, j, max_rot, rotations
		integer, dimension(2) :: indx
		real :: val, Amax

		n = size(A,1)
		max_rot = 5*n**2
		rotations = 0
		do
			! Find the largest off-diagonal-element:
			Amax = 0.0
			do i = 1,n
				do j = i+1,n ! j=1,n
					!if (i/=j) then
					val = abs(A(i,j))
					if (val > Amax) then
						Amax = val
						indx(1) = i
						indx(2) = j
					endif
					!endif
				enddo
			enddo

			if (Amax > acc) then
				call JacobiRot(A, indx(1), indx(2))
				rotations = rotations+1
				if (rotations>=max_rot) stop "Maximum rotation reached."
			else
				exit
			endif
		enddo
		! Return the number of rotations done:
		if (present(rot_done)) rot_done = rotations
	end subroutine Classical

	! Routine for doing Jacobi-rotation.
	! Matrix A is destroyed.
	subroutine JacobiRot(A, p, q)
		implicit none
		real, intent(inout) :: A(:,:)
		integer :: p, q
		integer :: i, n, tmp
		real :: phi, s, c
		real :: App, Aqq, Apq, Api, Aqi
		n = size(A,1)

		if (q < p) then
			tmp = p
			p = q
			q = tmp
		endif

		! Old matrix-elements:
		App = A(p,p)
		Aqq = A(q,q)
		Apq = A(p,q)

		! Calcultata phi, sin and cos:
		phi = 0.5*atan2(2*Apq, Aqq-App)
		s = sin(phi)
		c = cos(phi)

		! Do the rotation:
		A(p,p) = c**2*App - 2*s*c*Apq + s**2*Aqq
		A(q,q) = s**2*App + 2*s*c*Apq + c**2*Aqq
		A(p,q) = s*c*(App-Aqq) + (c**2-s**2)*Apq
		A(q,p) = A(p,q)
		do i = 1,n
			if (i/=p .and. i/=q) then
				Api = A(p,i)
				Aqi = A(q,i)
				A(p,i) = c*Api - s*Aqi
				A(i,p) = A(p,i)
				A(q,i) = s*Api + c*Aqi
				A(i,q) = A(q,i)
			endif
		enddo

		! Calculate eigenvectors:
		!real, optional, intent(inout) :: V(:,:)
		!if (present(V)) then
		!	do i = 1,n
		!		Vip = V(p,i)
		!		Viq = V(q,i)
		!		V(p,i) = c*Vip-s*Viq;
		!		V(q,i) = c*Viq+s*Vip;
		!	enddo
		!endif
	end subroutine