Some fixes and code refactoring

Core/Video/SmsVdp.cs

@@ -40,7 +40,7 @@ namespace Core.Video
             _isSecondControlByte = false; // Reading data resets the control latch
             byte value = _readBuffer;
             _readBuffer = VRAM[_controlWord & 0x3FFF];
-            _controlWord++;
+            IncrementVdpAddress();
             return value;
         }
 
@@ -82,7 +82,7 @@ namespace Core.Video
             }
 
             // THE FIX: The pointer MUST auto-increment so the CPU can blast data fast!
-            _controlWord++;
+            IncrementVdpAddress();
         }
 
         public void WriteControlPort(byte value) // Port 0xBF
@@ -104,7 +104,7 @@ namespace Core.Video
                 if (command == 0) // Code 0: Prep for VRAM Read
                 {
                     _readBuffer = VRAM[_controlWord & 0x3FFF];
-                    _controlWord++;
+                    IncrementVdpAddress();
                 }
                 else if (command == 2) // Code 2: Write to Internal VDP Register
                 {
@@ -116,6 +116,18 @@ namespace Core.Video
             }
         }
 
+        private void IncrementVdpAddress()
+        {
+            // The VDP address register is only 14 bits
+            // When it increments past 0x3FFF, it rolls over to 0x0000.
+            // It can't overflow and corrupt the 2-bit command register (bits 14 and 15).
+            ushort address = (ushort)(_controlWord & 0x3FFF);
+            ushort command = (ushort)(_controlWord & 0xC000);
+
+            address = (ushort)((address + 1) & 0x3FFF);
+            _controlWord = (ushort)(command | address);
+        }
+
         public byte ReadVCounter()
         {
              // NTSC Math: 262 lines. Counts 0 to 218, jumps to 213 (0xD5), counts to 255.