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

#include "devices/system.h"
#include "devices/terminal.h"
#include "devices/clock.h"
#include "devices/file.h"
#include "devices/beeper.h"

#define MEMORY_SIZE 0x10000

#define USER_STACK_SIZE 128
#define SYSTEM_FRAME_SIZE 8
#define SYSTEM_MAX_FRAMES 48
#define MAX_VECTORS 4

#define USER_MEMORY_START 0x000
#define DEVICE_AREA_START 0xff00

#define MAX_IMAGE_EXEC_DEPTH 8
#define MAX_IMAGE_EXEC_PATH_LENGTH 256

/* system device addresses */
#define SYSTEM_STATE 0xff00
#define SYSTEM_ERROR 0xff01
#define SYSTEM_PAGE 0xff02
#define SYSTEM_RANDOM 0xff03
#define SYSTEM_ON_ERROR 0xff04

/* Terminal device addresses */
#define TERMINAL_INPUT 0xff10
#define TERMINAL_ON_KEYPRESS 0xff12
#define TERMINAL_ON_ARGUMENT 0xff14

/* Clock device addresses */
#define CLOCK_TIMER 0xff28
#define CLOCK_ON_TIMER_ELAPSED 0xff2a

#ifdef _WIN32
#include <conio.h>
#include <windows.h>
int getch()
{
    return _getch();
}
#else
#include <unistd.h>
int getch()
{
    return getchar();
}
#endif
typedef enum
{
    XYW_ERROR_NONE = 0,
    XYW_ERROR_DIVISION_BY_ZERO,
    XYW_ERROR_STACK_UNDERFLOW,
    XYW_ERROR_STACK_OVERFLOW,
    XYW_ERROR_EXEC_DEPTH_EXCEEDED,
} xyw_error;

#define SYSTEM_STATE_RUNNING 0x01
#define SYSTEM_STATE_WAITING 0x02
#define SYSTEM_STATE_DEBUG 0x40
#define SYSTEM_STATE_ERROR 0x80

/// Main memory
xyw_byte xyw_memory[MEMORY_SIZE];

/// Stacks (not part of addressable memory)
static xyw_byte user_stack[USER_STACK_SIZE];
static xyw_byte system_stack[SYSTEM_MAX_FRAMES * SYSTEM_FRAME_SIZE];
static xyw_byte s = 0; // system stack frame index
static xyw_byte u = 0; // user stack pointer

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

//// registers
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;
xyw_byte *error = &xyw_memory[SYSTEM_ERROR];

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

//// Vector slots
static xyw_vector_slot vector_slots[MAX_VECTORS];

//// Helpers to retrieve device number and address within the device
static inline int get_device(xyw_word addr)
{
    if (addr < DEVICE_AREA_START)
        return -1;
    return (addr - DEVICE_AREA_START) / 16;
}

static inline xyw_byte *get_device_data(xyw_word addr)
{
    return xyw_devices[get_device(addr)].data;
}

static inline xyw_byte get_device_address(xyw_word addr)
{
    // Given $FF41, it should return $01
    return (xyw_byte)((addr - DEVICE_AREA_START) & 0x0F);
}

//// Read/write memory helpers

static inline xyw_byte readb(xyw_word addr)
{
    int dev_num = get_device(addr);
    if (dev_num >= 0 && xyw_devices[dev_num].input)
    {
        xyw_byte dev_addr = get_device_address(addr);
        return xyw_devices[dev_num].input(get_device_data(addr), dev_addr, error);
    }
    return xyw_memory[addr];
}

static inline void writeb(xyw_word addr, xyw_byte val)
{
    xyw_memory[addr] = val;
    int dev_num = get_device(addr);
    if (dev_num >= 0 && xyw_devices[dev_num].output)
    {
        xyw_byte dev_addr = get_device_address(addr);
        xyw_devices[dev_num].output(get_device_data(addr), dev_addr, error);
    }
}

