//	Altirra - Atari 800/800XL/5200 emulator
//	Copyright (C) 2009-2016 Avery Lee
//
//	This program is free software; you can redistribute it and/or modify
//	it under the terms of the GNU General Public License as published by
//	the Free Software Foundation; either version 2 of the License, or
//	(at your option) any later version.
//
//	This program is distributed in the hope that it will be useful,
//	but WITHOUT ANY WARRANTY; without even the implied warranty of
//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//	GNU General Public License for more details.
//
//	You should have received a copy of the GNU General Public License
//	along with this program; if not, write to the Free Software
//	Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

#include <stdafx.h>
#include <vd2/system/binary.h>
#include <vd2/system/math.h>
#include <at/atcore/logging.h>
#include <at/atcore/scheduler.h>
#include "disk.h"
#include "fdc.h"

extern ATLogChannel g_ATLCDisk;
ATLogChannel g_ATLCFDC(true, false, "FDC", "Floppy drive controller");
ATLogChannel g_ATLCFDCCommand(false, false, "FDCCMD", "Floppy drive controller commands");

ATFDCEmulator::ATFDCEmulator() {
	mpFnDrqChange = [](bool drq) {};
	mpFnIrqChange = [](bool drq) {};
}

ATFDCEmulator::~ATFDCEmulator() {
	Shutdown();
}

void ATFDCEmulator::Init(ATScheduler *sch, bool scheduler1MHz) {
	mpScheduler = sch;
	mbScheduler1MHz = scheduler1MHz;
	mCyclesPerRotation = scheduler1MHz ? kCyclesPerRotation1MHz : kCyclesPerRotation500KHz;
}

void ATFDCEmulator::Shutdown() {
	if (mpScheduler) {
		mpScheduler->UnsetEvent(mpStateEvent);
		mpScheduler = nullptr;
	}
}

void ATFDCEmulator::Reset() {
	AbortCommand();

	mState = kState_Idle;
	mRegCommand = 0xD8;		// force interrupt immediate
	mRegTrack = 0;
	mRegSector = 0;
	mRegStatus = 0;
	mRegData = 0;
	mbIrqPending = false;
	mbDataReadPending = false;
	mbRegStatusTypeI = true;

	mpFnDrqChange(false);

	mWeakBitLFSR = 1;

	mpScheduler->UnsetEvent(mpStateEvent);

	mpDiskInterface->SetShowMotorActive(true);
	mpDiskInterface->SetShowActivity(false, 0);
}

void ATFDCEmulator::SetAccurateTimingEnabled(bool enabled) {
	mbUseAccurateTiming = enabled;
}

void ATFDCEmulator::SetMotorRunning(bool running) {
	if (mbMotorRunning == running)
		return;

	UpdateRotationalPosition();
	mbMotorRunning = running;
}

void ATFDCEmulator::SetCurrentTrack(uint32 track, bool track0) {
	if (mPhysTrack != track) {
		mPhysTrack = track;

		g_ATLCFDC("Physical track is now %u\n", track);
	}

	mbTrack0 = track0;
}

void ATFDCEmulator::SetDensity(bool mfm) {
	if (mbMFM != mfm) {
		mbMFM = mfm;

		g_ATLCFDC("Density encoding now set to %s\n", mfm ? "MFM" : "FM");
	}
}

void ATFDCEmulator::SetDiskImage(IATDiskImage *image, bool diskReady) {
	mpDiskImage = image;
	mbDiskReady = diskReady;

	if (image)
		mDiskGeometry = image->GetGeometry();
	else
		mDiskGeometry = {};
}

void ATFDCEmulator::SetDiskInterface(ATDiskInterface *diskIf) {
	mpDiskInterface = diskIf;
}

uint8 ATFDCEmulator::DebugReadByte(uint8 address) const {
	switch(address & 3) {
		case 0:
		default:
			// If the last command was a type I status, read constantly updated status.
			if (mbRegStatusTypeI) {
				uint8 v = mRegStatus & 0x3D;

				// update not ready
				if (!mbDiskReady)
					v |= 0x80;

				// update write protect
				if (mpDiskImage && !mpDiskInterface->IsDiskWritable())
					v |= 0x40;

				// update track 0
				if (!mbTrack0)
					v |= 0x04;

				// update index
				if (mbIndexPulse)
					v |= 0x02;

				return v;
			}

			return mRegStatus;

		case 1:
			return mRegTrack;

		case 2:
			return mRegSector;

		case 3:
			return mRegData;
	}
}

uint8 ATFDCEmulator::ReadByte(uint8 address) {
	uint8 v = DebugReadByte(address);

	switch(address & 3) {
		case 0:
			// reading status clears IRQ request
			if (mbIrqPending) {
				mbIrqPending = false;

				mpFnIrqChange(false);
			}

			break;

		case 3:
			if (mbDataReadPending) {
				mbDataReadPending = false;

				mpFnDrqChange(false);
			}

			mRegStatus &= ~0x02;
			break;
	}

	return v;
}

