From ceb13384598274f0cfd389fdc670a14ddbdce3a2 Mon Sep 17 00:00:00 2001
From: SaxxonPike <saxxonpike@gmail.com>
Date: Thu, 4 Jul 2019 00:02:28 -0500
Subject: [PATCH] C64: Use proper sector gaps based on density when converting
 from D64.

---
 .../Computers/Commodore64/Media/D64.cs        | 12 ++-
 .../Computers/Commodore64/Media/G64.cs        | 75 +++++++++++++++++++
 2 files changed, 84 insertions(+), 3 deletions(-)

diff --git a/BizHawk.Emulation.Cores/Computers/Commodore64/Media/D64.cs b/BizHawk.Emulation.Cores/Computers/Commodore64/Media/D64.cs
index 87d0a60668..c84a505e5e 100644
--- a/BizHawk.Emulation.Cores/Computers/Commodore64/Media/D64.cs
+++ b/BizHawk.Emulation.Cores/Computers/Commodore64/Media/D64.cs
@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.IO;
+using System.Linq;
 
 namespace BizHawk.Emulation.Cores.Computers.Commodore64.Media
 {
@@ -63,6 +64,11 @@ namespace BizHawk.Emulation.Cores.Computers.Commodore64.Media
 			6250, 6666, 7142, 7692
 		};
 
+		private static readonly int[] StandardSectorGapLength =
+		{
+			9, 19, 13, 10
+		};
+
 		private static byte Checksum(byte[] source)
 		{
 			var count = source.Length;
@@ -76,7 +82,7 @@ namespace BizHawk.Emulation.Cores.Computers.Commodore64.Media
 			return result;
 		}
 
-		private static byte[] ConvertSectorToGcr(byte[] source, byte sectorNo, byte trackNo, byte formatA, byte formatB, out int bitsWritten)
+		private static byte[] ConvertSectorToGcr(byte[] source, byte sectorNo, byte trackNo, byte formatA, byte formatB, int gapLength, out int bitsWritten)
 		{
 			using (var mem = new MemoryStream())
 			{
@@ -96,7 +102,7 @@ namespace BizHawk.Emulation.Cores.Computers.Commodore64.Media
 				writer.Write(new byte[] { 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55 }); // gap
 				writer.Write(new byte[] { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }); // sync
 				writer.Write(EncodeGcr(writtenData)); // data
-				writer.Write(new byte[] { 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55 }); // gap
+				writer.Write(Enumerable.Repeat((byte)0x55, gapLength).ToArray()); // gap
 
 				bitsWritten = (int)mem.Length * 8;
 
@@ -185,7 +191,7 @@ namespace BizHawk.Emulation.Cores.Computers.Commodore64.Media
 						{
 							int bitsWritten;
 							var sectorData = reader.ReadBytes(256);
-							var diskData = ConvertSectorToGcr(sectorData, (byte)j, (byte)(i + 1), 0xA0, 0xA0, out bitsWritten);
+							var diskData = ConvertSectorToGcr(sectorData, (byte)j, (byte)(i + 1), 0xA0, 0xA0, StandardSectorGapLength[DensityTable[i]], out bitsWritten);
 							trackMem.Write(diskData, 0, diskData.Length);
 							trackLengthBits += bitsWritten;
 						}
diff --git a/BizHawk.Emulation.Cores/Computers/Commodore64/Media/G64.cs b/BizHawk.Emulation.Cores/Computers/Commodore64/Media/G64.cs
index fc54893e89..bceb344add 100644
--- a/BizHawk.Emulation.Cores/Computers/Commodore64/Media/G64.cs
+++ b/BizHawk.Emulation.Cores/Computers/Commodore64/Media/G64.cs
@@ -63,5 +63,80 @@ namespace BizHawk.Emulation.Cores.Computers.Commodore64.Media
 				return new Disk(84);
 			}
 		}
+		
+        public static byte[] Write(IList<byte[]> trackData, IList<int> trackNumbers, IList<int> trackDensities)
+        {
+            const byte version = 0;
+            const byte trackCount = 84;
+            const int headerLength = 0xC;
+            const byte dataFillerValue = 0xFF;
+
+            var trackMaxLength = (ushort)Math.Max(7928, trackData.Max(d => d.Length));
+
+            using (var mem = new MemoryStream())
+            {
+                var writer = new BinaryWriter(mem);
+
+                // header ID
+                writer.Write("GCR-1541".ToCharArray());
+
+                // version #
+                writer.Write(version);
+
+                // tracks in the image
+                writer.Write(trackCount);
+
+                // maximum track size in bytes
+                writer.Write(trackMaxLength);
+
+                // combine track data
+                var offsets = new List<int>();
+                var densities = new List<int>();
+                using (var trackMem = new MemoryStream())
+                {
+                    var trackMemWriter = new BinaryWriter(trackMem);
+                    for (var i = 0; i < trackCount; i++)
+                    {
+                        if (trackNumbers.Contains(i))
+                        {
+                            var trackIndex = trackNumbers.IndexOf(i);
+                            offsets.Add((int)trackMem.Length);
+                            densities.Add(trackDensities[trackIndex]);
+
+                            var data = trackData[trackIndex];
+                            var buffer = Enumerable.Repeat(dataFillerValue, trackMaxLength).ToArray();
+                            var dataBytes = data.Select(d => unchecked((byte)d)).ToArray();
+                            Array.Copy(dataBytes, buffer, dataBytes.Length);
+                            trackMemWriter.Write((ushort)dataBytes.Length);
+                            trackMemWriter.Write(buffer);
+                        }
+                        else
+                        {
+                            offsets.Add(-1);
+                            densities.Add(0);
+                        }
+                    }
+                    trackMemWriter.Flush();
+
+                    // offset table
+                    foreach (var offset in offsets.Select(o => o >= 0 ? o + headerLength + trackCount * 8 : 0))
+                    {
+                        writer.Write(offset);
+                    }
+
+                    // speed zone data
+                    foreach (var density in densities)
+                    {
+                        writer.Write(density);
+                    }
+
+                    // track data
+                    writer.Write(trackMem.ToArray());
+                }
+
+                writer.Flush();
+                return mem.ToArray();
+            }
+        }
 	}
 }