static inline xyw_word readw(xyw_word addr)
{
    // Read high and low bytes independently to handle device boundaries
    xyw_byte high = readb(addr);
    xyw_byte low = readb(addr + 1);
    return (xyw_word)((high << 8) | low);
}

static inline void writew(xyw_word addr, xyw_word val)
{
    // Write high and low bytes independently to handle device boundaries
    // and trigger output handlers for both bytes
    writeb(addr, (xyw_byte)(val >> 8));
    writeb(addr + 1, (xyw_byte)(val & 0xFF));
}

//// Device dispatchers

//// System stack management (frame-based)
// Each frame is SYSTEM_FRAME_SIZE (8) bytes:
//   offset 0-1: return address (xyw_word, big-endian)
//   offset 2:   x register (xyw_byte)
//   offset 3:   y register (xyw_byte)
//   offset 4-5: xw register (xyw_word, big-endian)
//   offset 6-7: yw register (xyw_word, big-endian)
// *s is a frame index (0..SYSTEM_MAX_FRAMES-1), not a byte offset.

static void ss_push_frame(xyw_word return_addr)
{
    if (s >= SYSTEM_MAX_FRAMES)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_OVERFLOW;
        return;
    }
    xyw_byte *base = &system_stack[s * SYSTEM_FRAME_SIZE];
    base[0] = (xyw_byte)(return_addr >> 8);
    base[1] = (xyw_byte)(return_addr & 0xFF);
    base[2] = *x;
    base[3] = *y;
    base[4] = (xyw_byte)(*xw >> 8);
    base[5] = (xyw_byte)(*xw & 0xFF);
    base[6] = (xyw_byte)(*yw >> 8);
    base[7] = (xyw_byte)(*yw & 0xFF);
    s++;
}

static xyw_word ss_pop_frame()
{
    if (s < 1)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_UNDERFLOW;
        return -1;
    }
    s--;
    xyw_byte *base = &system_stack[s * SYSTEM_FRAME_SIZE];
    xyw_word return_addr = (xyw_word)((base[0] << 8) | base[1]);
    *x = base[2];
    *y = base[3];
    *xw = (xyw_word)((base[4] << 8) | base[5]);
    *yw = (xyw_word)((base[6] << 8) | base[7]);
    // Clear the frame
    for (int i = 0; i < SYSTEM_FRAME_SIZE; i++) base[i] = 0;
    return return_addr;
}

//// Vector dispatch (deferred, non-recursive event handling)

// Request a vector
void xyw_request_vector(xyw_word handler)
{
    for (int i = 0; i < MAX_VECTORS; i++)
    {
        if (!vector_slots[i].pending)
        {
            vector_slots[i].handler = handler;
            vector_slots[i].pending = 1;
            return;
        }
    }
    XYW_DBG("Vector table full, dropping request for handler %04X\n", handler);
}

// Dispatch requested vectors (called once per xyw_eval iteration)
static void dispatch_vectors(void)
{
    for (int i = 0; i < MAX_VECTORS; i++)
    {
        if (vector_slots[i].pending)
        {
            vector_slots[i].pending = 0;
            XYW_DBG("Dispatching vector to handler %04X\n", vector_slots[i].handler);
            ss_push_frame(*pc);
            *pc = vector_slots[i].handler - 1;
            return; 
        }
    }
}

//// User stack management

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

static void us_pushw(xyw_word val)
{
    if (u > USER_STACK_SIZE - 2)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_OVERFLOW;
        return;
    }
    user_stack[u]     = (xyw_byte)(val >> 8);
    user_stack[u + 1] = (xyw_byte)(val & 0xFF);
    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 = user_stack[u];
    user_stack[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 = (xyw_word)((user_stack[u - 2] << 8) | user_stack[u - 1]);
    user_stack[u - 2] = 0;
    user_stack[u - 1] = 0;
    u -= 2;
    return val;
}

// clang-format off

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

