--This is the maximum time in seconds before aborting a path query
--Increasing this will handle more complex scenarios
MAX_THINK_TIME = 1.0

thinkTime = nil
failed = nil

function init()	
	--Find handles to start and goal bodies
	startBody = FindBody("start")
	goalBody = FindBody("goal")
	
	--Table to hold all points of the last computed path
	lastPath = {}
end


--This function retrieves the most recent path and stores it in lastPath
function retrievePath()
	lastPath = {}
	local length=GetPathLength()
	local l=0
	while l < length do
		lastPath[#lastPath+1] = GetPathPoint(l)
		l = l + 0.2
	end
end


--This function will draw the content of lastPath as a line
function drawPath()
	for i=1, #lastPath-1 do
		DrawLine(lastPath[i], lastPath[i+1])
	end
end


function tick(dt)
	--Get the current position of start and goal bodies
	local startPos = GetBodyTransform(startBody).pos
	local goalPos = GetBodyTransform(goalBody).pos

	local recalc = false
	local state = GetPathState()
	if state == "idle" then
		--Path finding system is not in use. Start a new query.
		recalc = true
	elseif state == "done" then
		--Path finding system has completed a path query
		--Store the result and start a new query.
		recalc = true
		failed = false
		retrievePath()
	elseif state == "fail" then
		--Path finding system has failed to find a valid path
		--It is still possible to retrieve a result (path to closest point found)
		--Store the result and start a new query.
		recalc = true	
		failed = true
		retrievePath()
	else
		--Path finding system is currently busy. If it has been thinking for more than the 
		--allowed time, abort current query and store the best result
		thinkTime = thinkTime + dt
		if thinkTime > MAX_THINK_TIME then
			AbortPath()
			recalc = true
			failed = true
			retrievePath()
		end
	end

	if recalc then
		--Compute path from startPos to goalPos but exclude startBody and goalBody from the query.
		--Set the maximum path length to 100 and let any point within 0.5 meters to the goal point
		--count as a valid path.
		QueryRejectBody(startBody)
		QueryRejectBody(goalBody)
		QueryPath(startPos, goalPos, 100.0, 0.5)
		thinkTime = 0
	end

	--Draw last computed path and a red cross at the end of it in case it didn't reach the goal.
	drawPath()
	if failed then
		DebugCross(lastPath[#lastPath], 1, 0, 0)
	end
end