Updated heuristic function to work for larger grids.