//// Operations that support stack and one register
#define OPC_SR1(opc, init, body) { \
    case opc: { const int w=0, rx=0, ry=0; init; body; } break; \
    case XYW_MODE_X|opc: { const int w=0, rx=1, ry=0; init; body; } break; \
    case XYW_MODE_Y|opc: { const int w=0, rx=0, ry=1; init; body; } break; \
    case XYW_MODE_W|opc: { const int w=1, rx=0, ry=0; init; body; } break; \
    case XYW_MODE_X|XYW_MODE_W|opc: { const int w=1, rx=1, ry=0; init; body; } break; \
    case XYW_MODE_Y|XYW_MODE_W|opc: { const int w=1, rx=0, ry=1; init; body; } break; \
}

//// Operations that support both registers always set
#define OPC_SR1r(opc, body) { \
    case XYW_MODE_X|XYW_MODE_Y|opc: { const int w=0, rx=1, ry=1; xyw_byte a = *x; xyw_byte b = *y; body;(void)(w);(void)(a);(void)(b);(void)(rx);(void)(ry); } break; \
    case XYW_MODE_X|XYW_MODE_Y|XYW_MODE_W|opc: { const int w=1, rx=1, ry=1; xyw_word a = *xw; xyw_word b = *yw; body;(void)(w);(void)(a);(void)(b);(void)(rx);(void)(ry); } break; \
}

// OVR: Duplicates the second stack item: a b -- a b a
#define OPC_OVR(opc) { \
    case opc: { \
        xyw_byte b = us_pop(); xyw_byte a = us_pop(); \
        us_push(a); us_push(b); us_push(a); \
    } break; \
    case XYW_MODE_W|opc: { \
        xyw_word b = us_popw(); xyw_word a = us_popw(); \
        us_pushw(a); us_pushw(b); us_pushw(a); \
    } break; \
}

// ROT: Rotates the third stack item to the top: a b c -- b c a
#define OPC_ROT(opc) { \
    case opc: { \
        xyw_byte c = us_pop(); xyw_byte b = us_pop(); xyw_byte a = us_pop(); \
        us_push(b); us_push(c); us_push(a); \
    } break; \
    case XYW_MODE_W|opc: { \
        xyw_word c = us_popw(); xyw_word b = us_popw(); xyw_word a = us_popw(); \
        us_pushw(b); us_pushw(c); us_pushw(a); \
    } break; \
}

// Create address based on direct page or full address
#define ADDR(ad) (w ? (ad) : (DIRECT_PAGE | (ad)))
// Push a value to stack
#define PUSH(n) do { if (w) { us_pushw((xyw_word)(n)); } else { us_push((xyw_byte)(n)); } } while(0)
// Pop value from stack
#define POP() (w ? us_popw() : us_pop())
// Get argument from x register or stack
#define ARGX (rx ? (w ? *xw : *x) : (w ? us_popw() : us_pop()))
// Get argument from y register or stack
#define ARGY (ry ? (w ? *yw : *y) : (w ? us_popw() : us_pop()))
// Get argument from x or y register or stack
#define ARG (rx ? ARGX : (ry ? ARGY : POP()))
// Set value to x register or push to stack
#define SETX(v) rx ? (w ? (*xw = (xyw_word)(v)) : (*x = (xyw_byte)(v))) : (w ? us_pushw((xyw_word)(v)) : us_push((xyw_byte)(v)))
// Set value to y register or push to stack
#define SETY(v) ry ? (w ? (*yw = (xyw_word)(v)) : (*y = (xyw_byte)(v))) : (w ? us_pushw((xyw_word)(v)) : us_push((xyw_byte)(v)))
// Set value to x or y register or push to stack
#define SET(v) do { if (rx) { SETX(v); } else if (ry) { SETY(v); } else { PUSH(v); } } while(0)

