Fixed roundRobin heuristic.
Added a Hashset to reduce time in prioityQueue(frontier).contains(),
this adds some Memory overhead but it reduce runtime significant.