
#include <exec/types.h>
#include <exec/memory.h>
#include <exec/tasks.h>
#include <devices/trackdisk.h>
#include <hardware/custom.h>
#include <hardware/dmabits.h>
#include <hardware/intbits.h>
#include <proto/exec.h>
#include <proto/dos.h>
#include <hardware/cia.h>

#define CIABPRB (*(volatile UBYTE *)0xBFD100)
#define DSKPTH (*(volatile UWORD *)0xDFF020)
#define DSKPTL (*(volatile UWORD *)0xDFF022)
#define DSKLEN (*(volatile UWORD *)0xDFF024)
#define DMACON (*(volatile UWORD *)0xDFF096)
#define INTREQ (*(volatile UWORD *)0xDFF09C)
#define INTREQR (*(volatile UWORD *)0xDFF01E)

#define CHUNK_SIZE  32766
#define NUM_DRIVES  4

struct ExecBase    *SysBase;
struct DosLibrary  *DOSBase;
#define custom (*(struct Custom *)0xDFF000)

struct MsgPort  *drivePort[NUM_DRIVES];
struct IOExtTD  *driveIO[NUM_DRIVES];
BOOL             driveOpen[NUM_DRIVES];

void stopAllDrives(void) {
    int i;
    for (i = 0; i < NUM_DRIVES; i++) {
        if (driveOpen[i]) {
            driveIO[i]->iotd_Req.io_Command = CMD_STOP;
            DoIO((struct IORequest *)driveIO[i]);
        }
    }
}

void startAllDrives(void) {
    int i;
    for (i = 0; i < NUM_DRIVES; i++) {
        if (driveOpen[i]) {
            driveIO[i]->iotd_Req.io_Command = CMD_START;
            DoIO((struct IORequest *)driveIO[i]);
        }
    }
}

void closeAllDrives(void) {
    int i;
    for (i = 0; i < NUM_DRIVES; i++) {
        if (driveOpen[i]) {
            CloseDevice((struct IORequest *)driveIO[i]);
            driveOpen[i] = FALSE;
        }
        if (driveIO[i])   { DeleteIORequest((struct IORequest *)driveIO[i]); driveIO[i] = NULL; }
        if (drivePort[i]) { DeleteMsgPort(drivePort[i]); drivePort[i] = NULL; }
    }
}

int main(int argc, char **argv) {
    int   i;
    LONG  err = 0;
    BPTR  fh  = NULL;
    UBYTE *audioData = NULL;

    SysBase = *((struct ExecBase **)4);
    DOSBase = (struct DosLibrary *)OpenLibrary("dos.library", 0);
    if (!DOSBase) return 1;

    for (i = 0; i < NUM_DRIVES; i++) {
        drivePort[i] = NULL;
        driveIO[i]   = NULL;
        driveOpen[i] = FALSE;
    }

    // Open and CMD_STOP all drives 
    for (i = 0; i < NUM_DRIVES; i++) {
        drivePort[i] = CreateMsgPort();
        if (!drivePort[i]) { Printf("Can't create port %ld\n", (LONG)i); continue; }

        driveIO[i] = (struct IOExtTD *)CreateIORequest(drivePort[i], sizeof(struct IOExtTD));
        if (!driveIO[i]) { Printf("Can't create IO %ld\n", (LONG)i); continue; }

        if (OpenDevice(TD_NAME, i, (struct IORequest *)driveIO[i], 0) == 0) {
            driveOpen[i] = TRUE;
            Printf("Opened DF%ld\n", (LONG)i);
        } else {
            Printf("DF%ld not available\n", (LONG)i);
        }
    }

    // Load audio file 
    fh = Open("8bitpcm.raw", MODE_OLDFILE);
    if (!fh) { Printf("Can't open file\n"); err = 1; goto doneit; }

    LONG fileSize = 1024 * 1024;
    audioData = AllocMem(fileSize, MEMF_CHIP | MEMF_CLEAR);
    if (!audioData) { Printf("Can't alloc chip RAM\n"); err = 2; goto doneit; }

    Read(fh, audioData, fileSize);
    Close(fh); fh = NULL;

    // Pre-encode sync bits 
    UWORD *wordBuf  = (UWORD *)audioData;
    LONG   wordCount = fileSize / 2;
    for (i = 0; i < wordCount; i++)
        wordBuf[i] = wordBuf[i] | 0x0101;

    // Boost priority and stop all drives 
    struct Task *myTask = FindTask(NULL);
    BYTE oldPri = SetTaskPri(myTask, 1);
    stopAllDrives();

    // Stream direct to hardware 
    UBYTE *ptr = audioData;
    UBYTE *end = audioData + fileSize;

    // Reset flags
    Printf("Reset CIA\n");
    CIABPRB = 0xFF;
    // Short delay for drive to settle
    for (volatile int i = 0; i < 1000; i++);

    // Enable motor
    Printf("Reset Enable Motor Line\n");
    // Short delay for drive to settle 
    for (volatile int i = 0; i < 1000; i++);

    // Select DF1
    Printf("Select DF1\n");
    CIABPRB = 0xFF ^ (CIAF_DSKSEL1 );
    // Short delay for drive to settle 
    for (volatile int i = 0; i < 1000; i++);


    Printf("Playback starting\n");
    DSKLEN = 0x4000;
    DMACON = DMAF_SETCLR | DMAF_MASTER | DMAF_DISK;
    DSKLEN = 0x4000;

    WORD first = 1;
    while (ptr < end) {
        LONG remaining = end - ptr;
        LONG chunkSize = (remaining > CHUNK_SIZE) ? CHUNK_SIZE : remaining;
        UWORD dsklen   = (chunkSize / 2) | 0xC000;
        ULONG dskptr   = ((ULONG)ptr) & 0xFFFFFFFE;

        // Wait for previous transfer to finish fetching from RAM 
        if (!first) {
            while (!(INTREQR & INTF_DSKBLK));
            INTREQ = INTF_DSKBLK;
        } else first = 0;

        // Immediately reload 
        DSKPTH = (UWORD)(dskptr >> 16);
        DSKPTL = (UWORD)(dskptr & 0xFFFF);
        DSKLEN = dsklen;
        DSKLEN = dsklen;

        ptr += chunkSize;
    }
     
	// Short delay for drive to settle 
    for (volatile int i = 0; i < 1000; i++);

    CIABPRB = 0x7F;
	// Short delay for drive to settle
    for (volatile int i = 0; i < 1000; i++);

    CIABPRB = 0xFF;

    // Restore
    startAllDrives();
    SetTaskPri(myTask, oldPri);

    Printf("Done.\n");

doneit:
    if (fh)        Close(fh);
    if (audioData) FreeMem(audioData, fileSize);
    closeAllDrives();
    if (DOSBase)   CloseLibrary((struct Library *)DOSBase);
    return err;
}

__attribute__((used)) __attribute__((section(".text.unlikely"))) void _start(int argc, char **argv)
{
	main(argc,argv);
}