// Read a value from memory
#define READ(addr) (w ? readw(addr) : readb(addr))
// Read top of the user stack (direct array access, no device dispatch)
#define TOP (w ? (xyw_word)((user_stack[u - 2] << 8) | user_stack[u - 1]) : (xyw_word)user_stack[u - 1])

//// Initialize devices

xyw_device xyw_devices[XYW_TOTAL_DEVICES] = {
    // System device
    {&xyw_memory[0xff00], system_setup, system_input, system_output, 0},
    // Terminal device
    {&xyw_memory[0xff10], 0, terminal_input, terminal_output, terminal_teardown},
    // Clock device
    {&xyw_memory[0xff20], clock_setup, clock_input, clock_output, 0},
    // File device
    {&xyw_memory[0xff30], file_setup, file_input, file_output, 0},
    // Beeper device
    {&xyw_memory[0xff40], 0, beeper_input, beeper_output, beeper_teardown},
    // ...
};
// clang-format on

static int system_error_state()
{
    return xyw_memory[SYSTEM_STATE] & SYSTEM_STATE_ERROR;
}

static int system_running_state()
{
    return xyw_memory[SYSTEM_STATE] & SYSTEM_STATE_RUNNING;
}

static int system_waiting_state()
{
    return xyw_memory[SYSTEM_STATE] & SYSTEM_STATE_WAITING;
}

//// Internal VM state
xyw_byte error_handler_entry_s = 0;

static int process_events()
{
    // Error handling
    if (xyw_memory[SYSTEM_ERROR] != XYW_ERROR_NONE && !system_error_state())
    {
        xyw_word on_error_handler = XYW_PEEKW(&xyw_memory[SYSTEM_ON_ERROR]);
        // If an error handler is set, jump to it
        if (on_error_handler != 0)
        {
            XYW_DBG("Error occurred: %02X, jumping to error handler at %04X\n", xyw_memory[SYSTEM_ERROR], on_error_handler);
            // Enter error state
            xyw_memory[SYSTEM_STATE] |= SYSTEM_STATE_ERROR;
            error_handler_entry_s = s; // Record stack depth before pushing return address
            ss_push_frame(*pc);
            *pc = on_error_handler;
        }
        else
        {
            // No error handler, halt execution
            return xyw_memory[SYSTEM_ERROR];
        }
    }
    // Timer elapsed handling
    xyw_word on_timer_handler = XYW_PEEKW(&xyw_memory[CLOCK_ON_TIMER_ELAPSED]);
    if (on_timer_handler != 0)
    {
        if (readb(CLOCK_ON_TIMER_ELAPSED))
        {
            XYW_DBG("Timer elapsed, requesting vector for handler at %04X\n", on_timer_handler);
            xyw_request_vector(on_timer_handler);
        }
    }

    return 0;
}

//// Snapshot management

typedef struct {
    xyw_byte memory[MEMORY_SIZE];
    xyw_byte user_stack[USER_STACK_SIZE];
    xyw_byte system_stack[SYSTEM_MAX_FRAMES * SYSTEM_FRAME_SIZE];
    xyw_byte s, u, x_val, y_val;
    xyw_word pc_val, xw_val, yw_val;
    xyw_vector_slot vector_slots[MAX_VECTORS];
    xyw_byte error_handler_entry_s;
    int argv_processed;
} xyw_image_snapshot;

static xyw_image_snapshot exec_stack[MAX_IMAGE_EXEC_DEPTH];
static int exec_stack_depth = 0;
static int exec_pending = 0;
static char exec_path_buf[MAX_IMAGE_EXEC_PATH_LENGTH];

// Read a null-terminated string out of VM memory into a local buffer
static void get_string_from_memory(xyw_word addr, char *buf, size_t bufsize)
{
    size_t i;
    for (i = 0; i < bufsize - 1 && xyw_memory[addr + i] != 0; i++)
        buf[i] = xyw_memory[addr + i];
    buf[i] = '\0';
}

void xyw_request_exec(xyw_word path_addr)
{
    get_string_from_memory(path_addr, exec_path_buf, sizeof(exec_path_buf));
    exec_pending = 1;
}

