Refactor mqtt message sending and implement variable length enc/dec

Added more unit tests for mqtt