      * |--------------------------------------
      */
 
-    private func mqttConnect(output: OutputStream, keepalive: UInt16) {
-        /**
-         * |----------------------------------------------------------------------------------
-         * |     7    |    6     |      5     |  4   3  |     2    |       1      |     0    |
-         * | username | password | willretain | willqos | willflag | cleansession | reserved |
-         * |----------------------------------------------------------------------------------
-         */
-
-        let clientId = options.concreteClientId
-
+    private func mqttConnect() {
+        // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718028
+
+        var packet = ControlPacket(type: .connect)
+        packet.payload += options.concreteClientId
+
+        // section 3.1.2.3
         var connectFlags: UInt8 = 0b00000010 // clean session
-
-        var remainingLength: Int = 10 // initial 10 bytes
-        remainingLength += (2 + clientId.utf8.count) // 2 byte client id length + codepoints
 
         if let username = options.username {
             connectFlags |= 0b10000000
-            remainingLength += (2 + username.utf8.count) // 2 byte username length + codepoints
+            packet.payload += username
+            
         }
 
         if let password = options.password {
             connectFlags |= 0b01000000
-            remainingLength += (2 + password.utf8.count) // 2 byte password length + codepoints
-        }
-
-        let remainingLengthBytes = encodeVariableLength(remainingLength)
-
-        let headerBytes: [UInt8] = [
-            0x10] +                             // FIXED BYTE 1   1 = CONNECT, 0 = DUP QoS RETAIN, not used in CONNECT
-            remainingLengthBytes +              // FIXED BYTE 2+  remaining length
-        [   0x00,                               // VARIA BYTE 1   length MSB
-            0x04,                               // VARIA BYTE 2   length LSB is 4
-            0x4d,                               // VARIA BYTE 3   M
-            0x51,                               // VARIA BYTE 4   Q
-            0x54,                               // VARIA BYTE 5   T
-            0x54,                               // VARIA BYTE 6   T
-            0x04,                               // VARIA BYTE 7   Version = 4
-            connectFlags,                       // VARIA BYTE 8   Username Password RETAIN QoS Will Clean flags
-            keepalive.highByte,                 // VARIA BYTE 9   Keep Alive MSB
-            keepalive.lowByte                   // VARIA BYTE 10  Keep Alive LSB
-        ]
-
-        var messageBytes = headerBytes + encode(string: clientId)
-
-        if let username = options.username {
-            messageBytes += encode(string: username)
-        }
-
-        if let password = options.password {
-            messageBytes += encode(string: password)
-        }
-
-        output.write(messageBytes, maxLength: messageBytes.count)
-    }
-
-    private func mqttDisconnect(output: OutputStream) {
-        let messageBytes: [UInt8] = [
-            0xe0,                               // FIXED BYTE 1   e = DISCONNECT, 0 = DUP QoS RETAIN (not used)
-            0x00                                // FIXED BYTE 2   remaining length = 0
-        ]
-
-        output.write(messageBytes, maxLength: messageBytes.count)
-    }
-
-    private func mqttSubscribe(output: OutputStream, to topic: String) {
-        messageId += 1
-
-        var remainingLength: Int = 3 // initial 3 bytes (messageID and QoS)
-        remainingLength += (2 + topic.utf8.count) // 2 byte topic length + codepoints
-
-        let remainingLengthBytes = encodeVariableLength(remainingLength)
-
-        let headerBytes: [UInt8] = [
-            0x82] +                             // FIXED BYTE 1   8 = SUBSCRIBE, 2 = DUP QoS RETAIN
-            remainingLengthBytes +              // FIXED BYTE 2+  remaining length
-        [   messageId.highByte,                 // VARIA BYTE 1   message id MSB
-            messageId.lowByte                   // VARIA BYTE 2   message id LSB
-        ]
-
-        let requestedQosByte: [UInt8] = [
-            0x00                                // Requested QoS
-        ]
-
-        let messageBytes = headerBytes + encode(string: topic) + requestedQosByte
-        output.write(messageBytes, maxLength: messageBytes.count)
-    }
-
-    private func mqttUnsubscribe(output: OutputStream, from topic: String) {
-        messageId += 1
-
-        var remainingLength: Int = 2 // initial 2 bytes (messageID)
-        remainingLength += (2 + topic.utf8.count) // 2 byte topic length + codepoints
-
-        let remainingLengthBytes = encodeVariableLength(remainingLength)
-
-        let headerBytes: [UInt8] = [
-            0xa2] +                             // FIXED BYTE 1   a = UNSUBSCRIBE, 2 = DUP QoS RETAIN
-            remainingLengthBytes +              // FIXED BYTE 2+  remaining length
-        [   messageId.highByte,                 // VARIA BYTE 1   message id MSB
-            messageId.lowByte                   // VARIA BYTE 2   message id LSB
-        ]
-
-        let messageBytes = headerBytes + encode(string: topic)
-        output.write(messageBytes, maxLength: messageBytes.count)
+            packet.payload += password
+        }
+
+        packet.variableHeader += "MQTT"            // section 3.1.2.1
+        packet.variableHeader += UInt8(0b00000100) // section 3.1.2.2
+        packet.variableHeader += connectFlags
+        packet.variableHeader += options.pingInterval // section 3.1.2.10
+
+        send(packet: packet)
+    }
+
+    private func mqttDisconnect() {
+        // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718090
+
+        let packet = ControlPacket(type: .disconnect)
+
+        send(packet: packet)
+    }
+
+    private func mqttSubscribe(to topic: String) {
+        // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718063
+
+        var packet = ControlPacket(type: .subscribe, flags: .leastOnce)
+        packet.variableHeader += nextMessageId()
+        packet.payload += topic    // section 3.8.3
+        packet.payload += UInt8(0) // QoS = 0
+
+        send(packet: packet)
+    }
+
+    private func mqttUnsubscribe(from topic: String) {
+        // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718072
+
+        var packet = ControlPacket(type: .unsubscribe, flags: .leastOnce)
+        packet.variableHeader += nextMessageId()
+        packet.payload += topic
+
+        send(packet: packet)
     }
 
     // MQTT publish only handles QOS 0 for now