static void split_exec_string(char *full, char **path, char **argstart)
{
    char *sp = strchr(full, ' ');
    if (sp)
    {
        *sp = '\0';
        *argstart = sp + 1;
    }
    else
    {
        *argstart = NULL;
    }
    *path = full;
}

static void reset_vm_state(void)
{
    memset(xyw_memory, 0, sizeof(xyw_memory));
    memset(user_stack, 0, sizeof(user_stack));
    memset(system_stack, 0, sizeof(system_stack));
    s = 0;
    u = 0;
    x_val = 0;
    y_val = 0;
    xw_storage = 0;
    yw_storage = 0;
    pc_storage = 0;
    memset(vector_slots, 0, sizeof(vector_slots));
    error_handler_entry_s = 0;
    xyw_argv_processed = 0;
}

static int load_and_launch(const char *path, char *argstart)
{
    FILE *fp = fopen(path, "rb");
    if (!fp)
    {
        fprintf(stderr, "Error - Could not open file [%s]\n", path);
        xyw_memory[SYSTEM_STATE] &= ~SYSTEM_STATE_RUNNING;
        return -1;
    }
    size_t bytes_read = fread(&xyw_memory[USER_MEMORY_START], 1, MEMORY_SIZE - USER_MEMORY_START, fp);
    fclose(fp);
    if (bytes_read == 0)
    {
        fprintf(stderr, "Error - Could not read file contents [%s]\n", path);
        xyw_memory[SYSTEM_STATE] &= ~SYSTEM_STATE_RUNNING;
        return -1;
    }
    xyw_memory[SYSTEM_STATE] |= SYSTEM_STATE_RUNNING;

    for (int i = 0; i < XYW_TOTAL_DEVICES; i++)
        if (xyw_devices[i].setup)
            xyw_devices[i].setup(xyw_devices[i].data);

    xyw_memory[SYSTEM_PAGE] = 0xff;
    *pc = 0x0000;

    if (argstart && XYW_PEEKW(&xyw_memory[TERMINAL_ON_ARGUMENT]))
    {
        char *tok = strtok(argstart, " ");
        int first = 1;
        while (tok)
        {
            if (!first)
                writeb(TERMINAL_INPUT, XYW_ARGUMENT_SEPARATOR);
            for (size_t j = 0; tok[j] != '\0'; j++)
                writeb(TERMINAL_INPUT, (xyw_byte)tok[j]);
            first = 0;
            tok = strtok(NULL, " ");
        }
        writeb(TERMINAL_INPUT, XYW_END_OF_ARGUMENTS);
        xyw_argv_processed = 1;
    }
    return 0;
}

static void save_snapshot(xyw_image_snapshot *snap)
{
    memcpy(snap->memory, xyw_memory, sizeof(xyw_memory));
    memcpy(snap->user_stack, user_stack, sizeof(user_stack));
    memcpy(snap->system_stack, system_stack, sizeof(system_stack));
    snap->s = s;
    snap->u = u;
    snap->x_val = x_val;
    snap->y_val = y_val;
    snap->pc_val = *pc;
    snap->xw_val = *xw;
    snap->yw_val = *yw;
    memcpy(snap->vector_slots, vector_slots, sizeof(vector_slots));
    snap->error_handler_entry_s = error_handler_entry_s;
    snap->argv_processed = xyw_argv_processed;
}

