Added diagonal heuristic and made heuristic passable