void ATFDCEmulator::WriteByte(uint8 address, uint8 value) {
	switch(address & 3) {
		case 0:
		default:
			// check if the controller is busy
			if (mState != kState_Idle) {
				// yes -- only force interrupt is accepted
				if ((value & 0xF0) != 0xD0)
					break;

				// abort existing command
				AbortCommand();

				// clear BUSY bit; leave all others
				mRegStatus &= 0xFE;

				// set IRQ
				//
				// Force Interrupt with bits 0-3 all clear is a special case -- it is
				// documented as not setting the interrupt.
				if (!mbIrqPending && mRegCommand != 0xD0) {
					mbIrqPending = true;
					mpFnIrqChange(true);
				}

				// clear activity indicator
				mpDiskInterface->SetShowActivity(false, 0);
			} else {
				// deassert IRQ
				if (mbIrqPending) {
					mbIrqPending = false;
					mpFnIrqChange(false);
				}

				mRegCommand = value;
				SetTransition(kState_BeginCommand, 1);
			}
			break;

		case 1:
			mRegTrack = value;
			break;

		case 2:
			mRegSector = value;
			break;

		case 3:
			mRegData = value;
			if (mbDataWritePending) {
				mbDataWritePending = false;

				mpFnDrqChange(false);

				mRegStatus &= 0xFD;
			}
			break;
	}
}

void ATFDCEmulator::OnIndexPulse(bool asserted) {
	if (mbIndexPulse == asserted)
		return;

	mbIndexPulse = asserted;

	if (!asserted)
		return;

	++mActiveOpIndexMarks;

	switch(mState) {
		case kState_ReadSector_TransferFirstByte:
		case kState_ReadSector_TransferFirstByteNever:
			if (mActiveOpIndexMarks >= 5) {
				g_ATLCFDC("Timing out read sector/address command -- sector not found after 5 revs\n");
				mActiveSectorStatus = 0x10;
				SetTransition(kState_ReadSector_TransferComplete, 1);
			}
			break;

		case kState_WriteTrack_WaitIndexMarks:
			if (!mbDataWritePending) {
				mpDiskInterface->SetShowActivity(true, mPhysTrack);
				SetTransition(kState_WriteTrack_TransferByte, 1);
			} else if (mActiveOpIndexMarks >= 2)
				SetTransition(kState_WriteTrack_InitialDrqTimeout, 1);
			break;

		case kState_WriteTrack_TransferByte:
			SetTransition(kState_WriteTrack_Complete, 1);
			break;
	}
}

void ATFDCEmulator::OnScheduledEvent(uint32 id) {
	mpStateEvent = nullptr;

	RunStateMachine();
}

void ATFDCEmulator::AbortCommand() {
	// Firmware may just abort a write track command rather than waiting for the second
	// index mark; we still have to handle the format command.
	if (!mWriteTrackBuffer.empty()) {
		FinalizeWriteTrack();
		mWriteTrackBuffer.clear();
	}

	mState = kState_Idle;
	mpScheduler->UnsetEvent(mpStateEvent);

	mbDataReadPending = false;
	mbDataWritePending = false;
	mpFnDrqChange(false);
}

