Partially working new heuristic function