Adjusted heuristic cost count but it needs more optimization as it doesn't count our cost fully correctly yet.