static void restore_snapshot(xyw_image_snapshot *snap)
{
    memcpy(xyw_memory, snap->memory, sizeof(xyw_memory));
    memcpy(user_stack, snap->user_stack, sizeof(user_stack));
    memcpy(system_stack, snap->system_stack, sizeof(system_stack));
    s = snap->s;
    u = snap->u;
    x_val = snap->x_val;
    y_val = snap->y_val;
    *pc = snap->pc_val;
    *xw = snap->xw_val;
    *yw = snap->yw_val;
    memcpy(vector_slots, snap->vector_slots, sizeof(vector_slots));
    error_handler_entry_s = snap->error_handler_entry_s;
    xyw_argv_processed = snap->argv_processed;

    // Devices with state outside xyw_memory (file cursor, clock timer target,
    // beeper device) don't roll back automatically — re-sync to a clean slate
    for (int i = 0; i < XYW_TOTAL_DEVICES; i++)
        if (xyw_devices[i].setup)
            xyw_devices[i].setup(xyw_devices[i].data);
}

static void handle_exec_requests(void)
{
    if (!exec_pending)
        return;
    exec_pending = 0;

    if (exec_stack_depth >= MAX_IMAGE_EXEC_DEPTH)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_EXEC_DEPTH_EXCEEDED;
        return;
    }
    save_snapshot(&exec_stack[exec_stack_depth++]);

    char path_copy[MAX_IMAGE_EXEC_PATH_LENGTH];
    strncpy(path_copy, exec_path_buf, sizeof(path_copy) - 1);
    path_copy[sizeof(path_copy) - 1] = '\0';
    char *path, *argstart;
    split_exec_string(path_copy, &path, &argstart);

    reset_vm_state();
    load_and_launch(path, argstart);
}

