Fixed collision handling (for real)

Code suddenly disappeared and the earlier commit contained almost nothing, no idea what happened.
This should work better now.
Added DEBUG_MODE macro and general printf cleaning.