module pc
	interface
		recursive subroutine pcdriver(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 pcstep(x, y, h, N, result, error)
			real, intent(in) :: x(:), y(:,:), h
			real, intent(out) :: result(size(y,2)), error
			integer, intent(in) :: N
		end subroutine
	end interface
end module

subroutine pcstep(x, y, h, N, 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,2)), error
	real, dimension(size(y,2)) :: k1, k2, c, d, yt, corr, y0, y1
	real :: x0, x1, x2
	integer, intent(in) :: N
	
	x0 = x(N-1)
	x1 = x(N)
	x2 = x1 + h
	y0 = y(N-1,:)
	y1 = y(N,:)

	! Predictor:
	k1 = f(x1, y1)
	c = (y0-y1-k1*(x0-x1))/(x0-x1)**2
	yt = y1 + h*k1 + c*h**2
	
	! Corrector:
	k2 = f(x2, yt)
	d = (k2 -k1 - 2*c*(x2-x1)) / (2*(x2-x1)*(x2-x0)+(x2-x1)**2)
	corr = d*(x2-x1)**2*(x2-x0)
	
	! Return:
	result = yt + corr
	error = norm(corr)
end subroutine

recursive subroutine pcdriver(xlist, ylist, b, acc, eps, h, nrk)
	use rk, only : rkstep
	use pc, only : pcstep
	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

	! So step and calculate tol:
	if (nrk == 1) then
		call rkstep(x, ylist(nrk,:), h, y1, err)
	else
		call pcstep(xlist, ylist, h, nrk, y1, err)
	endif
	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 pcdriver(xlist,ylist,b,acc,eps,newh,nrk)
	else
		call pcdriver(xlist,ylist,b,acc,eps,newh,nrk)
	endif
end subroutine