#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>

#include <sys/time.h>

#include "device.h"

uint16_t ram[0x10000];
uint16_t ra, rb, rc, rx, ry, rz, ri, rj;
uint16_t rpc, rsp, rex, ria;
uint16_t lit_a, lit_b; /* temporary storage for literal operands */

bool skip_next;
uint64_t ticks;
bool running;
bool trace;
bool intq_en;
#define MAX_INTQ_SIZE 256
uint16_t intq[MAX_INTQ_SIZE];
unsigned int intq_size;
uint8_t intq_head;
uint16_t hwn;
struct timeval time;

extern void dumpregs();

void intq_push(uint16_t v)
{
    if (ria == 0)
        return;
    if (intq_size < MAX_INTQ_SIZE) {
        intq[intq_head] = v;
        intq_head = (intq_head + 1) % MAX_INTQ_SIZE;
        intq_size++;
    } else {
        printf("Halt and catch fire!\n");
        running = false;
    }
}

uint16_t intq_pop()
{
    if (intq_size > 0) {
        return intq[(intq_head+MAX_INTQ_SIZE-intq_size--) % MAX_INTQ_SIZE];
    } else
        return 0xffff;
}

void reset()
{
    ra = rb = rc = rx = ry = rz = ri = rj = 0;
    rpc = rsp = rex = ria = 0;

    intq_en = false;
    intq_size = 0;
    intq_head = 0;

    skip_next = false;
    ticks = 0;

    trace = false;
    running = true;

    for (hwn = 0; iodevs[hwn] != NULL; hwn++) {
        printf("HWI %d: V:%08x P:%08x R:%04x %s\n",
                hwn, iodevs[hwn]->vendor, iodevs[hwn]->product,
                iodevs[hwn]->version, iodevs[hwn]->name);
        if (iodevs[hwn]->init)
            iodevs[hwn]->init();
    }

    printf("Initialized %d devices\n", hwn);
}

void shutdown()
{
    int i;
    for (i = 0; i < hwn; i++)
        if (iodevs[i]->free)
            iodevs[i]->free();
}

uint16_t *val(int operand, bool is_a)
{
    uint16_t *lit;

    lit = (is_a ? &lit_a : &lit_b);

    switch (operand) {
    case 0x00:
        return &ra;
    case 0x01:
        return &rb;
    case 0x02:
        return &rc;
    case 0x03:
        return &rx;
    case 0x04:
        return &ry;
    case 0x05:
        return &rz;
    case 0x06:
        return &ri;
    case 0x07:
        return &rj;
    case 0x08:
        return &ram[ra];
    case 0x09:
        return &ram[rb];
    case 0x0a:
        return &ram[rc];
    case 0x0b:
        return &ram[rx];
    case 0x0c:
        return &ram[ry];
    case 0x0d:
        return &ram[rz];
    case 0x0e:
        return &ram[ri];
    case 0x0f:
        return &ram[rj];
    case 0x10:
        ticks++;
        return &ram[(ra+ram[rpc++])&0xffff];
    case 0x11:
        ticks++;
        return &ram[(rb+ram[rpc++])&0xffff];
    case 0x12:
        ticks++;
        return &ram[(rc+ram[rpc++])&0xffff];
    case 0x13:
        ticks++;
        return &ram[(rx+ram[rpc++])&0xffff];
    case 0x14:
        ticks++;
        return &ram[(ry+ram[rpc++])&0xffff];
    case 0x15:
        ticks++;
        return &ram[(rz+ram[rpc++])&0xffff];
    case 0x16:
        ticks++;
        return &ram[(ri+ram[rpc++])&0xffff];
    case 0x17:
        ticks++;
        return &ram[(rj+ram[rpc++])&0xffff];
    case 0x18:
        return (is_a ? &ram[(skip_next?rsp:rsp++)] : &ram[(skip_next?rsp-1:--rsp)]);
    case 0x19:
        return &ram[rsp];
    case 0x1a:
        ticks++;
        return &ram[(rsp+ram[rpc++])&0xffff];
    case 0x1b:
        return &rsp;
    case 0x1c:
        return &rpc;
    case 0x1d:
        return &rex;
    case 0x1e:
        ticks++;
        return &ram[ram[rpc++]];
    case 0x1f:
        ticks++;
        *lit = ram[rpc++];
        return lit;
    default:
        *lit = operand - 0x21;
        return lit;
    }
}

void oNOP(uint16_t *pa, uint16_t *pb)
{
}

void oSET(uint16_t *a, uint16_t *b)
{
    *b = *a;
}

void oADD(uint16_t *a, uint16_t *b)
{
    uint16_t pre = *b;

    *b += *a;
    rex = (pre>*b ? 0x0001 : 0x0000);
    ticks++;
}

