Heuristic function

Extracted Heuristic() function from main code