Refactor Node comparison
Always get node first from queue if it is a solution