add diagonal heuristic