Improved algorithm for, should solve #16