Improved algorithm, added tests