#include <stdio.h>
#include "xyw.h"

#define MEMORY_SIZE 0xffff
#define STACK_SIZE 0xff

#define USER_MEMORY_START 0x000
#define SYSTEM_MEMORY_START 0xfd00
#define USER_STACK_START 0xfd00
#define SYSTEM_STACK_START 0xfe00
#define DEVICE_AREA_START 0xff00

typedef enum
{
    XYW_ERROR_NONE = 0,
    XYW_ERROR_INVALID_INSTRUCTION,
    XYW_ERROR_STACK_UNDERFLOW,
    XYW_ERROR_STACK_OVERFLOW,
    XYW_ERROR_DEVICE_ERROR
} xyw_error;

/* system device */
#define SYSTEM_STATE 0xff00
#define SYSTEM_ERROR 0xff01
#define SYSTEM_S 0xff02
#define SYSTEM_U 0xff03
#define SYSTEM_PAGE 0xff04
#define SYSTEM_ON_ERROR 0xff05

/* console device */
#define CONSOLE_BASE 0xff10
#define CONSOLE_INPUT 0xff10
#define CONSOLE_OUTPUT 0xff11
#define CONSOLE_ERROR 0xff12
#define CONSOLE_ON_KEYPRESS 0xff13

/// Main memory and stacks
xyw_byte xyw_memory[MEMORY_SIZE];
xyw_byte system_stack[STACK_SIZE];
xyw_byte user_stack[STACK_SIZE];

#define DIRECT_PAGE (xyw_memory[SYSTEM_PAGE] << 8)

//// 1-byte pointers and registers
xyw_byte *s = &xyw_memory[SYSTEM_S];
xyw_byte *u = &xyw_memory[SYSTEM_U];
static xyw_byte x_val = 0; // X register storage
static xyw_byte y_val = 0; // Y register storage
xyw_byte *x = &x_val;
xyw_byte *y = &y_val;

//// 2-byte registers
static xyw_word pc_storage = 0; // Program counter storage
static xyw_word xw_storage = 0; // 16-bit X register storage
static xyw_word yw_storage = 0; // 16-bit Y register storage
xyw_word *pc = &pc_storage;
xyw_word *xw = &xw_storage;
xyw_word *yw = &yw_storage;

//// Helpers to manage 2-byte values to memory (big-endian)

static inline xyw_word mget(xyw_word addr)
{
    return ((xyw_word)xyw_memory[addr] << 8) | xyw_memory[addr + 1];
}

static inline void mset(xyw_word addr, xyw_word val)
{
    xyw_memory[addr] = (val >> 8) & 0xFF;
    xyw_memory[addr + 1] = val & 0xFF;
}

//// System stack management

static xyw_word ss_popw()
{
    if (*s < 2)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_UNDERFLOW;
        return -1;
    }
    xyw_word val = mget(SYSTEM_STACK_START + (*s) - 2);
    mset(SYSTEM_STACK_START + (*s) - 2, 0);
    (*s) -= 2;
    return val;
}

static void ss_pushw(xyw_word val)
{
    if (*s > STACK_SIZE - 2)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_OVERFLOW;
        return;
    }
    mset(SYSTEM_STACK_START + *s, val);
    (*s) += 2;
}

//// User stack management

static void us_push(xyw_byte val)
{
    if (*u > STACK_SIZE - 1)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_OVERFLOW;
        return;
    }
    xyw_dbg(" => %02X\n", val);
    xyw_memory[USER_STACK_START + (*u)++] = val;
}

static void us_pushw(xyw_word val)
{
    if (*u > STACK_SIZE - 2)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_OVERFLOW;
        return;
    }
    mset(USER_STACK_START + *u, val);
    xyw_dbg(" => %04X\n", val);
    (*u) += 2;
}

static xyw_byte us_pop()
{
    if (*u < 1)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_UNDERFLOW;
        return -1;
    }
    (*u) -= 1;
    xyw_byte val = xyw_memory[USER_STACK_START + (*u)];
    xyw_memory[USER_STACK_START + (*u)] = 0;
    return val;
}

static xyw_word us_popw()
{
    if (*u < 2)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_UNDERFLOW;
        return -1;
    }
    xyw_word val = mget(USER_STACK_START + (*u) - 2);
    mset(USER_STACK_START + (*u) - 2, 0);
    (*u) -= 2;
    return val;
}

// clang-format off

//// Operations that support stack and all registers
#define OPC_SR2(opc, body) { \
    case opc: { const xyw_byte w = 0, a = us_pop(), b = us_pop(); body; } break; \
    case XYW_MODE_X|opc: { const xyw_byte w = 0, a = *x, b = us_pop(); body; } break; \
    case XYW_MODE_Y|opc: { const xyw_byte w = 0, a = *y, b = us_pop(); body; } break; \
    case XYW_MODE_X|XYW_MODE_Y|opc: { const xyw_byte w = 0, a = *x, b = *y; body; } break; \
    case XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = us_popw(); xyw_word b = us_popw(); body; } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = *xw; xyw_word b = us_popw(); body; } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = *yw; xyw_word b = us_popw(); body; } break; \
    case XYW_MODE_X|XYW_MODE_Y|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = *xw; xyw_word b = *yw; body; } break; \
}

