Add debug checks in algorithms

[matbesancon]

Add debug statements with @debug, make sure this is zero-cost when not in debug mode