Drives.cpp

//
//  Drives.cpp
//  SDL3Test
//
//  Created by Matt Parsons on 22/02/2026.
//

#include "Drives.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <sys/stat.h>
#include <cstring>

// MFM encode a block of longwords using Amiga odd/even split.
// Writes 2*longwordCount uint16_t values into dest (odd words first, then even words).
// Returns the number of uint16_t values written (always 2*longwordCount).
static int MFMEncodeBlock(uint16_t* dest, const uint8_t* src, int byteCount, uint16_t& prevWord) {
    int longwordCount = byteCount / 4;
    int pos = 0;

    // Pass 1: odd bits
    for (int i = 0; i < longwordCount; i++) {
        uint32_t lw = ((uint32_t)src[i*4] << 24) | ((uint32_t)src[i*4+1] << 16) |
                      ((uint32_t)src[i*4+2] << 8) | src[i*4+3];
        uint32_t odd = (lw >> 1) & 0x55555555;
        // Split into two 16-bit words
        uint16_t hi = (uint16_t)(odd >> 16);
        uint16_t lo = (uint16_t)(odd & 0xFFFF);

        // Add clock bits for hi word
        uint16_t clock = ~(hi | (hi << 1) | ((prevWord & 1) ? 0x8000 : 0));
        clock &= 0xAAAA;
        dest[pos] = hi | clock;
        prevWord = dest[pos];
        pos++;

        // Add clock bits for lo word
        clock = ~(lo | (lo << 1) | ((prevWord & 1) ? 0x8000 : 0));
        clock &= 0xAAAA;
        dest[pos] = lo | clock;
        prevWord = dest[pos];
        pos++;
    }

    // Pass 2: even bits
    for (int i = 0; i < longwordCount; i++) {
        uint32_t lw = ((uint32_t)src[i*4] << 24) | ((uint32_t)src[i*4+1] << 16) |
                      ((uint32_t)src[i*4+2] << 8) | src[i*4+3];
        uint32_t even = lw & 0x55555555;
        uint16_t hi = (uint16_t)(even >> 16);
        uint16_t lo = (uint16_t)(even & 0xFFFF);

        uint16_t clock = ~(hi | (hi << 1) | ((prevWord & 1) ? 0x8000 : 0));
        clock &= 0xAAAA;
        dest[pos] = hi | clock;
        prevWord = dest[pos];
        pos++;

        clock = ~(lo | (lo << 1) | ((prevWord & 1) ? 0x8000 : 0));
        clock &= 0xAAAA;
        dest[pos] = lo | clock;
        prevWord = dest[pos];
        pos++;
    }

    return pos;
}

// Compute Amiga MFM checksum: XOR all uint32_t in the range, mask with 0x55555555
static uint32_t MFMChecksum(const uint16_t* data, int wordCount) {
    uint32_t checksum = 0;
    for (int i = 0; i < wordCount; i += 2) {
        uint32_t lw = ((uint32_t)data[i] << 16) | data[i+1];
        checksum ^= lw;
    }
    return checksum & 0x55555555;
}

void Floppy::InsertDisk(const char* path) {
    EjectDisk();

    FILE* f = fopen(path, "rb");
    if (!f) {
        printf("Disk: Failed to open %s\n", path);
        return;
    }

    fseek(f, 0, SEEK_END);
    long size = ftell(f);
    fseek(f, 0, SEEK_SET);

    if (size != ADF_SIZE) {
        printf("Disk: Invalid ADF size %ld (expected %d)\n", size, ADF_SIZE);
        fclose(f);
        return;
    }

    adfImage = new uint8_t[ADF_SIZE];
    size_t bytesRead = fread(adfImage, 1, ADF_SIZE, f);
    fclose(f);

    if (bytesRead != ADF_SIZE) {
        printf("Disk: Short read %zu bytes\n", bytesRead);
        delete[] adfImage;
        adfImage = nullptr;
        return;
    }

    adfSize = ADF_SIZE;
    filePath = path;
    change = false;
    printf("Disk: Inserted %s\n", path);

    EncodeMFMTrack();
}

