Small changes to algorithm

More changes to come so I'm keeping the same revision number for now.