#include <exec/types.h>
#include <exec/devices.h>
#include <exec/io.h>
#include <exec/memory.h>
#include <exec/ports.h>

#include <devices/timer.h>

#include <proto/exec.h>
#include <proto/timer.h>
#include <proto/dos.h>
#include <proto/intuition.h>
#include <proto/expansion.h>

#include <clib/alib_protos.h>
#include <clib/timer_protos.h>
#include <clib/exec_protos.h>

#include <devices/scsidisk.h>
#include <dos/dosextens.h>

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>

struct Device* TimerBase;
static struct IORequest timereq;

bool DoDevIO(struct IOStdReq *ior, LONG cmd, ULONG off, ULONG len, WORD *data)
{
    ior->io_Command = cmd;
    ior->io_Offset = off;
    ior->io_Length = len;
    ior->io_Data   =  data;
    DoIO((struct IORequest *)ior);
    bool result = ior->io_Error == 0L;

    if (!result)
    {
        printf("DoDevIO() ERROR!!! io_Error= %ld\r\n", (long)ior->io_Error);
    }

    return result;
}

long DevCmd(struct IOStdReq *ior, LONG cmd, LONG len)
{
    ior->io_Command = cmd;
    ior->io_Offset = 0L;
    ior->io_Length = len;
    ior->io_Data   =  0L;
    ior->io_Actual = 0L;

    DoIO((struct IORequest *)ior);

    return ior->io_Actual;
}

static ULONG timer()
{
  struct timeval a;
  GetSysTime(&a);

  return a.tv_secs*1000 + a.tv_micro/1000;
}

#define STRIDE 16

void printbuffer(UBYTE *data, int len)
{
    int count = 0;
    char row[STRIDE+1];
    row[STRIDE] = 0;

    while (count < len)
    {
        for (int i =0; i < STRIDE; i++)
        {
            UBYTE v = data[count+i];
            row[i] = (v < 32) || (v > 128) ? '.' : (char) v;
            printf("%02x", v);
        }
        count += STRIDE;
        printf("%s\r\n", row);
    }
}

#define KB 1024
#define MB KB*KB
#define BUFSIZE 128*KB
#define MEGS_TO_READ 100

int main (int argc, char ** argv)
{
    struct MsgPort *SCSIMP;
    struct IOStdReq *SCSIReq;        /* a standard IOStdReq structure */
    struct SCSICmd  Cmd;             /* where the actual SCSI command goes */

    char* deviceName = "scsi.device";
    int unit = 0;

    printf("TF Dumbass Drive Speed Tool\n");
    printf("(C) 2019 S.J Leary\n");

    printf(" - opening message port\n");
    if (!(SCSIMP = CreateMsgPort())) {
        printf("Couldn't create message port\n");
        exit(101);
    }

        /* Create IORequest */
    if (!(SCSIReq = (struct IOStdReq *)CreateExtIO(SCSIMP,sizeof(struct IOStdReq)))) {
        printf("Can't create IORequest: %d\n",RETURN_FAIL);
        exit(102);
    }

    printf(" - opening the device\n");
    if (OpenDevice(deviceName, unit, (struct IORequest *)SCSIReq, 0))
    {
        printf("Couldn't open unit %ld on %s\n", (long)unit, deviceName);
        exit(100);
    }

    printf(" - allocating the buffer\n");
    UWORD *buffer = (UWORD *) AllocMem(BUFSIZE, MEMF_PUBLIC|MEMF_CLEAR);

    printf(" - initializing the timer\n");
    OpenDevice(TIMERNAME, UNIT_MICROHZ, &timereq, 0);
    TimerBase = timereq.io_Device;

    DevCmd(SCSIReq, CMD_START, 0 );


    printf(" - Reading %iMb of data... ", MEGS_TO_READ);
    ULONG startTime = timer();

    //Disable();

    // bloody thing locks up
    for (int i = 0; i < 100; i++)
    {
        printf("Reading a sector %i... ", i);
        if (DoDevIO(SCSIReq, CMD_READ, i*512, 1024, buffer))
        {
            printf("Done.\r\n");
            printbuffer((UBYTE*)buffer, 512);
        }
    }



    //Enable();

    ULONG endTime = timer();
    printf("Done.\n");

    DevCmd(SCSIReq, CMD_STOP, 0 );

    double timeTaken = ((double) endTime - (double) startTime) / 1000.0;
    printf("Total time taken in seconds: %2.2f\n", timeTaken);

    double readRate = (double) MEGS_TO_READ / timeTaken;
    printf("Read data rate is: %2.2fMb/sec\n", readRate);

    CloseDevice( &timereq );
    DeleteMsgPort( SCSIMP );

    CloseDevice( (struct IORequest *)SCSIReq );
    FreeMem(buffer, BUFSIZE);

    return 0;

}