Refactor. Fix bug in heuristic function.