#include "xyw.h"

#define MEMORY_SIZE 0xffff
#define STACK_SIZE 0xff
#define DEVICE_AREA_START 0x0000
#define SYSTEM_STACK_START 0x0100
#define USER_STACK_START 0x0200
#define USER_MEMORY_START 0x0300

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 0x00
#define SYSTEM_ERROR 0x01
#define SYSTEM_SSP 0x02
#define SYSTEM_USP 0x03
#define SYSTEM_X 0x04
#define SYSTEM_Y 0x05
#define SYSTEM_XW 0x06
#define SYSTEM_YW 0x08
#define SYSTEM_PC 0x0A
#define SYSTEM_PAGE 0x0C
#define SYSTEM_ON_ERROR 0x0D

/* console device */
#define CONSOLE_BASE 0x10
#define CONSOLE_INPUT 0x10
#define CONSOLE_OUTPUT 0x11
#define CONSOLE_ERROR 0x12
#define CONSOLE_ON_KEYPRESS 0x13

/* clock device */
#define CLOCK_YEAR 0x20
#define CLOCK_MONTH 0x22
#define CLOCK_WEEK 0x23
#define CLOCK_MONTHDAY 0x24
#define CLOCK_WEEKDAY 0x25
#define CLOCK_HOUR 0x26
#define CLOCK_MINUTE 0x27
#define CLOCK_SECOND 0x28
#define CLOCK_TIMER 0x29
#define CLOCK_ON_TIMER_ELAPSED 0x2B

/* file device */
#define FILE_PATH 0x30
#define FILE_OPERATION 0x32
#define FILE_STATE 0x33
#define FILE_TYPE 0x34
#define FILE_READ 0x35
#define FILE_WRITE 0x36
#define FILE_APPEND 0x37

/* display device */
#define DISPLAY_WIDTH 0x40
#define DISPLAY_HEIGHT 0x41
#define DISPLAY_FILL 0x42
#define DISPLAY_X 0x43
#define DISPLAY_Y 0x44
#define DISPLAY_ON_RENDER 0x45

/* display device */
#define BUZZER_STATE 0x50
#define BUZZER_SAMPLES 0x51
#define BUZZER_N_SAMPLES 0x53
#define BUZZER_ON_STOP 0x55

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

//// 1-byte pointers
xyw_byte *ssp = &xyw_memory[SYSTEM_SSP];
xyw_byte *usp = &xyw_memory[SYSTEM_USP];
xyw_byte *x = &xyw_memory[SYSTEM_X];
xyw_byte *y = &xyw_memory[SYSTEM_Y];

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

static inline void madd(xyw_word addr, int delta)
{
    xyw_word val = mget(addr);
    mset(addr, (xyw_word)(val + delta));
}

//// Helpers for common registers

static inline xyw_word pc(void) { return mget(SYSTEM_PC); }
// static inline void pc_set(xyw_word val) { mset(SYSTEM_PC, val); }
// static inline void pc_add(int delta) { madd(SYSTEM_PC, delta); }
static inline xyw_word xw(void) { return mget(SYSTEM_XW); }
// static inline void xw_set(xyw_word val) { mset(SYSTEM_XW, val); }
// static inline void xw_add(int delta) { madd(SYSTEM_XW, delta); }
static inline xyw_word yw(void) { return mget(SYSTEM_YW); }
// static inline void yw_set(xyw_word val) { mset(SYSTEM_YW, val); }
// static inline void yw_add(int delta) { madd(SYSTEM_YW, delta); }

//// Mode helpers

static int is_x() { return xyw_memory[pc()] & XYW_MODE_X; }
static int is_y() { return xyw_memory[pc()] & XYW_MODE_Y; }
static int is_w() { return xyw_memory[pc()] & XYW_MODE_W; }

//// System stack management

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

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

//// User stack management

static void us_push(xyw_byte val)
{
    if (*usp > STACK_SIZE - 1)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_OVERFLOW;
        return;
    }
    user_stack[(*usp)++] = val;
}

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

//// Instructions

static void PSH()
{
    if (*usp > STACK_SIZE - 1)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_OVERFLOW;
        return;
    }
    user_stack[(*usp)++] = xyw_memory[pc()];
}

static void PSHw()
{
    if (*usp > STACK_SIZE - 2)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_OVERFLOW;
        return;
    }
    mset(USER_STACK_START + *usp, mget(pc()));
    (*usp) += 2;
}

static xyw_byte POP(xyw_byte first)
{
    // First operand, get value from X if set
    if (first && is_x())
    {
        return *x;
    }
    // Othwerwise try Y
    if (first && is_y())
    {
        return *y;
    }
    // For the second operand, get value from Y if both X and Y modes are set
    if (!first && is_x() && is_y())
    {
        return *y;
    }
    // If no mode is set, pop from stack
    if (*usp < 1)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_UNDERFLOW;
        return -1;
    }
    xyw_byte val = user_stack[*usp];
    user_stack[*usp] = 0;
    (*usp)--;
    return val;
}

static xyw_word POPw(xyw_byte first)
{
    // First operand, get value from X if set
    if (first && is_x())
    {
        return xw();
    }
    // Othwerwise try Y
    if (first && is_y())
    {
        return yw();
    }
    // For the second operand, get value from Y if both X and Y modes are set
    if (!first && is_x() && is_y())
    {
        return yw();
    }
    // If no mode is set, pop from stack
    if (*usp < 2)
    {
        xyw_memory[SYSTEM_ERROR] = XYW_ERROR_STACK_UNDERFLOW;
        return -1;
    }
    xyw_word val = mget(USER_STACK_START + (*usp) - 1);
    mset(USER_STACK_START + (*usp) - 1, 0);
    (*usp) -= 2;
    return val;
}

