Cleanup, change non-debug printf, minor changes

Moving call to printf into the DEBUG_MINIMAL statement can reduce code
size quite a bit on Arduino .
Cleanup debugging a bit