refactoring and big improovement of the worst case of the algorithm