Fixed algorithm and tests