module rk
	interface
		recursive subroutine rkdriver(xlist, ylist, b, acc, eps, h, nrk)
			real, intent(inout) :: xlist(:), ylist(:,:)
			real, intent(in) :: b, acc, eps
			real, intent(inout) :: h
			integer, intent(inout) :: nrk
		end subroutine
		
		subroutine rkstep(x, y, h, result, error)
			real, intent(in) :: x, y(:), h
			real, intent(out) :: result(size(y)), error
		end subroutine
	end interface
end module

subroutine rkstep(x, y, h, result, error)
	use matrix, only : norm
	implicit none
	interface
		function f(x,y)
			real, intent(in) :: x, y(:)
			real :: f(size(y))
		end function
	end interface
	real, intent(in) :: x, y(:), h
	real, intent(out) :: result(size(y)), error
	real, dimension(size(y)) :: k0, y12, k12

	k0 = f(x,y)
	y12 = y + k0*h/2
	k12 = f(x+h/2, y12)
	
	result = y + h*k12
	error = h*norm(k0 - k12)
end subroutine

recursive subroutine rkdriver(xlist, ylist, b, acc, eps, h, nrk)
	use rk, only : rkstep
	use matrix, only : norm
	implicit none
	interface
		function f(x,y)
			real, intent(in) :: x, y(:)
			real :: f(size(y))
		end function
	end interface
	real, intent(inout) :: xlist(:), ylist(:,:), h
	real, intent(in) :: b, acc, eps
	integer, intent(inout) :: nrk
	real, dimension(size(ylist,2)) :: y1
	real :: a, x, tol, err, newh
	integer :: N
	N = size(xlist)

	! Protect against overflow:
	if (nrk > N) then
		stop "Overflow: XLIST not large enough!"
	endif

	a = xlist(1)
	x = xlist(nrk)
	if (x >= b) then
	 	! We have reached the end.
		return
	end if
	if (x+h > b) h = b-x

	call rkstep(x, ylist(nrk,:), h, y1, err)
	tol = (norm(y1)*eps+acc)*sqrt(h/(b-a))
	
	! Calculate the new step:
	if (err > 0) then
		newh = 0.95*h*(tol/err)**0.25
	else
		newh = 2*h
	endif
	
	if (tol > err) then
		! Add the point to the solution:
		nrk = nrk + 1
		xlist(nrk) = x+h
		ylist(nrk,:) = y1
		call rkdriver(xlist,ylist,b,acc,eps,newh,nrk)
	else
		call rkdriver(xlist,ylist,b,acc,eps,newh,nrk)
	endif
end subroutine