//// Operations that support stack and one register
#define OPC_SR1(opc, body) { \
    case opc: { const xyw_byte w = 0, a = us_pop(); body; } break; \
    case XYW_MODE_X|opc: { const xyw_byte w = 0, a = *x; body; } break; \
    case XYW_MODE_Y|opc: { const xyw_byte w = 0, a = *y; body; } break; \
    case XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = us_popw(); body; } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = *xw; body; } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = *yw; body; } break; \
}

//// STA
#define OPC_STA(opc, body) { \
    case opc: { const xyw_word a = us_popw(); const xyw_byte w = 0, b = us_pop(); body; } break; \
    case XYW_MODE_X|opc: { const xyw_word a = us_popw(); const xyw_byte w = 0, b = *x; body; } break; \
    case XYW_MODE_Y|opc: { const xyw_word a = us_popw(); const xyw_byte w = 0, b = *y; body; } break; \
    case XYW_MODE_W|opc: { const xyw_word a = us_popw(); const xyw_byte w = 1; xyw_word b = us_popw(); body; } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = us_popw(), b = *xw; body; } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = us_popw(), b = *yw; body; } break; \
}

//// STP
#define OPC_STP(opc, body) { \
    case opc: { const xyw_word a = DIRECT_PAGE | us_pop(); const xyw_byte w = 0, b = us_pop(); body; } break; \
    case XYW_MODE_X|opc: { const xyw_word a = DIRECT_PAGE | us_pop(); const xyw_byte w = 0, b = *x; body; } break; \
    case XYW_MODE_Y|opc: { const xyw_word a = DIRECT_PAGE | us_pop(); const xyw_byte w = 0, b = *y; body; } break; \
    case XYW_MODE_W|opc: { const xyw_word a = DIRECT_PAGE | us_pop(); const xyw_byte w = 1; xyw_word b = us_popw(); body; } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = DIRECT_PAGE | us_pop(), b = *xw; body; } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = DIRECT_PAGE | us_pop(), b = *yw; body; } break; \
}

//// Operations that support one register only and no stack
#define OPC_R1(opc, body) { \
    case XYW_MODE_X|opc: { const xyw_byte w = 0, a = *x; body; } break; \
    case XYW_MODE_Y|opc: { const xyw_byte w = 0, a = *y; body; } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = *xw; body; } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { const xyw_byte w = 1; xyw_word a = *yw; body; } break; \
}

//// Operations that support stack only
#define OPC_S(opc, body) { \
    case opc: { const xyw_byte w = 0; body; } break; \
    case XYW_MODE_W|opc: { const xyw_byte w = 1; body; } break; \
}

//// POP operations 
#define OPC_POP(opc) { \
    case opc: { us_pop(); } break; \
    case XYW_MODE_X|opc: { *x = us_pop(); } break; \
    case XYW_MODE_Y|opc: { *y = us_pop(); } break; \
    case XYW_MODE_W|opc: { us_pop(); } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { *xw = us_popw(); } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { *yw = us_popw(); } break; \
}

//// PSH operations
#define OPC_PSH(opc) { \
    case opc: { (*pc)++; us_push(xyw_memory[*pc]); } break; \
    case XYW_MODE_X|opc: { us_push(*x); } break; \
    case XYW_MODE_Y|opc: { us_push(*y); } break; \
    case XYW_MODE_W|opc: { (*pc)++; us_pushw(mget(*pc)); (*pc)++; } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { us_pushw(*xw); } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { us_pushw(*yw); } break; \
}
//// Increment/decrement operations
#define OPC_IDC(opc, delta) { \
    case opc: { us_push(us_pop() + delta);} break; \
    case XYW_MODE_X|opc: { *x = *x + delta; } break; \
    case XYW_MODE_Y|opc: { *y = *y + delta; } break; \
    case XYW_MODE_W|opc: { us_pushw(us_popw() + delta); } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { *xw = *xw + delta; } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { *yw = *yw + delta; } break; \
}

//// SWP operations
#define OPC_SWP(opc) { \
    case opc: { xyw_byte a = us_pop(); xyw_byte b = us_pop(); us_push(a); us_push(b); } break; \
    case XYW_MODE_W|opc: { xyw_word a = us_popw(); xyw_word b = us_popw(); us_pushw(a); us_pushw(b); } break; \
    case XYW_MODE_X|opc: { xyw_byte a = us_pop(); xyw_byte b = *x; *x = a; us_push(b); } break; \
    case XYW_MODE_Y|opc: { xyw_byte a = us_pop(); xyw_byte b = *y; *y = a; us_push(b); } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { xyw_word a = us_popw(); xyw_word b = *xw; *xw = a; us_pushw(b); } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { xyw_word a = us_popw(); xyw_word b = *yw; *yw = a; us_pushw(b); } break; \
    case XYW_MODE_X|XYW_MODE_Y|opc: { xyw_byte a = *x; xyw_byte b = *y; *x = b; *y = a; } break; \
    case XYW_MODE_X|XYW_MODE_Y|XYW_MODE_W|opc: { xyw_word a = *xw; xyw_word b = *yw; *xw = b; *yw = a; } break; \
}