//// Main evaluation function - runs from current pc until BRK
int xyw_eval(xyw_word start_pc)
{
    *pc = start_pc;

    // Save register state so event handlers don't corrupt the caller's registers
    xyw_byte saved_x = *x;
    xyw_byte saved_y = *y;
    xyw_word saved_xw = *xw;
    xyw_word saved_yw = *yw;

    if (xyw_debug)
    {
        xyw_memory[SYSTEM_STATE] |= SYSTEM_STATE_DEBUG;
    }
    while (1)
    {
        if (*pc >= DEVICE_AREA_START)
        {
            xyw_memory[SYSTEM_STATE] &= ~SYSTEM_STATE_RUNNING;
        } 
        if (!system_running_state())
        {
            if (exec_stack_depth > 0)
            {
                restore_snapshot(&exec_stack[--exec_stack_depth]);
                continue;   // resume the parent exactly where it left off
            }
            break;          // no parent -- genuinely done
        }

        process_events();
        dispatch_vectors();
        handle_exec_requests();
        if (!system_running_state())
        {
            continue;
        }
        XYW_DBG(" -- PC: %04X -> %02X (%s) [U:%02X|S:%02X|X:%02X|Y:%02X|XW:%04X|YW:%04X]\n", *pc, xyw_memory[*pc], xyw_instructions[xyw_memory[*pc] & 0x1F], u, s, *x, *y, *xw, *yw);
        switch (xyw_memory[*pc])
        {
            // clang-format off
            /* HLT */ case 0x00: return 0;
            /* NOP */ case 0x01: break;
            /* PSH */ OPC_SR1(0x02, , if (rx||ry) { PUSH(ARG); } else { (*pc)++; PUSH(READ(*pc)); if (w) { (*pc)++; } })
            /* PSH */ OPC_SR1r(0x02, PUSH(a); PUSH(b); )
            /* POP */ OPC_SR1(0x03, , if (rx||ry) { SET(POP()); } else { POP(); } );
            /* POP */ OPC_SR1r(0x03, if (w) { xyw_word val = us_popw(); *xw = val; *yw = val; } else { xyw_byte val = us_pop(); *x = val; *y = val; } );
            /* LDB */ OPC_SR1(0x04, , us_push(readb(ADDR(ARG))));
            /* LDB */ OPC_SR1r(0x04, us_push(readb(ADDR(ARGX))); us_push(readb(ADDR(ARGY))); );
            /* LDW */ OPC_SR1(0x05, , us_pushw(readw(ADDR(ARG)) ));
            /* LDW */ OPC_SR1r(0x05, us_pushw(readw(ADDR(ARGX))); us_pushw(readw(ADDR(ARGY))); );
            /* STB */ OPC_SR1(0x06, xyw_word addr = ADDR(ARG); xyw_byte val = us_pop(), writeb( addr, val ));
            /* STB */ OPC_SR1r(0x06, xyw_byte val = us_pop(); writeb( ADDR(ARGX), val ); writeb( ADDR(ARGY), val ); );
            /* STW */ OPC_SR1(0x07, xyw_word addr = ADDR(ARG); xyw_word val = us_popw(), writew( addr, val ));
            /* STW */ OPC_SR1r(0x07, xyw_word val = us_popw(); writew( ADDR(ARGX), val ); writew( ADDR(ARGY), val ); );
            /* INC */ OPC_SR1(0x08, , SET(ARG + 1));
            /* INC */ OPC_SR1r(0x08, SET(ARGX + 1); SET(ARGY + 1); );
            /* DEC */ OPC_SR1(0x09, , SET(ARG - 1));
            /* DEC */ OPC_SR1r(0x09, SET(ARGX - 1); SET(ARGY - 1); );
            /* SHL */ OPC_SR1(0x0A, xyw_byte val = us_pop(), SET(ARG << val));
            /* SHL */ OPC_SR1r(0x0A, xyw_byte val = us_pop(); SET(ARGX << val); SET(ARGY << val); );
            /* SHR */ OPC_SR1(0x0B, xyw_byte val = us_pop(), SET(ARG >> val));
            /* SHR */ OPC_SR1r(0x0B, xyw_byte val = us_pop(); SET(ARGX >> val); SET(ARGY >> val); );
            /* ADD */ OPC_SR2(0x0C, PUSH(a + b));
            /* SUB */ OPC_SR2(0x0D, PUSH(a - b));
            /* MUL */ OPC_SR2(0x0E, PUSH(a * b));
            /* DIV */ OPC_SR2(0x0F, if (b) { PUSH(a % b); PUSH(a / b); } else { xyw_memory[SYSTEM_ERROR] = XYW_ERROR_DIVISION_BY_ZERO; } );
            /* EQU */ OPC_SR2(0x10, us_push(a == b));
            /* NEQ */ OPC_SR2(0x11, us_push(a != b));
            /* GTH */ OPC_SR2(0x12, us_push(a > b));
            /* LTH */ OPC_SR2(0x13, us_push(a < b));
            /* NOT */ OPC_SR1(0x14, , SET(~ARG));
            /* NOT */ OPC_SR1r(0x14, SET(~ARGX); SET(~ARGY); );
            /* AND */ OPC_SR2(0x15, PUSH(a & b));
            /* IOR */ OPC_SR2(0x16, PUSH(a | b));
            /* XOR */ OPC_SR2(0x17, PUSH(a ^ b));
            /* DUP */ OPC_SR1(0x18, , SET(TOP));
            /* DUP */ OPC_SR1r(0x18, SETX(TOP); SETY(TOP); );
            /* SWP */ OPC_SR2(0x19, if ((rx||ry)) { SET(b); } else { PUSH(b); PUSH(a); } );
            /* OVR */ OPC_OVR(0x1A);
            /* ROT */ OPC_ROT(0x1B);
            /* JMP */ OPC_SR1(0x1C, , *pc = ADDR(ARG)-1;);
            /* JCN */ OPC_SR1(0x1D, , xyw_word addr = ADDR(ARG); xyw_byte cond = us_pop(); if (cond) { *pc = addr-1; });
            /* JSR */ OPC_SR1(0x1E, xyw_word addr = ADDR(ARG),  ss_push_frame(*pc+1); *pc = addr-1;);
            /* RTS */ case 0x1F: 
                *pc = ss_pop_frame()-1; 
                // If returning from error handler, clear flag to allow re-trigger
                if ((system_error_state()) && s == error_handler_entry_s) {
                    xyw_memory[SYSTEM_STATE] &= ~SYSTEM_STATE_ERROR;
                    error_handler_entry_s = 0;
                }
                break;
            // clang-format on
        }
        (*pc)++;
    }
    // Restore register state saved at entry (protects caller from event handler side-effects)
    *x = saved_x;
    *y = saved_y;
    *xw = saved_xw;
    *yw = saved_yw;
    return 0;
}

