Fix issues in MQTTv5 packet encoding and decoding.

Tests/MQTTnet.Core.Tests/MQTTv5/Client_Tests.cs

@@ -10,12 +10,47 @@ using MQTTnet.Client.Unsubscribing;
 using MQTTnet.Formatter;
 using MQTTnet.Protocol;
 using MQTTnet.Server;
+using MQTTnet.Tests.Mockups;
 
 namespace MQTTnet.Tests.MQTTv5
 {
     [TestClass]
     public class Client_Tests
     {
+        [TestMethod]
+        public async Task Connect_With_New_Mqtt_Features()
+        {
+            using (var testEnvironment = new TestEnvironment())
+            {
+                await testEnvironment.StartServerAsync();
+
+                // This test can be also executed against "broker.hivemq.com" to validate package format.
+                var client = await testEnvironment.ConnectClientAsync(
+                    new MqttClientOptionsBuilder()
+                        //.WithTcpServer("broker.hivemq.com")
+                        .WithTcpServer("127.0.0.1", testEnvironment.ServerPort)
+                        .WithProtocolVersion(MqttProtocolVersion.V500)
+                        .WithTopicAliasMaximum(20)
+                        .WithReceiveMaximum(20)
+                        .WithWillMessage(new MqttApplicationMessageBuilder().WithTopic("abc").Build())
+                        .WithWillDelayInterval(20)
+                        .Build());
+
+                await client.SubscribeAsync("a");
+
+                await client.PublishAsync(new MqttApplicationMessageBuilder()
+                    .WithTopic("a")
+                    .WithPayload("x")
+                    .WithUserProperty("a", "1")
+                    .WithUserProperty("b", "2")
+                    .WithPayloadFormatIndicator(MqttPayloadFormatIndicator.CharacterData)
+                    .WithAtLeastOnceQoS()
+                    .Build());
+
+                await Task.Delay(500);
+            }
+        }
+
         [TestMethod]
         public async Task Connect()
         {
@@ -63,7 +98,16 @@ namespace MQTTnet.Tests.MQTTv5
                 await server.StartAsync(new MqttServerOptions());
 
                 await client.ConnectAsync(new MqttClientOptionsBuilder().WithTcpServer("127.0.0.1").WithProtocolVersion(MqttProtocolVersion.V500).Build());
-                var result = await client.SubscribeAsync("a", MqttQualityOfServiceLevel.AtLeastOnce);
+
+                var result = await client.SubscribeAsync(new MqttClientSubscribeOptions()
+                {
+                    SubscriptionIdentifier = 1,
+                    TopicFilters = new List<TopicFilter>
+                    {
+                        new TopicFilter { Topic = "a", QualityOfServiceLevel = MqttQualityOfServiceLevel.AtLeastOnce}
+                    }
+                });
+
                 await client.DisconnectAsync();
 
                 Assert.AreEqual(1, result.Items.Count);

Tests/MQTTnet.Core.Tests/MQTTv5/Server_Tests.cs

@@ -0,0 +1,40 @@
+using System.Threading;
+using System.Threading.Tasks;
+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using MQTTnet.Client;
+using MQTTnet.Client.Options;
+using MQTTnet.Formatter;
+using MQTTnet.Tests.Mockups;
+
+namespace MQTTnet.Tests.MQTTv5
+{
+    [TestClass]
+    public class Server_Tests
+    {
+        [TestMethod]
+        public async Task Will_Message_Send()
+        {
+            using (var testEnvironment = new TestEnvironment())
+            {
+                var receivedMessagesCount = 0;
+
+                await testEnvironment.StartServerAsync();
+
+                var willMessage = new MqttApplicationMessageBuilder().WithTopic("My/last/will").WithAtMostOnceQoS().Build();
+
+                var clientOptions = new MqttClientOptionsBuilder().WithWillMessage(willMessage).WithProtocolVersion(MqttProtocolVersion.V500);
+
+                var c1 = await testEnvironment.ConnectClientAsync(new MqttClientOptionsBuilder().WithProtocolVersion(MqttProtocolVersion.V500));
+                c1.UseApplicationMessageReceivedHandler(c => Interlocked.Increment(ref receivedMessagesCount));
+                await c1.SubscribeAsync(new TopicFilterBuilder().WithTopic("#").Build());
+
+                var c2 = await testEnvironment.ConnectClientAsync(clientOptions);
+                c2.Dispose(); // Dispose will not send a DISCONNECT pattern first so the will message must be sent.
+
+                await Task.Delay(1000);
+
+                Assert.AreEqual(1, receivedMessagesCount);
+            }
+        }
+    }
+}

Tests/MQTTnet.Core.Tests/Mockups/TestEnvironment.cs

@@ -90,12 +90,24 @@ namespace MQTTnet.Tests.Mockups
 
         public async Task<IMqttClient> ConnectClientAsync(MqttClientOptionsBuilder options)
         {
+            if (options == null) throw new ArgumentNullException(nameof(options));
+
             var client = CreateClient();
             await client.ConnectAsync(options.WithTcpServer("localhost", ServerPort).Build());
 
             return client;
         }
 
+        public async Task<IMqttClient> ConnectClientAsync(IMqttClientOptions options)
+        {
+            if (options == null) throw new ArgumentNullException(nameof(options));
+
+            var client = CreateClient();
+            await client.ConnectAsync(options);
+
+            return client;
+        }
+
         public void ThrowIfLogErrors()
         {
             lock (_serverErrors)