Fixed Chebyshev calculation and added Diagonal heuristic