diff --git a/include/link_handler.h b/include/link_handler.h
index 90742d5..4ea9096 100644
--- a/include/link_handler.h
+++ b/include/link_handler.h
@@ -281,7 +281,7 @@ struct LinkPacket
     PayloadCommand command = CMD_NONE;
     byte argument[2] = {0x00, 0x00};
     u16 pointer = 0;
-    byte packetID;
+    byte packetID = 0;
 
     // Incoming data
     LinkConnectionError latestError = NO_ERROR;
diff --git a/source/link_handler.cpp b/source/link_handler.cpp
index 108c327..35c5825 100644
--- a/source/link_handler.cpp
+++ b/source/link_handler.cpp
@@ -539,12 +539,7 @@ void LinkConnection::handleStateLogic()
       break;
     }
 
-    if (allPacketsProcessed())
-    {
-      exitState = PRINT_LAST_PACKET;
-    }
-
-    if (subStateCounter % TOTAL_PACKET_LENGTH == 0)
+    if (subStateCounter % TOTAL_PACKET_LENGTH == 0 && subStateCounter != 0)
     {
       newPacket = true;
       processPacket();
@@ -554,6 +549,11 @@ void LinkConnection::handleStateLogic()
       newPacket = false;
     }
 
+    if (allPacketsProcessed())
+    {
+      exitState = PRINT_LAST_PACKET;
+    }
+
     dataOutBuffer[subStateCounter % TOTAL_PACKET_LENGTH] = inData;
   }
   break;
@@ -764,7 +764,13 @@ void LinkConnection::loadNextPacket()
     currPacket.latestError = PACKET_READ;
     if (((currPacket.command == CMD_ReadDataRequest) && (currPacket.pointer - linkPacketDataStart) < linkPacketDataSize))
     {
-      memcpy(&outDataArrayPtr[currPacket.pointer - linkPacketDataStart], &currPacket.recievedData[0], 8);
+      for (int i = 0; i < 8; i++)
+      {
+        if (((currPacket.pointer - linkPacketDataStart) + i) < linkPacketDataSize)
+        {
+          outDataArrayPtr[(currPacket.pointer - linkPacketDataStart) + i] = currPacket.recievedData[i];
+        }
+      }
       currPacket = LinkPacket(CMD_ReadDataRequest, 0x00, 0x00, linkPacketDataAddr);
       linkPacketDataAddr += 8;
     }
@@ -798,6 +804,18 @@ void LinkConnection::resetLinkPackets()
   {
     linkPacketArr[i] = LinkPacket();
   }
+
+  // Loop the packet index around back to 0
+  while (linkPacketArrIndex % LINK_PACKET_ARRAY_SIZE != 0)
+  {
+    linkPacketArrIndex++;
+  }
+
+  softResetActivated = false;
+  for (int i = 0; i < 16; i++)
+  {
+    dataOutBuffer[i] = 0;
+  }
 }
 
 void LinkConnection::waitForEnd()