void ATFDCEmulator::RunStateMachine() {
	static constexpr uint32 kStepDelayTable[2][4] = {
		{
			(uint32)(0.5 +  6.0 / 1000.0 * 500000.0),	// 6ms
			(uint32)(0.5 + 12.0 / 1000.0 * 500000.0),	// 12ms
			(uint32)(0.5 + 20.0 / 1000.0 * 500000.0),	// 20ms
			(uint32)(0.5 + 30.0 / 1000.0 * 500000.0),	// 30ms
		},
		{
			(uint32)(0.5 +  6.0 / 1000.0 * 1000000.0),	// 6ms
			(uint32)(0.5 + 12.0 / 1000.0 * 1000000.0),	// 12ms
			(uint32)(0.5 + 20.0 / 1000.0 * 1000000.0),	// 20ms
			(uint32)(0.5 + 30.0 / 1000.0 * 1000000.0),	// 30ms
		}
	};

	switch(mState) {
		case kState_Idle:
			break;

		case kState_BeginCommand:
			g_ATLCFDCCommand("Beginning command $%02X - track %u, sector %u (on track %u, %s)\n", mRegCommand, mRegTrack, mRegSector, mPhysTrack, mbMFM ? "MFM" : "FM");

			mRegStatus &= 0x80;
			mRegStatus |= 0x01;

			mActiveOpIndexMarks = 0;

			switch(mRegCommand & 0xF0) {
				case 0x00: SetTransition(kState_Restore, 1); break;
				case 0x10: SetTransition(kState_Seek, 1); break;
				case 0x20:
				case 0x30: SetTransition(kState_Step, 1); break;
				case 0x40:
				case 0x50: SetTransition(kState_StepIn, 1); break;
				case 0x60:
				case 0x70: SetTransition(kState_StepOut, 1); break;
				case 0x80:
				case 0x90: SetTransition(kState_ReadSector, 1); break;
				case 0xA0:
				case 0xB0: SetTransition(kState_WriteSector, 1); break;
				case 0xC0: SetTransition(kState_ReadAddress, 1); break;
				case 0xD0: SetTransition(kState_ForceInterrupt, 1); break;
				case 0xE0: SetTransition(kState_ReadTrack, 1); break;
				case 0xF0: SetTransition(kState_WriteTrack, 1); break;
			}
			break;

		case kState_EndTypeICommand:
end_type_I:
			// begin continuous update of type I status
			mbRegStatusTypeI = true;

			if (false) {
				// fall through

		case kState_EndNonTypeICommand:
				mbRegStatusTypeI = false;
			}

			mRegStatus &= 0x7C;		// clear BUSY, DRQ, and NOT READY

			if (!mbDiskReady)
				mRegStatus |= 0x80;

			g_ATLCFDCCommand("Ending command $%02X\n", mRegCommand);

			mWriteTrackBuffer.clear();

			mState = kState_Idle;

			if (mbDataReadPending || mbDataWritePending) {
				mbDataReadPending = false;
				mbDataWritePending = false;

				mpFnDrqChange(false);
			}

			// Set INTRQ.
			//
			// Force Interrupt with bits 0-3 all clear is a special case -- it is
			// documented as not setting the interrupt.
			if (!mbIrqPending && mRegCommand != 0xD0) {
				mbIrqPending = true;
				mpFnIrqChange(true);
			}

			mpDiskInterface->SetShowActivity(false, 0);
			break;

		case kState_Restore:
			mOpCount = 256;

			SetTransition(kState_Restore_Step, 1);
			break;

		case kState_Restore_Step:
			if (mbTrack0) {
				SetTransition(kState_EndTypeICommand, 1);
			} else if (--mOpCount > 0) {
				SetTransition(kState_Restore_Step, kStepDelayTable[mbScheduler1MHz][mRegCommand & 3]);
			} else {
				// set seek error status bit
				mRegStatus |= 0x10;

				SetTransition(kState_EndTypeICommand, 1);
			}
			break;

		case kState_StepIn:
			++mRegTrack;
			SetTransition(kState_EndTypeICommand, kStepDelayTable[mbScheduler1MHz][mRegCommand & 3]);
			break;
		case kState_StepOut:
			--mRegTrack;
			SetTransition(kState_EndTypeICommand, kStepDelayTable[mbScheduler1MHz][mRegCommand & 3]);
			break;

		case kState_Seek:
		case kState_Step:
			SetTransition(kState_EndTypeICommand, 1);
			break;

		case kState_ReadTrack:
			g_ATLCFDC <<= "Unsupported Read Track command issued\n";
			SetTransition(kState_EndTypeICommand, 1);
			break;

		case kState_WriteTrack:
			// check for write protect or format prohibited
			if (mpDiskImage && !mpDiskInterface->IsFormatAllowed()) {
				mRegStatus = 0x41;
				SetTransition(kState_EndNonTypeICommand, 1);
				break;
			}

			// set DRQ immediately -- 810 relies on being able to write first byte without
			// waiting for head load or first index mark, since it has the RIOT timer IRQ
			// disabled
			if (!mbDataWritePending) {
				mbDataWritePending = true;
				mRegStatus |= 0x02;
				mpFnDrqChange(true);
			}

			mWriteTrackBuffer.resize(kMaxBytesPerTrack, 0);
			mWriteTrackIndex = 0;

			// wait 20ms for head load
			SetTransition(kState_WriteTrack_WaitHeadLoad, 10000);
			break;

		case kState_WriteTrack_WaitHeadLoad:
			// wait for next index mark
			mActiveOpIndexMarks = 0;
			mState = kState_WriteTrack_WaitIndexMarks;
			break;

		case kState_WriteTrack_WaitIndexMarks:
			break;

		case kState_WriteTrack_TransferByte:
			// Check for lost data; WD docs say that $00 is written if data lost.
			if (mbDataWritePending) {
				mRegStatus |= 0x04;
				mRegData = 0;
				SetTransition(kState_WriteTrack_TransferByte, kCyclesPerByte);
			} else {
				g_ATLCFDC("Write track: $%02X\n", mRegData);

				// Check the byte that got written for write timing:
				//
				//	$F7 - two CRC bytes
				//	$F8-FB - data address mark (DAM)
				//	$FC - index address mark
				//	$FE - ID address mark (IDAM)

				if (mRegData == 0xF7)
					SetTransition(kState_WriteTrack_TransferByte, kCyclesPerByte * 2);
				else
					SetTransition(kState_WriteTrack_TransferByte, kCyclesPerByte);

				mbDataWritePending = true;
				mRegStatus |= 0x02;
				mpFnDrqChange(true);
			}

			mWriteTrackBuffer[mWriteTrackIndex++] = mRegData;
			if (mWriteTrackIndex >= kMaxBytesPerTrack)
				mWriteTrackIndex = 0;
			break;

		case kState_WriteTrack_InitialDrqTimeout:
			// set data lost flag
			mRegStatus |= 0x04;
			SetTransition(kState_EndNonTypeICommand, 1);
			break;

		case kState_WriteTrack_Complete:
			FinalizeWriteTrack();
			SetTransition(kState_EndNonTypeICommand, 1);
			break;

		case kState_ReadAddress: {
			mTransferIndex = 0;
			mTransferLength = 0;
			mbDataReadPending = false;

			UpdateRotationalPosition();

			uint32 delay = mCyclesPerRotation * 10;		// This just needs to be big enough for 5 index marks to occur.

			mActiveSectorStatus = 0x10;

			if (mpDiskImage && mbMotorRunning && mbMFM == mDiskGeometry.mbMFM) {
				// find next sector in rotational order on the entire track
				const float posf = (float)mRotPos / (float)mCyclesPerRotation;
				float bestDistance = 2.0f;
				float bestRotPos = 0;
				uint32 bestVSec = 0;
				uint32 bestVSecCount = 0;
				uint32 bestPhysSec = 0;
				uint32 bestPSecOffset = 0;
				int bestWeakDataOffset = -1;
				uint32 bestSectorSize = 0;

				for(uint32 sector = 1; sector <= mDiskGeometry.mSectorsPerTrack; ++sector) {
					const uint32 vsec = mPhysTrack * mDiskGeometry.mSectorsPerTrack + sector;

					ATDiskVirtualSectorInfo vsi {};

					if (vsec <= mpDiskImage->GetVirtualSectorCount())
						mpDiskImage->GetVirtualSectorInfo(vsec - 1, vsi);

					if (vsi.mNumPhysSectors) {
						for(uint32 i=0; i<vsi.mNumPhysSectors; ++i) {
							ATDiskPhysicalSectorInfo psi {};

							mpDiskImage->GetPhysicalSectorInfo(vsi.mStartPhysSector + i, psi);

							if ((psi.mFDCStatus & 0x18) == 0x10)
								continue;

							float distance = psi.mRotPos - posf;
							distance -= floorf(distance);

							if (distance < bestDistance) {
								bestDistance = distance;
								bestPhysSec = vsi.mStartPhysSector + i;
								bestRotPos = psi.mRotPos;

								// We need uninverted status since we're in the FDC (inversion is
								// handled in the 810 emulation code).
								mActiveSectorStatus = ~psi.mFDCStatus;
								bestWeakDataOffset = psi.mWeakDataOffset;
								bestSectorSize = psi.mSize;
								bestVSec = vsec;
								bestVSecCount = vsi.mNumPhysSectors;
								bestPSecOffset = i;
							}
						}
					}
				}

				if (bestVSec) {
					// check for a long sector not actually stored in the image
					if ((mActiveSectorStatus & 0x02) && bestSectorSize < 256)
						bestSectorSize = 256;

					// Synthesize the six bytes returned for the Read Address command.
					mTransferBuffer[0] = (uint8)((bestVSec - 1) / mDiskGeometry.mSectorsPerTrack);
					mTransferBuffer[1] = 0;
					mTransferBuffer[2] = (uint8)((bestVSec - 1) % mDiskGeometry.mSectorsPerTrack + 1);
					mTransferBuffer[3] = (bestSectorSize >= 1024 ? 3 : bestSectorSize >= 512 ? 2 : bestSectorSize >= 256 ? 1 : 0);

					// check for boot sector
					if (bestSectorSize == 128 && bestVSec <= mDiskGeometry.mBootSectorCount && mDiskGeometry.mbMFM && mDiskGeometry.mSectorSize > 128)
						mTransferBuffer[3] = 1;

					const uint8 crcbuf[5] = {
						0xFE,		// IDAM
						mTransferBuffer[0],
						mTransferBuffer[1],
						mTransferBuffer[2],
						mTransferBuffer[3]
					};

					uint16 crc = 0xFFFF;
					for(const uint8 c : crcbuf) {
						crc ^= (uint16_t)c << 8;
						for(int j=0; j<8; ++j) {
							uint16_t feedback = (crc & 0x8000) ? 0x1021 : 0;
							crc += crc;
							crc ^= feedback;
						}
					}

					VDWriteUnalignedBEU16(&mTransferBuffer[4], crc);

					mTransferLength = 6;

					if (mbUseAccurateTiming) {
						// Compute rotational delay. It takes about 26 raw bytes in the standard 810
						// format from start of address to start of first byte, so including the
						// first byte, that's 26*8*4 = 832 cycles of latency until first DRQ.
						delay = VDRoundToInt(bestDistance * (float)mCyclesPerRotation + 832);
					} else {
						// Warp disk to start of sector data field.
						mRotPos = VDRoundToInt(bestRotPos * (float)mCyclesPerRotation) + 832;

						if (mRotPos >= mCyclesPerRotation)
							mRotPos -= mCyclesPerRotation;

						// Use short delay.
						delay = 300;
					}

					mpDiskInterface->SetShowActivity(true, bestVSec);

					if (g_ATLCDisk.IsEnabled() && bestVSec) {
						g_ATLCDisk("Reading address vsec=%3d (%d/%d) (trk=%d), psec=%3d, rot=%.2f >> %.2f >> %.2f%s.\n"
								, bestVSec
								, bestPSecOffset + 1
								, bestVSecCount
								, mPhysTrack
								, (uint32)bestPhysSec
								, (float)mRotPos / (float)mCyclesPerRotation
								, bestRotPos
								, (float)mRotPos / (float)mCyclesPerRotation
								,  bestWeakDataOffset >= 0 ? " (w/weak bits)"
									: (mActiveSectorStatus & 0x02) ? " (w/long sector)"		// must use DRQ as lost data differs between drives
									: (mActiveSectorStatus & 0x08) ? " (w/CRC error)"
									: (mActiveSectorStatus & 0x10) ? " (w/missing sector)"
									: (mActiveSectorStatus & 0x20) ? " (w/deleted sector)"
									: ""
								);
					}

					// Don't flag a CRC without missing record since the data
					// frame is not validated on a Read Address.
					if ((mActiveSectorStatus & 0x18) == 0x08)
						mActiveSectorStatus &= ~0x08;
				}
			}

			if (mTransferLength)
				SetTransition(kState_ReadSector_TransferFirstByte, delay);
			else if (mActiveSectorStatus & 0x10)
				SetTransition(kState_ReadSector_TransferFirstByteNever, delay);
			else
				SetTransition(kState_ReadSector_TransferComplete, delay);
			break;
		}

		case kState_ReadSector: {
			mTransferIndex = 0;
			mTransferLength = 0;
			mbDataReadPending = false;

			UpdateRotationalPosition();

			uint32 delay = mCyclesPerRotation * 10;		// This just needs to be big enough for 5 index marks to occur.

			mActiveSectorStatus = 0x10;

			if (mpDiskImage && mRegSector > 0 && mRegSector <= mDiskGeometry.mSectorsPerTrack && mbMotorRunning && mbMFM == mDiskGeometry.mbMFM) {
				uint32 vsec = mPhysTrack * mDiskGeometry.mSectorsPerTrack + mRegSector;

				ATDiskVirtualSectorInfo vsi {};

				if (vsec <= mpDiskImage->GetVirtualSectorCount())
					mpDiskImage->GetVirtualSectorInfo(vsec - 1, vsi);

				if (vsi.mNumPhysSectors) {
					// find next sector in rotational order
					const float posf = (float)mRotPos / (float)mCyclesPerRotation;
					float bestDistance = 2.0f;
					float bestRotPos = 0;
					uint32 bestPhysSec = 0;
					int bestWeakDataOffset = -1;

					for(uint32 i=0; i<vsi.mNumPhysSectors; ++i) {
						ATDiskPhysicalSectorInfo psi {};

						mpDiskImage->GetPhysicalSectorInfo(vsi.mStartPhysSector + i, psi);

						float distance = psi.mRotPos - posf;
						distance -= floorf(distance);

						if (distance < bestDistance) {
							bestDistance = distance;
							bestPhysSec = vsi.mStartPhysSector + i;
							bestRotPos = psi.mRotPos;

							// We need uninverted status since we're in the FDC (inversion is
							// handled in the 810 emulation code).
							mActiveSectorStatus = ~psi.mFDCStatus;
							bestWeakDataOffset = psi.mWeakDataOffset;
							mTransferLength = std::min<uint32>(psi.mSize, vdcountof(mTransferBuffer));
						}
					}

					// check for a long sector not actually stored in the image
					if ((mActiveSectorStatus & 0x02) && mTransferLength < 256)
						mTransferLength = 256;

					memset(mTransferBuffer, 0xFF, mTransferLength);
					mpDiskImage->ReadPhysicalSector(bestPhysSec, mTransferBuffer, mTransferLength);

					// check for a boot sector on a double density disk
					if (mTransferLength == 128 && vsec <= mDiskGeometry.mBootSectorCount && mDiskGeometry.mbMFM && mDiskGeometry.mSectorSize > 128)
						mTransferLength = 256;

					// apply weak bits
					if (bestWeakDataOffset >= 0) {
						for(uint32 i = (uint32)bestWeakDataOffset; i < mTransferLength; ++i) {
							mTransferBuffer[i] ^= (uint8)mWeakBitLFSR;

							mWeakBitLFSR = (mWeakBitLFSR << 8) + (0xff & ((mWeakBitLFSR >> (28 - 8)) ^ (mWeakBitLFSR >> (31 - 8))));
						}
					}

					// We have to invert the data, because all Atari drives write data inverted.
					for(uint32 i=0; i<mTransferLength; ++i)
						mTransferBuffer[i] = ~mTransferBuffer[i];

					if (mbUseAccurateTiming) {
						// Compute rotational delay. It takes about 26 raw bytes in the standard 810
						// format from start of address to start of first byte, so including the
						// first byte, that's 26*8*4 = 832 cycles of latency until first DRQ.
						delay = VDRoundToInt(bestDistance * (float)mCyclesPerRotation + 832);
					} else {
						// Warp disk to start of sector data field.
						mRotPos = VDRoundToInt(bestRotPos * (float)mCyclesPerRotation) + 832;

						if (mRotPos >= mCyclesPerRotation)
							mRotPos -= mCyclesPerRotation;

						// Use short delay.
						delay = 1000;
					}

					mpDiskInterface->SetShowActivity(true, vsec);

					if (g_ATLCDisk.IsEnabled()) {
						g_ATLCDisk("Reading vsec=%3d (%d/%d) (trk=%d), psec=%3d, rot=%.2f >> %.2f >> %.2f%s.\n"
								, vsec
								, (uint32)bestPhysSec - vsi.mStartPhysSector + 1
								, vsi.mNumPhysSectors
								, (vsec - 1) / mDiskGeometry.mSectorsPerTrack
								, (uint32)bestPhysSec
								, (float)mRotPos / (float)mCyclesPerRotation
								, bestRotPos
								, (float)mRotPos / (float)mCyclesPerRotation
								,  bestWeakDataOffset >= 0 ? " (w/weak bits)"
									: (mActiveSectorStatus & 0x02) ? " (w/long sector)"		// must use DRQ as lost data differs between drives
									: (mActiveSectorStatus & 0x08) ? " (w/CRC error)"
									: (mActiveSectorStatus & 0x10) ? " (w/missing sector)"
									: (mActiveSectorStatus & 0x20) ? " (w/deleted sector)"
									: ""
								);
					}
				}
			}

			if (mTransferLength)
				SetTransition(kState_ReadSector_TransferFirstByte, delay);
			else if (mActiveSectorStatus & 0x10)
				SetTransition(kState_ReadSector_TransferFirstByteNever, delay);
			else
				SetTransition(kState_ReadSector_TransferComplete, delay);
			break;
		}

		case kState_ReadSector_TransferFirstByteNever:
			// we just sit here and wait for 5 index marks
			break;

		case kState_ReadSector_TransferFirstByte:
		case kState_ReadSector_TransferByte:
			// check for lost data
			if (mbDataReadPending)
				mRegStatus |= 0x04;

			// set DRQ
			if (!mbDataReadPending) {
				mbDataReadPending = true;

				mpFnDrqChange(true);
			}

			mRegStatus |= 0x02;

			mRegData = mTransferBuffer[mTransferIndex];
			if (++mTransferIndex >= mTransferLength)
				SetTransition(kState_ReadSector_TransferComplete, 64);
			else
				SetTransition(kState_ReadSector_TransferByte, 32);
			break;

		case kState_ReadSector_TransferComplete:
			// Update status based on sector read:
			//
			//	bit 7 (not ready)			drop - recomputed
			//	bit 6 (record type)			drop - recomputed
			//	bit 5 (record type)			replicate to bit 5/6
			//	bit 4 (record not found)	keep
			//	bit 3 (CRC error)			keep
			//	bit 2 (lost data)			drop - driven by sequencer
			//	bit 1 (DRQ)					drop - driven by sequencer
			//	bit 0 (busy)				drop - driven by sequencer
			mRegStatus = (mRegStatus & ~0x78) + (mActiveSectorStatus & 0x38);
			mRegStatus += (mRegStatus & 0x20) * 2;

			SetTransition(kState_EndNonTypeICommand, 1);
			break;

		case kState_WriteSector: {
			mTransferIndex = 0;
			mTransferLength = 0;

			UpdateRotationalPosition();

			if (!mpDiskInterface->IsDiskWritable()) {
				mRegStatus |= 0x40;
				SetTransition(kState_EndNonTypeICommand, 1);
				break;
			}

			uint32 delay = mCyclesPerRotation * 2;

			mActiveSectorStatus = 0x10;

			if (mpDiskImage && mRegSector > 0 && mRegSector <= mDiskGeometry.mSectorsPerTrack && mbMotorRunning && mbMFM == mDiskGeometry.mbMFM) {
				uint32 vsec = mPhysTrack * mDiskGeometry.mSectorsPerTrack + mRegSector;

				ATDiskVirtualSectorInfo vsi {};

				if (vsec <= mpDiskImage->GetVirtualSectorCount())
					mpDiskImage->GetVirtualSectorInfo(vsec - 1, vsi);

				if (vsi.mNumPhysSectors) {
					// find next sector in rotational order
					const float posf = (float)mRotPos / (float)mCyclesPerRotation;
					float bestDistance = 2.0f;
					float bestRotPos = 0;
					uint32 bestPhysSec = 0;
					int bestWeakDataOffset = -1;

					for(uint32 i=0; i<vsi.mNumPhysSectors; ++i) {
						ATDiskPhysicalSectorInfo psi {};

						mpDiskImage->GetPhysicalSectorInfo(vsi.mStartPhysSector + i, psi);

						float distance = psi.mRotPos - posf;
						distance -= floorf(distance);

						if (distance < bestDistance) {
							bestDistance = distance;
							bestPhysSec = vsi.mStartPhysSector + i;
							bestRotPos = psi.mRotPos;

							// We need uninverted status since we're in the FDC (inversion is
							// handled in the 810 emulation code).
							mActiveSectorStatus = ~psi.mFDCStatus;
							bestWeakDataOffset = psi.mWeakDataOffset;
							mTransferLength = std::min<uint32>(psi.mSize, vdcountof(mTransferBuffer));
						}
					}

					mActivePhysSector = bestPhysSec;

					// check for a long sector not actually stored in the image
					if ((mActiveSectorStatus & 0x02) && mTransferLength < 256)
						mTransferLength = 256;

					// check for a boot sector on a double density disk
					if (mTransferLength == 128 && vsec <= mDiskGeometry.mBootSectorCount && mDiskGeometry.mbMFM && mDiskGeometry.mSectorSize > 128)
						mTransferLength = 256;

					if (mbUseAccurateTiming) {
						// Compute rotational delay. It takes about 26 raw bytes in the standard 810
						// format from start of address to start of first byte, so including the
						// first byte, that's 26*8*4 = 832 cycles of latency until first DRQ.
						delay = VDRoundToInt(bestDistance * (float)mCyclesPerRotation + 832);
					} else {
						// Warp disk to start of sector data field.
						mRotPos = VDRoundToInt(bestRotPos * (float)mCyclesPerRotation) + 832;

						if (mRotPos >= mCyclesPerRotation)
							mRotPos -= mCyclesPerRotation;

						// Use short delay.
						delay = 1000;
					}

					mpDiskInterface->SetShowActivity(true, vsec);

					if (g_ATLCDisk.IsEnabled()) {
						g_ATLCDisk("Writing vsec=%3d (%d/%d) (trk=%d), psec=%3d, rot=%.2f >> %.2f >> %.2f%s.\n"
								, vsec
								, (uint32)bestPhysSec - vsi.mStartPhysSector + 1
								, vsi.mNumPhysSectors
								, (vsec - 1) / mDiskGeometry.mSectorsPerTrack
								, (uint32)bestPhysSec
								, (float)mRotPos / (float)mCyclesPerRotation
								, bestRotPos
								, (float)mRotPos / (float)mCyclesPerRotation
								,  bestWeakDataOffset >= 0 ? " (w/weak bits)"
									: (mActiveSectorStatus & 0x02) ? " (w/long sector)"		// must use DRQ as lost data differs between drives
									: (mActiveSectorStatus & 0x08) ? " (w/CRC error)"
									: (mActiveSectorStatus & 0x10) ? " (w/missing sector)"
									: (mActiveSectorStatus & 0x20) ? " (w/deleted sector)"
									: ""
								);
					}
				}
			}

			if (mActiveSectorStatus & 0x10)
				SetTransition(kState_WriteSector_TransferComplete, delay);
			else
				SetTransition(kState_WriteSector_InitialDrq, delay);
			break;
		}

		case kState_WriteSector_InitialDrq:
			// Set DRQ and wait 9 bytes
			mbDataWritePending = true;
			mRegStatus |= 0x02;
			mpFnDrqChange(true);
			SetTransition(kState_WriteSector_CheckInitialDrq, 9 * kCyclesPerByte);
			break;

		case kState_WriteSector_CheckInitialDrq:
			if (mbDataWritePending) {
				// No initial data -- signal lost data and abort the command immediately
				mRegStatus |= 0x04;
				SetTransition(kState_WriteSector_TransferComplete, 1);
			} else {
				// Write DAM; DRQ does not get set again until after this occurs
				SetTransition(kState_WriteSector_TransferByte, 2 * kCyclesPerByte);
			}
			break;

		case kState_WriteSector_TransferByte:
			// Check for lost data and transfer to disk buffer; WD docs say that $00 is
			// written if data lost. Note that we need to invert the data as the disk image
			// interface expects computer-type noninverted data.
			if (mbDataWritePending) {
				mRegStatus |= 0x04;
				mTransferBuffer[mTransferIndex] = 0xFF;
			} else
				mTransferBuffer[mTransferIndex] = ~mRegData;

			// check if we have more bytes to write
			++mTransferIndex;
			if (mTransferIndex < mTransferLength) {
				// Yes, we do -- set DRQ, then wait a byte's time while this byte is
				// being written
				if (!mbDataWritePending) {
					mbDataWritePending = true;
					mRegStatus |= 0x02;

					mpFnDrqChange(true);
				}

				SetTransition(kState_WriteSector_TransferByte, kCyclesPerByte);
			} else {
				// No, we don't. Write the sector to persistent storage now, and delay four
				// bytes for the write timing for the remaining data (byte just transferred,
				// CRC 1 and 2, and $FF).
				if (mpDiskImage && mActivePhysSector < mpDiskImage->GetPhysicalSectorCount() && mpDiskInterface->IsDiskWritable()) {
					try {
						mpDiskInterface->OnDiskModified();
						mpDiskImage->WritePhysicalSector(mActivePhysSector, mTransferBuffer, mTransferLength);
					} catch(...) {
						// mark write fault
						mRegStatus |= 0x20;
					}
				} else {
					// mark write fault
					mRegStatus |= 0x20;
				}

				SetTransition(kState_WriteSector_TransferComplete, kCyclesPerByte * 4);
			}
			break;

		case kState_WriteSector_TransferComplete:
			// Update status based on sector write:
			//
			//	bit 7 (not ready)			drop - recomputed
			//	bit 6 (write protect)		drop - recomputed
			//	bit 5 (write fault)			drop - recomputed
			//	bit 4 (record not found)	keep
			//	bit 3 (CRC error)			keep
			//	bit 2 (lost data)			drop - driven by sequencer
			//	bit 1 (DRQ)					drop - driven by sequencer
			//	bit 0 (busy)				drop - driven by sequencer
			mRegStatus = (mRegStatus & ~0x18) + (mActiveSectorStatus & 0x18);

			SetTransition(kState_EndNonTypeICommand, 1);
			break;

		case kState_ForceInterrupt:
			SetTransition(kState_ForceInterrupt_2, 16);
			break;

		case kState_ForceInterrupt_2:
			goto end_type_I;
	}
}

