Pathfinding updated with possible range function (unable to test at the moment)