void oSUB(uint16_t *a, uint16_t *b)
{
    uint32_t t;

    t = *b - *a;
    *b = t;
    rex = t >> 16;
    ticks++;
}

void oMUL(uint16_t *a, uint16_t *b)
{
    uint32_t res = *b * *a;

    *b = res & 0xffff;
    rex = (res>>16) & 0xffff;
    ticks++;
}

void oMLI(uint16_t *a, uint16_t *b)
{
    int32_t res = (int16_t)*a * (int16_t)*b;

    *b = (uint32_t)res & 0xffff;
    rex = ((uint32_t)res>>16) & 0xffff;
    ticks++;
}

void oDIV(uint16_t *a, uint16_t *b)
{
    uint32_t wb, wt;

    if (*a) {
        wb = *b;
        wt = (wb<<16) / *a;
        *b = wt>>16;
        rex = wt&0xffff;
    } else {
        *b = rex = 0;
    }
}

void oDVI(uint16_t *a, uint16_t *b)
{
    int32_t wa, wb, wt;

    if (*a) {
        wa = (*a << 16) >> 16;
        wb = (*b << 16) >> 16;
        wt = (wb<<16) / wa;
        *b = wt>>16;
        rex = wt&0xffff;
    } else {
        *b = rex = 0;
    }
}

void oMOD(uint16_t *a, uint16_t *b)
{
    if (*a) {
        *b %= *a;
    } else {
        *b = 0;
    }
}

void oMDI(uint16_t *a, uint16_t *b)
{
    if (*a) {
        *b = (int16_t)*b % (int16_t)*a;
    } else {
        *b = 0;
    }
}

void oAND(uint16_t *a, uint16_t *b)
{
    *b &= *a;
}

void oBOR(uint16_t *a, uint16_t *b)
{
    *b |= *a;
}

void oXOR(uint16_t *a, uint16_t *b)
{
    *b ^= *a;
}

void oSHR(uint16_t *a, uint16_t *b)
{
    uint32_t t;

    t = (((uint32_t)*b) << 16) >> *a;
    *b = t >> 16;
    rex = t;
}

void oASR(uint16_t *a, uint16_t *b)
{
    int32_t t;

    t = (((int32_t)*b) << 16) >> *a;
    *b = t >> 16;
    rex = t;
}

void oSHL(uint16_t *a, uint16_t *b)
{
    uint32_t t;

    t = ((uint32_t)*b) << *a;
    *b = t;
    rex = t >> 16;
}

#define DOIF(c) if (!(c)) { skip_next = true; ticks++; }

void oIFB(uint16_t *a, uint16_t *b)
{
    DOIF((*b & *a) != 0);
}

void oIFC(uint16_t *a, uint16_t *b)
{
    DOIF((*b & *a) == 0);
}

void oIFE(uint16_t *a, uint16_t *b)
{
    DOIF(*b == *a);
}

void oIFN(uint16_t *a, uint16_t *b)
{
    DOIF(*b != *a);
}

void oIFG(uint16_t *a, uint16_t *b)
{
    DOIF(*b > *a);
}

void oIFA(uint16_t *a, uint16_t *b)
{
    DOIF((int16_t)*b > (int16_t)*a);
}

void oIFL(uint16_t *a, uint16_t *b)
{
    DOIF(*b < *a);
}

void oIFU(uint16_t *a, uint16_t *b)
{
    DOIF((int16_t)*b < (int16_t)*a);
}

void oADX(uint16_t *a, uint16_t *b)
{
    uint16_t t = *b;

    *b += *a + rex;
    rex = (t>*b ? 0x0001 : 0x0000);
    ticks += 2;
}

void oSBX(uint16_t *a, uint16_t *b)
{
    uint16_t t = *b;

    *b -= *a - rex;
    rex = (t<*b ? 0xffff : 0x0000);
    ticks += 2;
}

void oSTI(uint16_t *a, uint16_t *b)
{
    *b = *a;
    ri++;
    rj++;
    ticks++;
}

void oSTD(uint16_t *a, uint16_t *b)
{
    *b = *a;
    ri--;
    rj--;
    ticks++;
}

void oJSR(uint16_t *a, uint16_t *b)
{
    ram[--rsp] = rpc;
    rpc = *a;
}

void oINT(uint16_t *a, uint16_t *b)
{
    intq_push(*a);
    ticks += 3;
}

void oIAG(uint16_t *a, uint16_t *b)
{
    *a = ria;
}

void oIAS(uint16_t *a, uint16_t *b)
{
    ria = *a;
    if (ria == 0)
        intq_size = 0;
}

void oRFI(uint16_t *a, uint16_t *b)
{
    intq_en = false;
    ra = ram[rsp++];
    rpc = ram[rsp++];
}

void oIAQ(uint16_t *a, uint16_t *b)
{
    intq_en = (*a != 0);
}

