Some fixes on the algorithm