Made the heuristic avoid hitting walls