Working algorithm for both heuristics