Fix algorithm bugs