Completed the path-finding algorythm