implemented pathToNearestNUnexplored, simple unit test passes