void Floppy::EjectDisk() {
    if (adfImage) {
        delete[] adfImage;
        adfImage = nullptr;
    }
    adfSize = 0;
    change = true;
    trackDirty = false;
    mfmTrackLength = 0;
}

void Floppy::EncodeMFMTrack() {
    if (!adfImage) {
        mfmTrackLength = 0;
        return;
    }

    int adfOffset = (track * 2 + side) * 11 * 512;
    int pos = 0;
    uint16_t prevWord = 0xAAAA;

    for (int sector = 0; sector < 11; sector++) {
        // Gap bytes (2 words of 0x0000 MFM-encoded = 0xAAAA)
        mfmTrack[pos++] = 0xAAAA;
        mfmTrack[pos++] = 0xAAAA;
        prevWord = 0xAAAA;

        // Sync words
        mfmTrack[pos++] = 0x4489;
        mfmTrack[pos++] = 0x4489;
        prevWord = 0x4489;

        // Header info: format=$FF, track, sector, sectors-to-gap
        uint8_t header[4];
        header[0] = 0xFF;                          // AmigaDOS format
        header[1] = (uint8_t)(track * 2 + side);   // Track number (0-159)
        header[2] = (uint8_t)sector;                // Sector number (0-10)
        header[3] = (uint8_t)(11 - sector);         // Sectors until gap

        int headerStart = pos;
        pos += MFMEncodeBlock(&mfmTrack[pos], header, 4, prevWord);

        // Sector label: 16 bytes of zeros
        uint8_t label[16];
        memset(label, 0, 16);
        pos += MFMEncodeBlock(&mfmTrack[pos], label, 16, prevWord);

        // Header checksum
        uint32_t hdrChecksum = MFMChecksum(&mfmTrack[headerStart], pos - headerStart);
        uint8_t checksumBytes[4];
        checksumBytes[0] = (uint8_t)(hdrChecksum >> 24);
        checksumBytes[1] = (uint8_t)(hdrChecksum >> 16);
        checksumBytes[2] = (uint8_t)(hdrChecksum >> 8);
        checksumBytes[3] = (uint8_t)(hdrChecksum);
        pos += MFMEncodeBlock(&mfmTrack[pos], checksumBytes, 4, prevWord);

        // Data checksum placeholder — we need to encode data first, then compute
        int dataChecksumPos = pos;
        pos += 4; // Reserve 4 words for data checksum (2 odd + 2 even)

        // Sector data (512 bytes)
        int dataStart = pos;
        const uint8_t* sectorData = &adfImage[adfOffset + sector * 512];
        pos += MFMEncodeBlock(&mfmTrack[pos], sectorData, 512, prevWord);

        // Now compute data checksum and fill it in
        uint32_t dataChecksum = MFMChecksum(&mfmTrack[dataStart], pos - dataStart);
        checksumBytes[0] = (uint8_t)(dataChecksum >> 24);
        checksumBytes[1] = (uint8_t)(dataChecksum >> 16);
        checksumBytes[2] = (uint8_t)(dataChecksum >> 8);
        checksumBytes[3] = (uint8_t)(dataChecksum);
        uint16_t prevBeforeChecksum = (dataChecksumPos > 0) ? mfmTrack[dataChecksumPos - 1] : 0xAAAA;
        MFMEncodeBlock(&mfmTrack[dataChecksumPos], checksumBytes, 4, prevBeforeChecksum);
    }

    // Fill remaining track with gap
    while (pos < MFM_TRACK_WORDS) {
        mfmTrack[pos++] = 0xAAAA;
    }

    mfmTrackLength = MFM_TRACK_WORDS;
}

void Floppy::step(){

    if(direction){
        track++;
        track0 = false;
    }else{
        track--;
        if(track == -1){
            track = 0;
        }
        if(track==0){
            track0 = true;
        }
    }

    EncodeMFMTrack();
}

