Remove debug code
Simplify algorithm