-    private func mqttPublish(output: OutputStream, topic: String, message: Data) {
-        messageId += 1
-
+    private func mqttPublish(to topic: String, message: Data) {
+        // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718037
+
+        var packet = ControlPacket(type: .publish, flags: .mostOnce)
+        packet.variableHeader += topic // section 3.3.2
         // TODO: Add 2 (for messageId) if/when QOS > 0
-        let remainingLengthBytes = encodeVariableLength(2 + topic.utf8.count + message.count)
-
-        let headerBytes: [UInt8] = [
-            0x30] +                             // FIXED BYTE 1   3 = PUBLISH, 0 = DUP QoS RETAIN
-        remainingLengthBytes                    // remaining length, variable
-
-        let messageBytes = headerBytes + encode(string: topic) + [UInt8](message)
-        output.write(messageBytes, maxLength: messageBytes.count)
-    }
-
-    @objc private func mqttPing(output: OutputStream) {
-        let messageBytes: [UInt8] = [
-            0xc0,                               // FIXED BYTE 1   c = PINGREQ, 0 = DUP QoS RETAIN (not used)
-            0x00                                // FIXED BYTE 2   remaining length = 0
-        ]
+        packet.payload += message      // section 3.3.3
+
+        send(packet: packet)
+    }
+
+    @objc private func mqttPing() {
+        // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718081
+
+        let packet = ControlPacket(type: .pingreq)
         
-        output.write(messageBytes, maxLength: messageBytes.count)
-    }
-    
-    // MARK: - Utils
-    
-    private func encodeVariableLength(_ length: Int) -> [UInt8] {
-        var remainingBytes: [UInt8] = []
-        var workingLength = UInt(length)
+        send(packet: packet)
+    }
+
+    private func send(packet:ControlPacket) {
+        guard let output = outputStream else { return } 
+
+        let serialized = packet.encoded
+        var toSend = serialized.count
+        var sent = 0
+
+        writeQueue.sync {
+            while toSend > 0 {
+                let thisSend = serialized.withUnsafeBytes {
+                    output.write($0.advanced(by: sent), maxLength: toSend)
+                }
+                if thisSend < 0 {
+                    // print the output.error?
+                    return
+                }
+                toSend -= thisSend
+                sent += thisSend
+            }
+        }
+    }
+}
+
+
+fileprivate struct AppendableData {
+    var data = Data()
+
+    static func += (block: inout AppendableData, byte:UInt8) {
+        block.data.append(byte)
+    }
+
+    static func += (block: inout AppendableData, short:UInt16) {
+        block += UInt8(short >> 8)
+        block += UInt8(short & 0xFF)
+    }
+
+    static func += (block: inout AppendableData, data:Data) {
+        block.data += data
+    }
+
+    static func += (block: inout AppendableData, string:String) {
+        block += UInt16(string.utf8.count)
+        block += string.data(using: .utf8)!
+    }
+}
+
+
+fileprivate struct ControlPacket {
+    var type: PacketType
+    var flags: PacketFlags
+    var variableHeader = AppendableData()
+    var payload = AppendableData()
+
+    var remainingLength:Data {
+        // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718023
+        var workingLength = UInt(variableHeader.data.count + payload.data.count)
+        var encoded = Data()
         
-        while workingLength > 0 {
+        while true {
             var byte = UInt8(workingLength & 0x7F)
             workingLength >>= 7
             if workingLength > 0 {
                 byte |= 0x80
             }
-            remainingBytes.append(byte)
-        }
-        
-        return remainingBytes
-    }
-    
-    private func encode(string: String) -> [UInt8] {
-        // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718016
-        let encoded = string.utf8
-        return [UInt16(encoded.count).highByte, UInt16(encoded.count).lowByte] + [UInt8](encoded)
-    }
-    
+            encoded.append(byte)
+            if workingLength == 0 {
+                return encoded
+            }
+        }
+    } 
+
+    var encoded:Data {
+        var bytes = Data()
+        bytes.append(type.rawValue | flags.rawValue)
+        bytes += remainingLength
+        bytes += variableHeader.data
+        bytes += payload.data
+        return bytes
+    }
+
+    init(type: PacketType, flags: PacketFlags = .mostOnce) {
+        self.type = type
+        self.flags = flags
+    }
+
 }

                strongSelf.readStream(input: streams.input, output: streams.output)
            }

            strongSelf.mqttConnect(output: streams.output, keepalive: strongSelf.options.pingInterval)
            strongSelf.delayedPing(output: streams.output, interval: strongSelf.options.pingInterval)

            strongSelf.messageId = 0


    }

    func disconnect() {
        mqttDisconnect()
        closeStreams()
    }

    func subscribe(to topic: String) {
        mqttSubscribe(to: topic)
    }

    func unsubscribe(from topic: String) {
        mqttUnsubscribe(from: topic)
    }

    func publish(to topic: String, message: Data?) {
        mqttPublish(to: topic, message: message ?? Data())
    }

    // MARK: - Keep alive timer

    private func delayedPing() {
        let interval = options.pingInterval
        let time = DispatchTime.now() + Double(interval / 2)
        DispatchQueue.main.asyncAfter(deadline: time) { [weak self] in
            if let output = self?.outputStream, output.streamStatus == .open {
                // pass, ok to keep going
            }
            else {
                return // recursion stopper
            } 

            self?.mqttPing()
            self?.delayedPing()
        }
    }

