PathFinding logic improvements