First version of path finding algorithm