//// "Microcode" similar to Uxn's, to be used with above macros
// Push a value to stack
#define PUSH(n) do { if (w) { us_pushw(n); } else { us_push(n); } } while(0)
// Pop value from stack
#define POP() (w ? us_popw() : (xyw_word)us_pop())
// Read value from address
#define READ(addr) (w ? mget(addr) : (xyw_byte)xyw_memory[addr])
// Write value as word or byte depending on width flag 'w'
#define WRITE(addr, val) do { if (w) { mset(addr, val); } else { xyw_memory[addr] = val; } } while(0)

// clang-format on
int run(const char *input)
{

    // Load bytecode into memory
    FILE *fp = fopen(input, "rb");
    if (!fp)
    {
        fprintf(stderr, "Error - Could not open file [%s]\n", input);
        return -1;
    }
    // Check header
    xyw_byte header[6];
    xyw_dbg("Reading header from file [%s]\n", input);
    fread(header, 1, 6, fp);
    if (header[0] != 0x0F || header[1] != 'x' || header[2] != 'y' || header[3] != 'w' || header[4] != 0x00 || header[5] != 0xF0)
    {
        fprintf(stderr, "Error - Invalid file header [%s]\n", input);
        fclose(fp);
        return -1;
    }
    xyw_dbg("File header OK\n");
    // Load the rest of the file into memory
    size_t bytes_read = fread(&xyw_memory[USER_MEMORY_START], 1, MEMORY_SIZE - USER_MEMORY_START, fp);
    if (bytes_read == 0)
    {
        fprintf(stderr, "Error - Could not read file contents [%s]\n", input);
        fclose(fp);
        return -1;
    }
    xyw_dbg("Loaded %zu bytes into memory\n", bytes_read);
    fclose(fp);
    xyw_memory[SYSTEM_STATE] = 1;
    while (xyw_memory[SYSTEM_STATE] && *pc < SYSTEM_MEMORY_START)
    {
        xyw_dbg("PC: %04X -> %02X (%s)\n", *pc, xyw_memory[*pc], xyw_instructions[xyw_memory[*pc] & 0x1F]);
        switch (xyw_memory[*pc])
        {
            // clang-format off
            /* BRK */ case 0x00: break;
            /* PSH */ OPC_PSH(0x01);
            /* POP */ OPC_POP(0x02);
            /* LDR */ OPC_R1(0x03, PUSH(READ(a)));
            /* LDA */ OPC_S(0x04, READ(us_popw()));
            /* STA */ OPC_STA(0x05, WRITE(a, b));
            /* LDP */ OPC_S(0x06, READ(DIRECT_PAGE | us_pop()));
            /* STP */ OPC_STP(0x07, WRITE(a, b));
            /* INC */ OPC_IDC(0x08, +1);
            /* DEC */ OPC_IDC(0x09, -1);
            /* SHL */ OPC_SR2(0x0A, PUSH(b << a));
            /* SHR */ OPC_SR2(0x0B, PUSH(b >> a));
            /* ADD */ OPC_SR2(0x0C, PUSH(a + b));
            /* SUB */ OPC_SR2(0x0D, PUSH(b - a));
            /* MUL */ OPC_SR2(0x0E, PUSH(a * b));
            /* DIV */ OPC_SR2(0x0F, PUSH(b / a));
            /* EQU */ OPC_SR2(0x10, PUSH(b == a));
            /* NEQ */ OPC_SR2(0x11, PUSH(b != a));
            /* GTH */ OPC_SR2(0x12, PUSH(b > a));
            /* LTH */ OPC_SR2(0x13, PUSH(b < a));
            /* AND */ OPC_SR2(0x14, PUSH(b & a));
            /* IOR */ OPC_SR2(0x15, PUSH(b | a));
            /* XOR */ OPC_SR2(0x16, PUSH(b ^ a));
            /* NOT */ OPC_SR1(0x17, PUSH(~a));
            /* SWP */ OPC_SWP(0x18);
            /* DUP */ OPC_SR1(0x19, PUSH(a));
            /* OVR */ OPC_SR2(0x1A, PUSH(b); PUSH(a); PUSH(b));
            /* ROT */ OPC_SR2(0x1B, PUSH(b); PUSH(POP()); PUSH(a));
            /* JMP */ case 0x1C: *pc = us_popw(); break;
            /* JCN */ OPC_SR1(0x1D, if (a) { *pc = POP(); } );
            /* JSR */ case 0x1E: xyw_word addr = us_popw(); ss_pushw(*pc); *pc = addr; break;
            /* RTS */ case 0x1F: *pc = ss_popw(); break;
            // clang-format on
        }
        // TODO: error handling etc.
        (*pc)++;
    }

    return 0;
}