void oHWN(uint16_t *a, uint16_t *b)
{
    *a = hwn;
}

void oHWQ(uint16_t *a, uint16_t *b)
{
    uint16_t idx = *a;

    if (idx < hwn) {
        ra = iodevs[idx]->product & 0xffff;
        rb = (iodevs[idx]->product >> 16) & 0xffff;
        rc = iodevs[idx]->version;
        rx = iodevs[idx]->vendor & 0xffff;
        ry = (iodevs[idx]->vendor >> 16) & 0xffff;
    } else {
        ra = rb = rc = rx = ry = 0;
    }
}

void oHWI(uint16_t *a, uint16_t *b)
{
    uint16_t idx = *a;

    if (idx < hwn) {
        if (iodevs[idx]->irqh)
            iodevs[idx]->irqh();
    }
}

typedef void (*op_t)(uint16_t *, uint16_t *);

const op_t ops[] = {
    NULL, oSET, oADD, oSUB, oMUL, oMLI, oDIV, oDVI,     /* 00-07 */
    oMOD, oMDI, oAND, oBOR, oXOR, oSHR, oASR, oSHL,     /* 08-0f */
    oIFB, oIFC, oIFE, oIFN, oIFG, oIFA, oIFL, oIFU,     /* 10-17 */
    NULL, NULL, oADX, oSBX, NULL, NULL, oSTI, oSTD      /* 18-1f */
};

const op_t sops[] = {
    NULL, oJSR, NULL, NULL, NULL, NULL, NULL, NULL,     /* 00-07 */
    oINT, oIAG, oIAS, oRFI, oIAQ, NULL, NULL, NULL,     /* 08-0f */
    oHWN, oHWQ, oHWI, NULL, NULL, NULL, NULL, NULL,     /* 10-17 */
    NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL      /* 18-1f */
};

void next()
{
    uint16_t ir;
    int opcode, a, b;
    uint16_t *pa, *pb;
    op_t f;
    uint16_t i;

    gettimeofday(&time, NULL);

    // if ((!intq_en) && (intq_size > 0) && (!skip_next)) {
    //     i = intq_pop();
    //     if (ria != 0) {
    //         printf("Firing interrupt with message 0x%04x\n", i);
    //         intq_en = true;
    //         ram[--rsp] = rpc;
    //         ram[--rsp] = ra;
    //         rpc = ria;
    //         ra = i;
    //     }
    // }
    if ((!intq_en) && (intq_size > 0) && (!skip_next)) {
        i = intq_pop();
        if (ria != 0) {
            //printf("Firing interrupt with message 0x%04x\n", i);
            intq_en = true;
            ram[--rsp] = rpc;
            ram[--rsp] = ra;
            rpc = ria;
            ra = i;
        }
    }
    /*
        if (trace)
            dumpregs();
    */
    // printf("PC: %04x\n", rpc);
    // dumpregs();

    ir = ram[rpc++];

    opcode = ir & 0x001f;
    a = (ir >> 10) & 0x003f;
    b = (ir >> 5) & 0x001f;

    if (opcode == 0) { /* special instruction */
        f = sops[b];
        pa = val(a, true);
        pb = NULL;
    } else {
        f = ops[opcode];
        pa = val(a, true);
        pb = val(b, false);
    }

    if (f == NULL) {
        printf("Invalid opcode 0x%04x at 0x%04x (o=%02x, a=%02x, b=%02x)\n", ir, rpc-1, opcode, a, b);
        running = false;
        sleep(5);
        return;
    }

    if (rpc == 0) {
        printf("CRASH!\n");
    }

    if (!skip_next)
        f(pa, pb);
    else
        skip_next = ((opcode & 0x18) == 0x10);  /* Skip chained conditionals */

    if (rpc == 0) {
        sleep(5);
        running = false;
    }
        
    for (i = 0; iodevs[i] != NULL; i++)
        if (iodevs[i]->tick)
            iodevs[i]->tick();

    ticks++;
}

int load_image(char *filename)
{
    int fd;
    ssize_t r;
    ssize_t pos;
    uint8_t *buf;

    printf("Loading %s ...\n", filename);
    fd = open(filename, O_RDONLY);
    if (fd < 0)
        return fd;
    buf = (uint8_t *)ram;
    for (pos=0;;) {
        r = read(fd, &buf[pos], sizeof(ram)-pos);
        if (r > 0)
            pos += r;
        else {
            close(fd);
            if (r == 0)
                return pos;
            else
                return r;
        }
    }
}

int main(int argc, char *argv[])
{
    if (argc > 1) {
        load_image(argv[1]);
        reset();
        while (running) next();
        shutdown();
    }
    return EXIT_SUCCESS;
}

/* vim: set et sw=4 ts=4 sts=4: */