[MOD] Added DEBUG macro + minor fixes
[ADD] Added MQTTSN_DEBUG