static void xyw_teardown_devices(void)
{
    for (int i = 0; i < XYW_TOTAL_DEVICES; i++)
    {
        if (xyw_devices[i].teardown)
        {
            xyw_devices[i].teardown(xyw_devices[i].data);
        }
    }
}

//// Main run function
int xyw_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;
    }
    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] |= SYSTEM_STATE_RUNNING;
    // Setup devices
    for (int i = 0; i < XYW_TOTAL_DEVICES; i++)
    {
        if (xyw_devices[i].setup)
        {
            xyw_devices[i].setup(xyw_devices[i].data);
        }
    }
    atexit(xyw_teardown_devices);
    // Set direct page to device page by default
    xyw_memory[SYSTEM_PAGE] = 0xff;

    int eval_result = xyw_eval(0x0000);
    if (!eval_result)
    {
        // Handle command line arguments
        if (!xyw_argv_processed && !(system_error_state()) && XYW_PEEKW(&xyw_memory[TERMINAL_ON_ARGUMENT]))
        {
            for (int i = 1; i < xyw_argc; i++)
            {
                XYW_DBG("Processing argument %d of %d: %s\n", i, xyw_argc, xyw_argv[i]);
                const char *arg = xyw_argv[i];
                for (size_t j = 0; arg[j] != '\0'; j++)
                {
                    writeb(TERMINAL_INPUT, (xyw_byte)arg[j]);
                }
                if (i < xyw_argc - 1)
                {
                    writeb(TERMINAL_INPUT, XYW_ARGUMENT_SEPARATOR);
                }
            }
            writeb(TERMINAL_INPUT, XYW_END_OF_ARGUMENTS);
            xyw_argv_processed = 1;
        }
        
        // Check if we need to enter an event loop (keypress handler or timer)
        xyw_word on_keypress_handler = XYW_PEEKW(&xyw_memory[TERMINAL_ON_KEYPRESS]);
        xyw_word on_timer_handler = XYW_PEEKW(&xyw_memory[CLOCK_ON_TIMER_ELAPSED]);

        if (on_keypress_handler)
        {
            XYW_DBG("terminal.on_keypress handler set to %04X, entering input loop\n", on_keypress_handler);
            while (system_running_state() || system_waiting_state())
            {
                xyw_memory[SYSTEM_STATE] |= SYSTEM_STATE_WAITING;
                int c = getch();
                // if CTRL+C or CTRL+D is pressed, exit
                if (c == 3 || c == 4)
                {
                    return 0;
                }
                xyw_memory[SYSTEM_STATE] |= SYSTEM_STATE_RUNNING;
                writeb(TERMINAL_INPUT, (xyw_byte)c);
            }
        }
        else if (on_timer_handler)
        {
            XYW_DBG("clock.on_timer_elapsed handler set to %04X, entering timer loop\n", on_timer_handler);
            while (system_running_state() || system_waiting_state())
            {
                xyw_memory[SYSTEM_STATE] |= SYSTEM_STATE_WAITING;
                if (readb(CLOCK_ON_TIMER_ELAPSED))
                {
                    xyw_memory[SYSTEM_STATE] |= SYSTEM_STATE_RUNNING;
                    XYW_DBG("Timer elapsed, calling handler at %04X\n", on_timer_handler);
                    xyw_eval(on_timer_handler);
                    // Refresh handler in case it was changed
                    on_timer_handler = XYW_PEEKW(&xyw_memory[CLOCK_ON_TIMER_ELAPSED]);
                    if (!on_timer_handler)
                    {
                        break;
                    }
                }
            }
        }
    }

    return 0;
}