Removed bad smells from algorithm, small refactorings