void ATFDCEmulator::SetTransition(State nextState, uint32 ticks) {
	mState = nextState;
	mpScheduler->SetEvent(ticks, this, 1, mpStateEvent);
}

void ATFDCEmulator::UpdateRotationalPosition() {
	if (mbMotorRunning) {
		const uint64 t = mpScheduler->GetTick64();
		mRotPos = (mRotPos + (t - mRotTimeBase)) % mCyclesPerRotation;
		mRotTimeBase = t;
	}
}

void ATFDCEmulator::FinalizeWriteTrack() {
	if (!mpDiskImage || !mpDiskInterface->IsDiskWritable())
		return;

	// $F7 bytes actually produce two raw bytes, so we can easily have more than the allowed
	// number of bytes in the track. Work backwards from the end until we have the max number
	// of bytes; everything before that would have been overwritten by track wrap.
	uint32 endPos = mWriteTrackIndex;
	uint32 startPos = endPos;
	uint32 extraCrcLen = 0;
	uint32 validLen = 0;

	while(validLen + extraCrcLen < kMaxBytesPerTrack) {
		if (startPos == 0)
			startPos = kMaxBytesPerTrack;
		--startPos;

		++validLen;
		if (mWriteTrackBuffer[startPos] == 0xF7)
			++extraCrcLen;
	}

	// Rotate the buffer around so the valid area is contiguous.
	std::rotate(mWriteTrackBuffer.begin(), mWriteTrackBuffer.begin() + startPos, mWriteTrackBuffer.end());

	// Parse out the valid sectors.
	enum ParseState {
		kParseState_WaitFor00,
		kParseState_WaitForIdAddressMark,
		kParseState_ProcessIdRecord,
		kParseState_WaitForDataAddressMark00,
		kParseState_WaitForDataAddressMark,
		kParseState_ProcessDataRecord,
	};

	ParseState parseState = kParseState_WaitFor00;
	uint32 parseStateCounter = 0;
	uint32 parseSectorLength = 0;
	uint8 idField[5] = {};

	struct ParsedSector {
		uint8 mId;
		uint32 mStart;
		uint32 mLen;
	};

	vdfastvector<ParsedSector> parsedSectors;

	const uint8 *const src = mWriteTrackBuffer.data();
	for(uint32 i = 0; i < validLen; ++i) {
		const uint8 c = src[i];

		switch(parseState) {
			case kParseState_WaitFor00:
				if (c == 0x00)
					parseState = kParseState_WaitForIdAddressMark;
				break;

			case kParseState_WaitForIdAddressMark:
				if (c == 0xFE) {
					parseState = kParseState_ProcessIdRecord;
					parseStateCounter = 0;
				} else if (c != 0x00)
					parseState = kParseState_WaitFor00;
				break;

			case kParseState_ProcessIdRecord:
				idField[parseStateCounter++] = c;

				if (parseStateCounter >= 5) {
					parseState = kParseState_WaitFor00;
					parseStateCounter = 0;

					// check if track field is valid
					if (idField[0] != mPhysTrack)
						break;

					// check if head, sector, and sector length fields are valid
					if ((uint8)(idField[1] + 1) >= 0xF8
						|| (uint8)(idField[2] + 1) >= 0xF8
						|| (uint8)(idField[3] + 1) >= 0xF8)
						break;

					// check if CRC is valid
					if (idField[4] != 0xF7)
						break;

					parseSectorLength = 128 << (idField[3] & 0x03);

					parseState = kParseState_WaitForDataAddressMark00;

					// The Happy copier does something very evil, which is that it writes out
					// the ID address field but not the data field, instead just padding out
					// a bunch of $00s, relying on subsequent Write Sector commands to write
					// the DAM. This means we have to establish the sectors just based on the
					// ID alone.
					parsedSectors.push_back({ idField[2], i, parseSectorLength });
				}
				break;

			case kParseState_WaitForDataAddressMark00:
				if (++parseStateCounter > 30)
					parseState = kParseState_WaitFor00;
				else if (c == 0x00)
					parseState = kParseState_WaitForDataAddressMark;
				break;

			case kParseState_WaitForDataAddressMark:
				if (++parseStateCounter > 30)
					parseState = kParseState_WaitFor00;
				else if ((c & 0xFC) == 0xF8) {
					parseState = kParseState_ProcessDataRecord;
					parseStateCounter = 0;
				} else if (c != 0x00)
					parseState = kParseState_WaitForDataAddressMark00;
				break;

			case kParseState_ProcessDataRecord:
				if (parseStateCounter < parseSectorLength) {
					if (c < 0xF7 || c == 0xFF) {
						++parseStateCounter;
						break;
					}
				} else {
					if (c == 0xF7) {
						g_ATLCFDC("Found sector %u\n", idField[2]);
					}
				}
				parseState = kParseState_WaitFor00;
				break;
		};
	}

	ATDiskVirtualSectorInfo newVirtSectors[18] = {};
	vdfastvector<ATDiskPhysicalSectorInfo> newPhysSectors;
	newPhysSectors.reserve(parsedSectors.size());

	if (!parsedSectors.empty()) {
		UpdateRotationalPosition();

		for(uint32 i=0; i<18; ++i) {
			newVirtSectors[i].mStartPhysSector = (uint32)newPhysSectors.size();

			for(const auto& parsedSector : parsedSectors) {
				if (parsedSector.mId != i+1)
					continue;

				++newVirtSectors[i].mNumPhysSectors;

				ATDiskPhysicalSectorInfo psi {};

				psi.mbDirty = true;
				psi.mDiskOffset = -1;
				psi.mFDCStatus = 0xFF;
				psi.mOffset = parsedSector.mStart;
				psi.mRotPos = ((float)mRotPos - validLen + (float)parsedSector.mStart * kCyclesPerByte) / (float)mCyclesPerRotation;
				psi.mRotPos -= floorf(psi.mRotPos);
				psi.mSize = parsedSector.mLen;
				psi.mWeakDataOffset = -1;

				newPhysSectors.push_back(psi);
			}
		}
	}

	mpDiskImage->FormatTrack(18 * mPhysTrack, 18, newVirtSectors, (uint32)newPhysSectors.size(), newPhysSectors.data(), mWriteTrackBuffer.data());
	mpDiskInterface->OnDiskChanged();
}
