Java: Testing and refactoring required. Both algorithms are implemented!