Greatly optimizing algorithm and refactoring for readability