Some optimization/refactoring and little work on the "algorithm control"