Fix wrong packet writer buffer length calculation.

Source/MQTTnet/Serializer/MqttPacketWriter.cs

@@ -89,6 +89,8 @@ namespace MQTTnet.Serializer
 
         public void Write(byte[] array, int offset, int count)
         {
+            if (array == null) throw new ArgumentNullException(nameof(array));
+
             EnsureAdditionalCapacity(count);
 
             Array.Copy(array, offset, _buffer, _position, count);
@@ -135,17 +137,18 @@ namespace MQTTnet.Serializer
                 return;
             }
 
-            EnsureCapacity(additionalCapacity - freeSpace);
+            EnsureCapacity(_buffer.Length + additionalCapacity - freeSpace);
         }
 
         private void EnsureCapacity(int capacity)
         {
-            if (_buffer.Length >= capacity)
+            var newBufferLength = _buffer.Length;
+
+            if (newBufferLength >= capacity)
             {
                 return;
             }
-
-            var newBufferLength = _buffer.Length;
+            
             while (newBufferLength < capacity)
             {
                 newBufferLength *= 2;
@@ -157,6 +160,7 @@ namespace MQTTnet.Serializer
         private void IncreasePostition(int length)
         {
             _position += length;
+
             if (_position > Length)
             {
                 Length = _position;

Tests/MQTTnet.Core.Tests/MqttPacketWriterTests.cs

@@ -0,0 +1,25 @@
+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using MQTTnet.Serializer;
+
+namespace MQTTnet.Core.Tests
+{
+    [TestClass]
+    public class MqttPacketWriterTests
+    {
+        [TestMethod]
+        public void WritePacket()
+        {
+            var writer = new MqttPacketWriter();
+            Assert.AreEqual(0, writer.Length);
+
+            writer.WriteWithLengthPrefix("1234567890");
+            Assert.AreEqual(10 + 2, writer.Length);
+
+            writer.WriteWithLengthPrefix(new byte[300]);
+            Assert.AreEqual(300 + 2 + 12, writer.Length);
+
+            writer.WriteWithLengthPrefix(new byte[5000]);
+            Assert.AreEqual(5000 + 2 + 300 + 2 + 12, writer.Length);
+        }
+    }
+}