FIX: Fix algorithm and tests