Revert previous commit for a faster implementation
Fix tests