Fix to algorithm