Refactored solution
FIxed distance calculation of heuristic function