Heuristic cost function added