WIP: Use new algorithm
FIX: Fix algorithm and tests
STY: Cosmit
ENH: Add tests from textbook