include "inc/cmdsys.plh"
include "inc/fileio.plh"
include "inc/args.plh"
include "inc/sdfat.plh"

const COPY_BUF_SIZE = 4096 // 4K
const COPY_BLK_SIZE = COPY_BUF_SIZE/512
const COPY_BLK_CNT  = COPY_BLK_SIZE-1
const DRIVE1 = $60 // drive 1, slot 6
const DRIVE2 = $D0 // drive 2, slot 6
const LOWER_DIFF = 'a' - 'A'
//
// Track sector order
//
const ORDER_DOS = 0
const ORDER_PRODOS = 1

word arg, image
byte unit = DRIVE1
byte order
//
// DOS to ProDOS sector ordering
//
byte[] secOrder = $0,$E,$D,$C,$B,$A,$9,$8,$7,$6,$5,$4,$3,$2,$1,$F

def putb(b)#0
    byte c
    c = ((b >> 4) & $0F) + '0'
    if c > '9'
        c = c + 7
    fin
    putc(c)
    c = (b & $0F) + '0'
    if c > '9'
        c = c + 7
    fin
    putc(c)
end

def getYN(prompt)
    byte yn

    puts(prompt)
    yn = getc
    putln
    return yn == 'Y' or yn == 'y'
end

def trkSecToBlk(bufSec, bufBlk)#0
    byte sector

    for sector = 0 to 15
        memcpy(bufBlk + (sector << 8), bufSec + (secOrder[sector] << 8), 256)
    next
end

def bigFatWrite(buf, len)
    word xferLen, fatLen

    xferLen = 0
    repeat
        if len > MAX_FAT_BUF_SIZE
            fatLen = MAX_FAT_BUF_SIZE
        else
            fatLen = len
        fin
        fatLen = sdFAT:writeFile(buf, fatLen)
        if fatLen > 0
            xferLen = xferLen + fatLen
            len     = len     - fatLen
            buf     = buf     + fatLen
        else
            len = 0
        fin
    until len == 0
    return xferLen
end

def fatReadImage(src, drv, order)
    word inBuf, outBuf, copyLen, freeAddr
    word blocknum, bufblk

    inBuf = heapallocalign(COPY_BUF_SIZE * 2, 8, @freeAddr)
    if not inBuf
        puts("Not enough free memory!\n"); putln
        return -1
    fin
    if order == ORDER_DOS
        outBuf = inBuf + COPY_BUF_SIZE
    else
        outBuf = inBuf
    fin
    //
    // Copy FAT image over one track at a time
    //
    if sdFAT:openFile(src, O_READ | O_WRITE | O_CREAT)
        if !drv; putc(7); fin
        for blocknum = 0 to 279 step COPY_BLK_SIZE
            if drv
                ^$24=^$20 // Move cursor to left edge
                puts("Reading blocks: "); puti(blocknum)
                    for bufblk = 0 to COPY_BLK_CNT
                        if fileio:readblock(drv, inBuf + (bufblk << 9), blocknum + bufblk)
                            puts("Read disk error: $"); putb(perr); putln
                            break
                        fin
                    next
            fin
            if order == ORDER_DOS
                trkSecToBlk(inBuf, outBuf)
            fin
            copyLen = bigFatWrite(outBuf, COPY_BUF_SIZE)
            if copyLen <> COPY_BUF_SIZE
                puts("Write image file error\n");
            fin
            if copyLen < COPY_BUF_SIZE or perr
                puts("Error creating image: "); puts(src); putln
                break
            fin
        next
        if !drv; putc(7);fin
        putln
        sdFAT:closeFile()
    else
        puts("Error opening image file:"); puts(src); putln
        return -1
    fin
    heaprelease(freeAddr)
    return 0
end

arg = argNext(argFirst)
if ^arg
    image = arg
    arg = argNext(arg)
    if ^arg
        when ^(arg + 1)
            is '2' // Drive 2 option
                unit = DRIVE2
                break
            is 'T' // Test speed option
            is 't'
                unit = 0
                break
        wend
    fin
    if unit
        puts("Reading "); puts(image); puts(" from Drive "); putc((unit >> 7) + '1')
    else
        puts("Speed test writing 143K to SD card");
    fin
    //
    // Figure sector ordering from filename
    //
    puts("\nUsing ")
    if toupper(^(image + ^image)) == 'O' and toupper(^(image + ^image - 1)) == 'P'
        order = ORDER_PRODOS
        puts("ProDOS")
    else
        order = ORDER_DOS
        puts("DOS 3.3")
    fin
    if getYN(" order. Continue? (Y/N)")
        fatReadImage(image, unit, order)
    fin
else
    puts("Read DSK image from floppy disk drive\n")
    puts("Usage: +FATREADDSK <DSK image file> [1,2,T]\n")
fin
done
