Tweaked algorithm again + refactoring