static xyw_byte LDR(xyw_word addr)
{
    return xyw_memory[addr];
}

static xyw_word LDRw(xyw_word addr)
{
    return mget(addr);
}

static xyw_byte LDP(xyw_byte addr)
{
    return xyw_memory[((xyw_word)xyw_memory[SYSTEM_PAGE] << 8) | addr];
}

static xyw_word LDPw(xyw_byte addr)
{
    return mget(((xyw_word)xyw_memory[SYSTEM_PAGE] << 8) | addr);
}

static xyw_byte ILD(xyw_word addr)
{
    madd(addr, 1);
    return xyw_memory[mget(addr)];
}

static xyw_word ILDw(xyw_word addr)
{
    madd(addr, 1);
    return mget(mget(addr));
}

static xyw_byte LDI(xyw_word addr)
{
    xyw_byte r = xyw_memory[addr];
    madd(addr, 1);
    return r;
}

static xyw_word LDIw(xyw_word addr)
{
    xyw_word r = mget(addr);
    madd(addr, 1);
    return r;
}

static xyw_byte DLD(xyw_word addr)
{
    madd(addr, -1);
    return xyw_memory[mget(addr)];
}

static xyw_word DLDw(xyw_word addr)
{
    madd(addr, -1);
    return mget(mget(addr));
}

static xyw_byte LDD(xyw_word addr)
{
    xyw_byte r = xyw_memory[addr];
    madd(addr, -1);
    return r;
}

static xyw_word LDDw(xyw_word addr)
{
    xyw_word r = mget(addr);
    madd(addr, -1);
    return r;
}

static void STR(xyw_word addr, xyw_byte val)
{
    xyw_memory[addr] = val;
}

static void STRw(xyw_word addr, xyw_word val)
{
    mset(addr, val);
}

static void STP(xyw_byte addr, xyw_byte val)
{
    xyw_memory[((xyw_word)xyw_memory[SYSTEM_PAGE] << 8) | addr] = val;
}

static void STPw(xyw_byte addr, xyw_word val)
{
    mset(((xyw_word)xyw_memory[SYSTEM_PAGE] << 8) | addr, val);
}

//// Macro helpers

// Infix operation
#define IOP(op) is_w() ? us_pushw((POPw(1) op POPw(0))) : us_push((POP(1) op POP(0)))
// Binary operation
#define BOP(op) is_w() ? us_pushw(op(POPw(1), POPw(0))) : us_push(op(POP(1), POP(0)))
// Unary operation
#define UOP(op) is_w() ? us_pushw(op(POPw(1))) : us_push(op(POP(1)))

int run()
{
    xyw_word current;
    while (1)
    {
        current = pc();
        madd(SYSTEM_PC, 1);
        switch (xyw_memory[current])
        {
            // clang-format off
            /* BRK */ case 0x00: break;
            /* PSH */ case 0x01: is_w() ? PSHw() : PSH(); break;
            /* POP */ case 0x02: is_w() ? POPw(1) : POP(1); break;
            /* NOT */ case 0x03: UOP(!); break;

            /* LDR */ case 0x04: is_w() ? UOP(LDRw) : UOP(LDR); break;
            /* STR */ case 0x05: is_w() ? STRw(POPw(1), POPw(0)) : STR(POP(1), POP(0)); break;
            /* LDP */ case 0x06: is_w() ? UOP(LDPw) : UOP(LDP); break;
            /* STP */ case 0x07: is_w() ? STPw(POPw(1), POPw(0)) : STP(POP(1), POP(0)); break;

            /* ILD */ case 0x08: is_w() ? UOP(ILD) : UOP(ILDw); break;
            /* LDI */ case 0x09: is_w() ? UOP(LDI) : UOP(LDIw); break;
            /* DLD */ case 0x0A: is_w() ? UOP(DLD) : UOP(DLDw); break;
            /* LDD */ case 0x0B: is_w() ? UOP(LDD) : UOP(LDDw); break;

            /* ADD */ case 0x0C: IOP(+); break;
            /* SUB */ case 0x0D: IOP(-); break;
            /* MUL */ case 0x0E: IOP(*); break;
            /* DIV */ case 0x0F: IOP(/); break;

            /* EQU */ case 0x10: IOP(==); break;
            /* NEQ */ case 0x11: IOP(!=); break;
            /* GTH */ case 0x12: IOP(>); break;
            /* LTH */ case 0x13: IOP(<); break;

            /* AND */ case 0x14: IOP(&); break;
            /* IOR */ case 0x15: IOP(|); break;
            /* XOR */ case 0x16: IOP(^); break;
            /* SFT */ case 0x17: break; // TODO from here

            /* SWP */ case 0x18: break;
            /* DUP */ case 0x19: break;
            /* OVR */ case 0x1A: break;
            /* ROT */ case 0x1B: break;

            /* JMP */ case 0x1C: break;
            /* JCN */ case 0x1D: break;
            /* JSR */ case 0x1E: break;
            /* RTS */ case 0x1F: break;
            // clang-format on
        }
    }

    return 0;
}