Based on work from Suhas Daftar.
Depends on D531
make check
Nit: Is this debug output that you forgot to remove?
Yes :)
Remove use of std::cout