Path finding algorithm by Emily

Alex helped