minor improvements to heuristic function
moved testing code to the end of the file