Simplify algorithm a bit