bool Floppy::DiskPresent(){
    return adfImage != nullptr;
}



//Drives

void Drives::selectDisk(int disk){

    if(IDMode == true){
        // Drive select during ID mode — latch which drive we're identifying
        if(currentDisk == -1 && disk >= 0){
            currentDisk = disk;
            IDCountdown = 32;
            //printf("Disk: %d - Enter ID Mode\n", currentDisk);
        }
        // Deselect just updates currentDisk
        if(disk == -1){
            currentDisk = -1;
        }
        return;
    }

    currentDisk = disk;
}

bool Drives::motor(){
    if(currentDisk > -1){
        return disk[currentDisk].motor;
    }
    return false;
}

bool Drives::track0(){
    if(currentDisk > -1){
        return disk[currentDisk].track0;
    }
    return false;
}

bool Drives::ready(){
    if(currentDisk > -1){
        return disk[currentDisk].ready;
    }
    return false;
}


void Drives::setMotor(bool on){

    // Motor ON with no drive selected arms ID mode
    if(on && currentDisk == -1 && !IDMode){
        IDMode = true;
        IDCountdown = 0;  // 0 = armed, waiting for drive select
        //printf("Disk: ID Mode armed (motor on, no drive)\n");
        return;
    }

    // During ID mode, motor toggles clock out ID bits
    if(IDMode && currentDisk >= 0){
        // Each motor toggle clocks one bit
        if(IDCountdown > 0){
            IDCountdown--;
            if(IDCountdown <= 0){
                IDMode = false;
                IDCountdown = 0;
                //printf("Disk: %d - Exit ID Mode\n", currentDisk);
            }
        }
        return;
    }

    // Not in ID mode — normal motor control
    if(currentDisk > -1){
        if(on){
            disk[currentDisk].motor = true;
            disk[currentDisk].ready = true;
        }else{
            disk[currentDisk].motor = false;
            disk[currentDisk].ready = false;
        }
    }

}



bool Drives::writeProtect(){
    if(currentDisk > -1){
        return disk[currentDisk].writeProtect;
    }
    return false;
}

bool Drives::change(){
    if(currentDisk > -1){
        return disk[currentDisk].change;
    }
    return false;
}


int Drives::side(){
    if(currentDisk > -1){
        return disk[currentDisk].side;
    }
    return 0;
}

int Drives::direction(){
    if(currentDisk > -1){
        return disk[currentDisk].direction;
    }
    return 0;
}

void Drives::setSide(int side){
    if(currentDisk > -1){
        disk[currentDisk].side = side;
        disk[currentDisk].EncodeMFMTrack();
    }
}

void Drives::setDirection(int direction){
    if(currentDisk > -1){
        disk[currentDisk].direction = direction;
    }
}



void Drives::setIDMode(int drive){
    //printf("ID Mode Set\n");
    IDMode = drive;
    IDCountdown = 32;
}

void Drives::step(){
    if(currentDisk > -1){
        disk[currentDisk].step();
    }
}

Floppy* Drives::getSelectedDisk(){
    if(currentDisk > -1){
        return &disk[currentDisk];
    }
    return nullptr;
}

void Drives::updatePRB(int driveNum, bool motorOn, int sideVal, int directionVal, bool stepPulse) {
    currentDisk = driveNum;

    // DD drives have no ID circuit — just handle motor/signals directly.
    // During Kick 3's ID sequence (rapid motor toggles), ready tracks motor
    // instantly: motor off → ready=false → RDY inactive (1). This naturally
    // produces ID = 0xFFFFFFFF for DD drives.

    if (driveNum >= 0) {
        disk[driveNum].motor = motorOn;
        disk[driveNum].ready = motorOn;
        disk[driveNum].direction = directionVal;

        if (disk[driveNum].side != sideVal) {
            disk[driveNum].side = sideVal;
            disk[driveNum].EncodeMFMTrack();
        }

        if (stepPulse) {
            disk[driveNum].step();
        }
    }
}