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

uint16_t ram[0x10000];
uint16_t ra, rb, rc, rx, ry, rz, ri, rj;
uint16_t rpc, rsp, rex, ria;
bool skip_next;
uint64_t ticks;
bool running;
bool intq_en;
#define MAX_INTQ_SIZE 256
uint16_t intq[MAX_INTQ_SIZE];
unsigned int intq_size;
uint8_t intq_head;

void intq_push(uint16_t v)
{
    if (intq_size < MAX_INTQ_SIZE) {
        intq[intq_head] = v;
        intq_head = (intq_head + 1) % MAX_INTQ_SIZE;
        intq_size++;
    }
}

uint16_t intq_pop()
{
    if (intq_size > 0) {
        return intq[(intq_head-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;

    running = true;
}

typedef void (*dev_t)(void);

struct dev_entry {
    uint32_t vendor;
    uint32_t product;
    uint16_t version;
    void (*irqh)();
    void (*init)();
    void (*free)();
    void (*tick)();
};

struct dev_entry iodevs[] = {
    { 0, 0, 0, NULL, NULL, NULL, NULL}
};

uint16_t lit[] = {
    0xffff, 0, 1, 2, 3, 4, 5, 6,
    7, 8, 9, 10, 11, 12, 13, 14,
    15, 16, 17, 18, 19, 20, 21, 22,
    23, 24, 25, 26, 27, 28, 29, 30
};

uint16_t *val(int operand, bool is_a)
{
    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++]];
    case 0x11:
        ticks++;
        return &ram[rb+ram[rpc++]];
    case 0x12:
        ticks++;
        return &ram[rc+ram[rpc++]];
    case 0x13:
        ticks++;
        return &ram[rx+ram[rpc++]];
    case 0x14:
        ticks++;
        return &ram[ry+ram[rpc++]];
    case 0x15:
        ticks++;
        return &ram[rz+ram[rpc++]];
    case 0x16:
        ticks++;
        return &ram[ri+ram[rpc++]];
    case 0x17:
        ticks++;
        return &ram[rj+ram[rpc++]];
    case 0x18:
        return (is_a ? &ram[rsp++] : &ram[--rsp]);
    case 0x19:
        return &ram[rsp];
    case 0x1a:
        ticks++;
        return &ram[rsp+ram[rpc++]];
    case 0x1b:
        return &rsp;
    case 0x1c:
        return &rpc;
    case 0x1d:
        return &rex;
    case 0x1e:
        ticks++;
        return &ram[ram[rpc++]];
    case 0x1f:
        ticks++;
        return &ram[rpc++]; /* FIXME: write to literal */
    default:
        return &lit[operand-0x20];
    }
}

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)
{
    uint16_t pre = *b;

    *b -= *a;
    rex = (pre<*b ? 0xffff : 0x0000);
    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)
{
    if (*a) {
        rex = ((uint32_t)*b<<16) / *a;
        *b /= *a;
    } else {
        *b = rex = 0;
    }
}

void oDVI(uint16_t *a, uint16_t *b)
{
    if (*a) {
        rex = ((int32_t)*b<<16) / (int16_t)*a;
        *b = (int16_t)*b / (int16_t)*a;
    } 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)
{
    uint16_t t;

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

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

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

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

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

#define DOIF(c) if (!(c)) skip_next = true

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;
}

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 = sizeof(iodevs) / sizeof(struct dev_entry);
}

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

    if (idx < sizeof(iodevs) / sizeof(struct dev_entry)) {
        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 < sizeof(iodevs) / sizeof(struct dev_entry)) {
        if (iodevs[idx].irqh)
            iodevs[idx].irqh();
    }
}

void oHLT(uint16_t *a, uint16_t *b)
{
    running = false;
}

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

const op_t ops[] = {
    oNOP, 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 */
    oNOP, oNOP, oADX, oSBX, oNOP, oNOP, oSTI, oSTD      /* 18-1f */
};

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

void dumpregs()
{
    printf("%4s %4s %4s %4s %4s %4s %4s %4s %4s %4s %4s %4s\n",
           "PC","SP","A","B","C","X","Y","Z","I","J","EX","IA");
    printf("%04hx %04hx %04hx %04hx %04hx %04hx %04hx %04hx %04hx %04hx %04hx %04hx\n",
           rpc, rsp, ra, rb, rc, rx, ry, rz, ri, rj, rex, ria);
}

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

    if ((!intq_en) && (intq_size > 0)) {
        i = intq_pop();
        if (ria != 0) {
            intq_en = true;
            ram[--rsp] = rpc;
            ram[--rsp] = ra;
            rpc = ria;
            ra = i;
        }
    }

    dumpregs();

    ir = ram[rpc++];

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

    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 (!skip_next)
        f(pa, pb);
    else
        skip_next = false;

    ticks++;
}

int main()
{
    reset();
    while (running) next();
    return EXIT_SUCCESS;
}