#include <stdbool.h>
#include <string.h>

#include "f6502.h"
#include "f6502_consts.h"

#ifdef F6502_TRACE
#include "f6502_opcodes.h"
#include <stdio.h>
#endif

#ifdef F6502_TEST
#include <stddef.h>
#define container_of(ptr, type, member) ({                      \
        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
        (type *)((char *)__mptr - offsetof(type,member));})
#endif

//#define NESE_DEBUG "Core"
#include "log.h"


// TOTO: Temp Hack
volatile uint8_t memval;

static inline uint8_t f6502_read_zp(nes_Memory* mem,
                                    uint8_t addr) {
    return mem->ram[addr];
}

static inline uint16_t f6502_read16_zp(nes_Memory* mem,
                                       uint8_t addr) {
    return (    f6502_read_zp(mem, addr) |
                ((uint16_t)f6502_read_zp(mem, addr + 1) << 8));
}

static inline uint8_t read_gamepad_bit(nes_Gamepad* gamepad) {
    uint8_t state = 1;
    if (gamepad->shift < 0) {
        state = (gamepad->buttons >> Button_A) & 1;
    } else if (gamepad->shift < nes_controller_num_buttons) {
        state = (gamepad->buttons >> gamepad->shift) & 1;
        gamepad->shift++;
    }
    return state;
}

static inline uint8_t f6502_read(nes_Memory* mem,
                                 uint16_t addr) {
#ifdef F6502_FLAT
#ifdef F6502_TRACE
//    printf("R $%04X -> %02X\n", addr, mem->ram[addr]);
#endif
    return mem->ram[addr];
#else
    if (addr >= 0x8000U) {
        return mem->rom_bank[(addr - 0x8000U) >> 13][addr & 0x1FFFU];
    }

    switch (addr & 0x6000U) {
    case 0x0000:
        return mem->ram[addr & 0x7FFU];

    case 0x2000:
        switch(addr & 0x7) {
        case ppu_reg_status:
        {   uint8_t val = mem->ppu.status;

            mem->ppu.status &= ~ppu_Status_VBlank;

            mem->ppu.latch = 0;

            // TODO: Set nametable to 0 when in VBlank??

            return val;
        }

        case ppu_reg_oam_data:
            return mem->ppu.oam[mem->ppu.oam_addr++];

        case ppu_reg_data:
        {   addr = mem->ppu.addr & 0x3FFFU;
            mem->ppu.addr += mem->ppu.addr_inc;
            uint8_t val = mem->ppu.data;
            mem->ppu.data = mem->ppu.bank[addr >> 10][addr & 0x3FFU];
            return val;
        }
        }
        break;

    case 0x4000:
        switch (addr & 0x1FU) {
        case 0x15:  /* APU Status */
        {   uint8_t ret = mem->apu.status;
            mem->apu.status &= ~apu_Status_Frame_Int;
            return ret;
        }

        case 0x16:
        case 0x17:
            return ((addr >> 8) & nes_controller_bus_mask) |
                   read_gamepad_bit(
                            &mem->input.gamepads[addr & 1]
                        );

        default:
            if (mem->mapper.read_apu) {
                return mem->mapper.read_apu(&mem->mapper, mem,
                                            addr & 0x1FU);
            }
        }
        break;

    case 0x6000:
        if (mem->sram_bank) {
            return mem->sram_bank[addr & 0x1FFFU];
        }
    }

    // Upper byte stays on the bus
    return (addr >> 8);
#endif
}

static inline uint16_t f6502_read16(nes_Memory* mem,
                                    uint16_t addr) {
    return (    f6502_read(mem, addr) |
                ((uint16_t)f6502_read(mem, addr + 1) << 8));
}

static inline int f6502_write(nes_Memory* mem,
                              uint16_t addr, uint8_t val) {
    int ret = 0;
#ifdef F6502_TRACE
    printf("W $%04X <- %02X\n", addr, val);
#endif
#ifdef F6502_FLAT
#ifdef F6502_TEST
    if (addr == 0xbffc) {
        f6502_Core* core = container_of(mem, f6502_Core, memory);

        int irq = val & 0b01;
        int nmi = val & 0b10;

#ifdef F6502_TRACE
        if (!irq ^ !(core->interrupts & f6502_Int_IRQ)) {
            fprintf(stdout, "%s IRQ\n", irq ? "Set" : "Clear");
        }
        if (!nmi ^ !(core->interrupts & f6502_Int_NMI)) {
            fprintf(stdout, "%s NMI\n", nmi ? "Set" : "Clear");
        }
#endif

        f6502_set_NMI(core, nmi);
        f6502_set_IRQ(core, irq);

        // Interrupt [may have been] triggered
        // We're not strictly counting cycles,
        // so just overload the counter
        ret = 1000;
    }
#endif
    mem->ram[addr] = val;
#else
    switch (addr & 0xe000) {
    case 0x0000:
        mem->ram[addr & 0x7FFU] = val;
        break;

    case 0x2000:
        switch (addr & 0x7U) {
        case ppu_reg_ctrl:
            mem->ppu.ctrl &= ppu_Control_Nametable_Mask;
            mem->ppu.ctrl |= (val & ~ppu_Control_Nametable_Mask);
            mem->ppu.t &= ~(ppu_Control_Nametable_Mask << 10);
            mem->ppu.t |= ((uint16_t)val & ppu_Control_Nametable_Mask) << 10;
            mem->ppu.addr_inc = (val & ppu_Control_VRAM_Inc) ? 32 : 1;
            // TODO: Trigger NMI if VBlank status is set?
            break;

        case ppu_reg_mask:
            mem->ppu.mask = val;
            break;

        case ppu_reg_oam_addr:
            mem->ppu.oam_addr = val;
            break;

        case ppu_reg_oam_data:
            mem->ppu.oam[(int)mem->ppu.oam_addr++] = val;
            break;

        case ppu_reg_scroll:
            if (mem->ppu.latch) {
                LOGI("PPU: SCROLL_Y %02X", val);
                mem->ppu.t &= 0b0000110000011111U;
                mem->ppu.t |= (uint16_t)(val & 0b00000111U) << 12;
                mem->ppu.t |= (uint16_t)(val & 0b11111000U) << 2;
            } else {
                LOGI("PPU: SCROLL_X %02X", val);
                mem->ppu.t &= ~(0b11111U);
                mem->ppu.t |= (val & 0b11111000U) >> 3;
                mem->ppu.x = (val & 0b111U);
            }
            mem->ppu.latch = !mem->ppu.latch;
            break;

        case ppu_reg_addr:
            if (mem->ppu.latch) {
                mem->ppu.t &= 0xFF00U;
                mem->ppu.t |= val;
                mem->ppu.addr = (mem->ppu.t & 0x3FFFU);
                // Keep ctrl & t nametable in sync
                mem->ppu.ctrl &= ~ppu_Control_Nametable_Mask;
                mem->ppu.ctrl |= ((val >> 10) & ppu_Control_Nametable_Mask);
                LOGI("PPU: ADDR %04X", mem->ppu.addr);
            } else {
                mem->ppu.t &= 0x00FFU;
                mem->ppu.t |= (uint16_t)(val & 0x3FU) << 8;
            }
            mem->ppu.latch = !mem->ppu.latch;
            break;

        case ppu_reg_data:
            // Disallow CHR ROM writes
            addr = mem->ppu.addr & 0x3FFFU;
            if (addr >= NES_PPU_CHR_SIZE || mem->ppu.chr_ram) {
                LOGI("PPU W %04X < %02X", addr, val);
                mem->ppu.bank[addr >> 10][addr & 0x3FFU] = val;
                if (addr >= NES_PPU_PAL_START) {
                    // Copy to render reference
                    addr &= 0x1FU;
                    uint8_t* pal = mem->ppu.palette;
                    if (0 != (addr & 0x3)) {
                        pal[addr] = val & 0x3FU;
                    }

                    // Memory-mapped mirroring
                    addr |= 0x300U;
                    for (int mirror = 0; mirror < 0x100; mirror += 0x20) {
                        mem->ppu.pal_bank[addr + mirror] = val;
                    }
                    if (0 == (addr & 3)) {
                        for (int mirror = 0; mirror < 0x100; mirror += 0x20) {
                            mem->ppu.pal_bank[(addr ^ 0x10U) + mirror] = val;
                        }
                    }
                }
            } else {
                LOGE("PPU W %04X < %02X : INVALID", addr, val);
            }
            mem->ppu.addr += mem->ppu.addr_inc;
            break;
        }
        break;

    case 0x4000:
        switch (addr & 0x1FU) {
        case 0x17:  // APU Frame
            // TODO: Clear interrupt here?
            /*
            if (val & apu_Frame_apu_Frame_Inhibit) {
                f6502_Int_IRQ(core, 0);
            }
            */
        case 0x00:
        case 0x01:
        case 0x02:
        case 0x03:
        case 0x04:
        case 0x05:
        case 0x06:
        case 0x07:
        case 0x08:
        case 0x09:
        case 0x0A:
        case 0x0B:
        case 0x0C:
        case 0x0D:
        case 0x0E:
        case 0x0F:
        case 0x10:
        case 0x11:
        case 0x12:
        case 0x13:
        case 0x15:  // APU Status
            if (mem->apu.n_events >= APU_MAX_EVENTS) {
                LOGE("APU event buffer full");
            } else {
                mem->apu.events[mem->apu.n_events++] =
                    (nes_APU_Event){
                        .reg = addr & 0x1FU,
                        .val = val,
                    };
            }
            break;

        case 0x14:  // OAM DMA
        {   uint8_t* src = NULL;
            // OAM DMA
            switch (val >> 5) {
            case 0: // 0x0000 - 0x1FFF RAM
                src = &mem->ram[((int)val << 8) & 0x7FFU];
                break;

            case 3: // 0x6000 - 0x7FFF SRAM
                if (mem->sram_bank) {
                    src = &mem->sram_bank[((int)val << 8) & 0x1FFFU];
                }
                break;

            case 4: // 0x8000 - 0x9FFF ROM
            case 5: // 0xA000 - 0xBFFF ROM
            case 6: // 0xC000 - 0xDFFF ROM
            case 7: // 0xE000 - 0xFFFF ROM
                src = &mem->rom_bank[(val >> 5) & 3][((int)val << 8) & 0x1FFFU];
                break;
            }
            if (NULL != src) {
                memcpy(mem->ppu.oam, src, NES_PPU_OAM_SIZE);
            }
            // TODO: Why does this throw off MMC3?
//            ret = 513;
        }   break;

        case 0x16:  // Reset Gamepad
            if (val & 1) {
                mem->input.gamepads[0].shift = -1;
                mem->input.gamepads[1].shift = -1;
            } else {
                mem->input.gamepads[0].shift = 0;
                mem->input.gamepads[1].shift = 0;
            }
            break;

        default:
            if (mem->mapper.write_apu) {
                ret = mem->mapper.write_apu(&mem->mapper, mem,
                                            addr, val);
            }
        }
        break;

    case 0x6000:
        if (mem->mapper.write_sram) {
            ret = mem->mapper.write_sram(&mem->mapper,
                                         mem, addr, val);
        } else if (mem->sram_bank) {
            mem->sram_bank[addr & 0x1FFFU] = val;
            mem->flags |= nes_Mem_SRAM_Used;
        }
        break;

    case 0x8000:
    case 0xA000:
    case 0xC000:
    case 0xE000:
        if (mem->mapper.write_rom) {
            ret = mem->mapper.write_rom(&mem->mapper,
                                        mem, addr, val);
        }
    }
#endif
    return ret;
}


void f6502_init(f6502_Core* core) {
    // TODO: Nothing for now
}

void f6502_reset(f6502_Core* core) {
    core->registers.PC = f6502_read16(&core->memory,
                                      f6502_Vector_Reset),
    core->registers.S -= 3;
    core->registers.P |= (f6502_Status_B | f6502_Status_1);
}

void f6502_set_NMI(f6502_Core* core, bool active) {
    if (active) core->interrupts |= f6502_Int_NMI;
    else core->interrupts &= ~(f6502_Int_NMI | f6502_Int_NMI_Serviced);
}

void f6502_set_IRQ(f6502_Core* core, bool active) {
    if (active) core->interrupts |= f6502_Int_IRQ;
    else core->interrupts &= ~f6502_Int_IRQ;
}

#define PUSH(core, S, val) \
    f6502_write(&core->memory, f6502_Base_Stack + S--, (val))
#define PUSH16(core, S, val) \
    PUSH(core, S, ((val) >> 8)); \
    PUSH(core, S, ((val) & 0xFFU))

#define POP(core, S, dst) \
    dst = f6502_read(&core->memory, f6502_Base_Stack + ++S)
#define POP16(core, S, dst)             \
    POP(core, S, dst);                  \
    dst |= (f6502_read(&core->memory,   \
                       f6502_Base_Stack + ++S) << 8)

#define CLR(R, flags)  R &= ~(flags)
#define SET(R, flags)  R |= (flags)

static inline int f6502_interrupt(f6502_Core* core,
                                  uint16_t addr) {
    PUSH16(core, core->registers.S, core->registers.PC);
    PUSH(core, core->registers.S,
         core->registers.P & ~f6502_Status_B);
    SET(core->registers.P, f6502_Status_I);
    core->registers.PC = f6502_read16(&core->memory, addr);
    return 7;
}

static inline bool f6502_nmi_ready(const f6502_Core* core) {
    return (    (core->interrupts & f6502_Int_NMI) &&
                !(core->interrupts & f6502_Int_NMI_Serviced));
}

static inline bool f6502_irq_ready(const f6502_Core* core) {
    return (    (core->interrupts & f6502_Int_IRQ) &&
                !(core->registers.P & f6502_Status_I));
}

static inline int f6502_check_interrupts(f6502_Core* core) {
    if (f6502_nmi_ready(core)) {
        core->interrupts |= f6502_Int_NMI_Serviced;
        return f6502_interrupt(core, f6502_Vector_NMI);
    } else if (f6502_irq_ready(core)) {
        return f6502_interrupt(core, f6502_Vector_IRQ);
    }
    return 0;
}

#define CLK(n)  clocks_elapsed += n

#define TEST(val) \
    CLR(P, f6502_Status_Z | f6502_Status_N); \
    SET(P, table_test[val])

#define A_ABS   ({                                          \
    uint16_t addr_lo = f6502_read(&core->memory, PC++);     \
    uint16_t addr_hi = f6502_read(&core->memory, PC++);     \
    (addr_lo | (addr_hi << 8));                             \
})
#define A_ABSX  (A_ABS + X)
#define A_ABSY  (A_ABS + Y)
#define A_ZP    f6502_read(&core->memory, PC++)
#define A_ZPX   (uint8_t)(f6502_read(&core->memory, PC++) + X)
#define A_ZPY   (uint8_t)(f6502_read(&core->memory, PC++) + Y)
#define A_IX    f6502_read16_zp(                            \
                        &core->memory,                      \
                        f6502_read(&core->memory, PC++) + X \
                    )
#define A_IY    (f6502_read16_zp(                       \
                        &core->memory,                  \
                        f6502_read(&core->memory, PC++) \
                    ) + Y)
#define D_IMM   f6502_read(&core->memory, PC++)
#define D_ABS   f6502_read(&core->memory, A_ABS)
#define D_ABSX  ({                                  \
    register uint16_t addr_base = A_ABS;            \
    register uint16_t addr = addr_base + X;         \
    CLK((addr_base & 0x100U) != (addr & 0x100U));   \
    f6502_read(&core->memory, addr);                \
})
#define D_ABSY  ({                                  \
    register uint16_t addr_base = A_ABS;            \
    register uint16_t addr = addr_base + Y;         \
    CLK((addr_base & 0x100U) != (addr & 0x100U));   \
    f6502_read(&core->memory, addr);                \
})
#define D_ZP    f6502_read_zp(&core->memory, A_ZP)
#define D_ZPX   f6502_read_zp(&core->memory, A_ZPX)
#define D_ZPY   f6502_read_zp(&core->memory, A_ZPY)
#define D_IX    f6502_read(&core->memory, A_IX)
#define D_IY    ({                                  \
    register uint16_t addr_base = f6502_read16_zp(  \
            &core->memory,                          \
            f6502_read(&core->memory, PC++)         \
        );                                          \
    register uint16_t addr = addr_base + Y;         \
    CLK((addr_base & 0x100U) != (addr & 0x100U));   \
    f6502_read(&core->memory, addr);                \
})

#define ADC(v) {                                            \
    register uint8_t val = v;                               \
    register uint16_t res = A + val + (P & f6502_Status_C); \
    CLR(P, f6502_Status_N | f6502_Status_V |                \
           f6502_Status_Z | f6502_Status_C);                \
    SET(P, table_test[(uint8_t)res]);                       \
    SET(P, (res > 0xFFU));                                  \
    if ((~(A ^ val) & (A ^ (uint8_t)res)) & 0x80) {          \
        SET(P, f6502_Status_V);                             \
    }                                                       \
    A = res;                                                \
}
#define AND(v) A &= (v); TEST(A)
#define ASL(a) ({                                               \
    CLR(P, f6502_Status_N | f6502_Status_Z | f6502_Status_C);   \
    register uint16_t addr = a;                                 \
    register uint8_t val = f6502_read(&core->memory, addr);     \
    SET(P, table_asl[val]);                                     \
    val <<= 1;                                                  \
    clocks_elapsed += f6502_write(&core->memory, addr, val);                      \
})
#define ASL_A()                                                 \
    CLR(P, f6502_Status_N | f6502_Status_Z | f6502_Status_C);   \
    SET(P, table_asl[A]);                                       \
    A <<= 1
#ifdef F6502_HCF
#define BIF_HCF()                           \
    if (pc_prev == PC + 2) {                \
        ++PC;                               \
        clocks_elapsed = -clocks_elapsed;   \
        goto step_done;                     \
    }
#else
#define BIF_HCF()
#endif
#define BIF(cond)                                       \
    if (cond) {                                         \
        register uint16_t pc_prev = PC;                 \
        PC += (int8_t)f6502_read(&core->memory, PC);    \
        BIF_HCF();                                      \
        CLK(3 + ((pc_prev & 0x100U) != (PC & 0x100U))); \
    } else {                                            \
        CLK(2);                                         \
    }                                                   \
    ++PC
#define BIT(v) {                                                \
    register uint8_t val = (v);                                 \
    CLR(P, f6502_Status_N | f6502_Status_V | f6502_Status_Z);   \
    SET(P, (val & (f6502_Status_N | f6502_Status_V)));          \
    if (!(val & A)) SET(P, f6502_Status_Z);                     \
}
#define CP(reg, v) {                                            \
    uint16_t diff = (uint16_t)reg - (v);                        \
    CLR(P, f6502_Status_N | f6502_Status_Z | f6502_Status_C);   \
    SET(P, table_test[diff & 0xFFU]);                           \
    if (diff < 0x100) SET(P, f6502_Status_C);                   \
}
#define DEC(a) ({                                           \
    register uint16_t addr = a;                             \
    register uint8_t val = f6502_read(&core->memory, addr); \
    --val;                                                  \
    TEST(val);                                              \
    clocks_elapsed += f6502_write(&core->memory, addr, val);                  \
})
#define EOR(v) A ^= (v); TEST(A)
#define INC(a) ({                                           \
    register uint16_t addr = a;                             \
    register uint8_t val = f6502_read(&core->memory, addr); \
    ++val;                                                  \
    TEST(val);                                              \
    clocks_elapsed += f6502_write(&core->memory, addr, val);                \
})
#define JMP(a) PC = (a)
#define LD(reg, v) reg = (v); TEST(reg)
#define LSR(a) ({                                               \
    register uint16_t addr = a;                                 \
    register uint8_t val = f6502_read(&core->memory, addr);     \
    CLR(P, f6502_Status_N | f6502_Status_Z | f6502_Status_C);   \
    SET(P, val & f6502_Status_C);                               \
    val >>= 1;                                                  \
    SET(P, val & f6502_Status_N);                               \
    if (!val) SET(P, f6502_Status_Z);                           \
    clocks_elapsed += f6502_write(&core->memory, addr, val);                      \
})
#define LSR_A()                                                 \
    CLR(P, f6502_Status_N | f6502_Status_Z | f6502_Status_C);   \
    SET(P, A & f6502_Status_C);                                 \
    A >>= 1;                                                    \
    SET(P, A & f6502_Status_N);                                 \
    if (!A) SET(P, f6502_Status_Z)
#define ORA(v) A |= (v); TEST(A)
#define ROL(a) ({                                               \
    register uint16_t addr = a;                                 \
    register uint8_t val = f6502_read(&core->memory, addr);     \
    register uint8_t new_c = (val & 0x80) ? f6502_Status_C : 0;   \
    val = (val << 1) | (P & f6502_Status_C);                    \
    CLR(P, f6502_Status_C | f6502_Status_Z | f6502_Status_N);   \
    P |= new_c;                                                 \
    if (!val) SET(P, f6502_Status_Z);                           \
    SET(P, val & f6502_Status_N);                               \
    clocks_elapsed += f6502_write(&core->memory, addr, val);                      \
})
#define ROL_A() {                                               \
    register uint8_t new_c = (A & 0x80) ? f6502_Status_C : 0;   \
    A = (A << 1) | (P & f6502_Status_C);                        \
    CLR(P, f6502_Status_C | f6502_Status_Z | f6502_Status_N);   \
    P |= new_c;                                                 \
    if (!A) SET(P, f6502_Status_Z);                             \
    SET(P, A & f6502_Status_N);                                 \
}
#define ROR(a) ({                                               \
    register uint16_t addr = a;                                 \
    register uint8_t val = f6502_read(&core->memory, addr);     \
    register uint8_t new_c = (val & f6502_Status_C);            \
    val >>= 1;                                                  \
    if (P & f6502_Status_C) val |= f6502_Status_N;              \
    CLR(P, f6502_Status_C | f6502_Status_Z | f6502_Status_N);   \
    P |= new_c;                                                 \
    if (!val) SET(P, f6502_Status_Z);                           \
    SET(P, (val & f6502_Status_N));                             \
    clocks_elapsed += f6502_write(&core->memory, addr, val);                      \
})
#define ROR_A() {                                               \
    register uint8_t new_c = (A & f6502_Status_C);              \
    A >>= 1;                                                    \
    if (P & f6502_Status_C) A |= f6502_Status_N;                \
    CLR(P, f6502_Status_C | f6502_Status_Z | f6502_Status_N);   \
    P |= new_c;                                                 \
    if (!A) SET(P, f6502_Status_Z);                             \
    SET(P, (A & f6502_Status_N));                               \
}
#define SBC(v) {                                            \
    register uint8_t val = v;                               \
    register uint16_t res = A - val - (~P & f6502_Status_C);\
    CLR(P, f6502_Status_N | f6502_Status_V |                \
           f6502_Status_Z | f6502_Status_C);                \
    SET(P, table_test[(uint8_t)res]);                       \
    SET(P, (res < 0x100U));                                 \
    if (((A ^ val) & (A ^ (uint8_t)res)) & 0x80) {          \
        SET(P, f6502_Status_V);                             \
    }                                                       \
    A = res;                                                \
}
#define ST(reg, a) clocks_elapsed += f6502_write(&core->memory, (a), reg)


static int f6502_do_step(f6502_Core* core, int clocks) {
    register int clocks_elapsed = 0;

    register uint16_t PC = core->registers.PC;
    register uint8_t   S = core->registers.S;
    register uint8_t   A = core->registers.A;
    register uint8_t   X = core->registers.X;
    register uint8_t   Y = core->registers.Y;
    register uint8_t   P = core->registers.P;

    while (clocks_elapsed < clocks) {
        uint8_t opcode = f6502_read(&core->memory, PC++);
#ifdef F6502_TRACE
        printf("S:%02x A:%02x X:%02x Y:%02x P:%02x\n",
               S, A, X, Y, P);

        printf("$%04x:", (int)S + f6502_Base_Stack);
        for (int i = S; i < 256; ++i)
            printf(" %02x", (int)f6502_read(&core->memory, i + f6502_Base_Stack));
        putc('\n', stdout);

//        printf("INT %02x\n", core->interrupts);
        uint16_t operand = f6502_read16(&core->memory, PC);

        printf("$%04x: ", PC - 1);
        f6502_dump_instr(core, opcode, (uint8_t*)&operand);
        printf(" -> ");

        f6502_fprintf_instr(stdout, opcode, (uint8_t*)&operand);
        putc('\n', stdout);
#endif
        switch (opcode) {
        case 0x00:  // BRK
            ++PC;
            PUSH16(core, S, PC);
            PUSH(core, S, P | f6502_Status_B | f6502_Status_1);
            SET(P, f6502_Status_I);
            PC = f6502_read16(&core->memory, f6502_Vector_IRQ);
            CLK(7);
            break;

        case 0x01:  // ORA (zp, X)
            ORA(D_IX);
            CLK(6);
            break;

        // Undefined: 0x02 - 0x04

        case 0x05:  // ORA zp
            ORA(D_ZP);
            CLK(3);
            break;

        case 0x06:  // ASL zp
            ASL(A_ZP);
            CLK(5);
            break;

        // Undefined: 0x07

        case 0x08:  // PHP
            SET(P, f6502_Status_B);
            PUSH(core, S, P);
            CLK(3);
            break;

        case 0x09:  // ORA imm
            ORA(D_IMM);
            CLK(2);
            break;

        case 0x0A:  // ASL A
            ASL_A();
            CLK(2);
            break;

        // Undefined: 0x0B
        // Special: 0x0C

        case 0x0D:  // ORA abs
            ORA(D_ABS);
            CLK(4);
            break;

        case 0x0E:  // ASL abs
            ASL(A_ABS);
            CLK(6);
            break;

        // Undefined: 0x0F

        case 0x10:  // BPL
            BIF(!(P & f6502_Status_N));
            break;

        case 0x11:  // ORA (zp), Y
            ORA(D_IY);
            CLK(5);
            break;

        // Undefined: 0x12 - 0x13
        // Special: 0x14

        case 0x15:  // ORA zp, X
            ORA(D_ZPX);
            CLK(4);
            break;

        case 0x16:  // ASL zp, X
            ASL(A_ZPX);
            CLK(4);
            break;

        // Undefined: 0x17

        case 0x18:  // CLC
            CLR(P, f6502_Status_C);
            CLK(2);
            break;

        case 0x19:  // ORA abs, Y
            ORA(D_ABSY);
            CLK(4);
            break;

        // Special: 0x1A
        // Undefined: 0x1B
        // Special: 0x1C

        case 0x1D:  // ORA abs, X
            ORA(D_ABSX);
            CLK(4);
            break;

        case 0x1E:  // ASL abs, X
            ASL(A_ABSX);
            CLK(7);
            break;

        // Undefined: 0x1F

        case 0x20:  // JSR
        {   uint16_t addr_lo = f6502_read(&core->memory, PC++);
            uint16_t addr_hi = f6502_read(&core->memory, PC);
            PUSH16(core, S, PC);
            PC = addr_lo | (addr_hi << 8);
            CLK(6);
        }   break;

        case 0x21:  // AND (zp, X)
            AND(D_IX);
            CLK(6);
            break;

        // Undefined: 0x22 - 0x23

        case 0x24:  // BIT zp
            BIT(D_ZP);
            CLK(3);
            break;

        case 0x25:  // AND zp
            AND(D_ZP);
            CLK(3);
            break;

        case 0x26:  // ROL zp
            ROL(A_ZP);
            CLK(5);
            break;

        // Undefined: 0x27

        case 0x28:  // PLP
            POP(core, S, P);
            SET(P, f6502_Status_1 | f6502_Status_B);
            CLK(4);
            break;

        case 0x29:  // AND imm
            AND(D_IMM);
            CLK(2);
            break;

        case 0x2A:  // ROL A
            ROL_A();
            CLK(2);
            break;

        // Undefined: 0x2B

        case 0x2C:  // BIT abs
            BIT(D_ABS);
            CLK(4);
            break;

        case 0x2D:  // AND abs
            AND(D_ABS);
            CLK(4);
            break;

        case 0x2E:  // ROL abs
            ROL(A_ABS);
            CLK(6);
            break;

        // Undefined: 0x2F

        case 0x30:  // BMI
            BIF(P & f6502_Status_N);
            break;

        case 0x31:  // AND (zp), Y
            AND(D_IY);
            CLK(5);
            break;

        // Undefined: 0x32 - 0x33
        // Special: 0x34

        case 0x35:  // AND zp, X
            AND(D_ZPX);
            CLK(4);
            break;

        case 0x36:  // ROL zp, X
            ROL(A_ZPX);
            CLK(6);
            break;

        // Undefined: 0x37

        case 0x38:  // SEC
            SET(P, f6502_Status_C);
            CLK(2);
            break;

        case 0x39:  // AND abs, Y
            AND(D_ABSY);
            CLK(4);
            break;

        // Special: 0x3A
        // Undefined: 0x3B
        // Special: 0x3C

        case 0x3D:  // AND abs, X
            AND(D_ABSX);
            CLK(4);
            break;

        case 0x3E:  // ROL abs, X
            ROL(A_ABSX);
            CLK(7);
            break;

        // Undefined: 0x3F

        case 0x40:  // RTI
            POP(core, S, P);
            SET(P, f6502_Status_1 | f6502_Status_B);
            POP16(core, S, PC);
            CLK(6);
            // Interrupt might be triggered; break out
            clocks = 0;
            break;

        case 0x41:  // EOR (zp, X)
            EOR(D_IX);
            CLK(6);
            break;

        // Undefined: 0x42 - 0x43
        // Special: 0x44

        case 0x45:  // EOR zp
            EOR(D_ZP);
            CLK(3);
            break;

        case 0x46:  // LSR zp
            LSR(A_ZP);
            CLK(5);
            break;

        // Undefined: 0x47

        case 0x48:  // PHA
            PUSH(core, S, A);
            CLK(3);
            break;

        case 0x49:  // EOR imm
            EOR(D_IMM);
            CLK(2);
            break;

        case 0x4A:  // LSR A
            LSR_A();
            CLK(2);
            break;

        // Undefined: 0x4B

        case 0x4C:  // JMP abs
#ifdef F6502_HCF
        {   uint16_t addr = A_ABS;
            if (PC - 3 == addr) {
                JMP(addr);
                clocks_elapsed = -clocks_elapsed;
                goto step_done;
            }
            JMP(addr);
        }
#else
            JMP(A_ABS);
#endif
            CLK(3);
            break;

        case 0x4D:  // EOR abs
            EOR(D_ABS);
            CLK(4);
            break;

        case 0x4E:  // LSR abs
            LSR(A_ABS);
            CLK(6);
            break;

        // Undefined: 0x4F

        case 0x50:  // BVC
            BIF(!(P & f6502_Status_V));
            break;

        case 0x51:  // EOR (zp), Y
            EOR(D_IY);
            CLK(5);
            break;

        // Undefined: 0x52 - 0x53
        // Special: 0x54

        case 0x55:  // EOR zp, X
            EOR(D_ZPX);
            CLK(4);
            break;

        case 0x56:  // LSR zp, X
            LSR(A_ZPX);
            CLK(6);
            break;

        // Undefined: 0x57

        case 0x58:  // CLI
        {   register uint8_t p_old = P;
            CLR(P, f6502_Status_I);
            CLK(2);
            if (    (p_old & f6502_Status_I) &&
                    (core->interrupts & f6502_Int_IRQ)) {
                CLK(7);
                PUSH16(core, S, PC);
                PUSH(core, S, P & ~f6502_Status_B);
                SET(P, f6502_Status_I);
                PC = f6502_read16(&core->memory,
                                  f6502_Vector_IRQ);
            }

        }   break;

        case 0x59:  // EOR abs, Y
            EOR(D_ABSY);
            CLK(4);
            break;

        // Special: 0x5A
        // Undefined: 0x5B
        // Special: 0x5C

        case 0x5D:  // EOR abs, X
            EOR(D_ABSX);
            CLK(4);
            break;

        case 0x5E:  // LSR abs, X
            LSR(A_ABSX);
            CLK(7);
            break;

        // Undefined: 0x5F

        case 0x60:  // RTS
            POP16(core, S, PC);
            ++PC;
            CLK(6);
            break;

        case 0x61:  // ADC (zp, X)
            ADC(D_IX);
            CLK(6);
            break;

        // Undefined: 0x62 - 0x63
        // Special: 0x64

        case 0x65:  // ADC zp
            ADC(D_ZP);
            CLK(3);
            break;

        case 0x66:  // ROR zp
            ROR(A_ZP);
            CLK(5);
            break;

        // Undefined: 0x67

        case 0x68:  // PLA
            POP(core, S, A);
            TEST(A);
            CLK(4);
            break;

        case 0x69:  // ADC imm
            ADC(D_IMM);
            CLK(2);
            break;

        case 0x6A:  // ROR A
            ROR_A();
            CLK(2);
            break;

        // Undefined: 0x6B

        case 0x6C:  // JMP (abs)
        {   // Reproduce the indirect addressing bug
            register uint16_t addr = A_ABS;
            register uint8_t addr_lo = f6502_read(&core->memory,
                                                  addr);
            if ((addr & 0xFFU) == 0xFFU) {
                addr = f6502_read(&core->memory, addr - 0xffU);
            } else {
                addr = f6502_read(&core->memory, addr + 1);
            }
            JMP(addr_lo | (addr << 8));
            CLK(5);
        }   break;

        case 0x6D:  // ADC abs
            ADC(D_ABS);
            CLK(4);
            break;

        case 0x6E:  // ROR abs
            ROR(A_ABS);
            CLK(6);
            break;

        // Undefined: 0x6F

        case 0x70:  // BVS
            BIF(P & f6502_Status_V);
            break;

        case 0x71:  // ADC (zp), Y
            ADC(D_IY);
            CLK(5);
            break;

        // Undefined: 0x72 - 0x73
        // Special: 0x74

        case 0x75:  // ADC zp, X
            ADC(D_ZPX);
            CLK(4);
            break;

        case 0x76:  // ROR zp, X
            ROR(A_ZPX);
            CLK(6);
            break;

        // Undefined: 0x77

        case 0x78:  // SEI
            SET(P, f6502_Status_I);
            CLK(2);
            break;

        case 0x79:  // ADC abs, Y
            ADC(D_ABSY);
            CLK(4);
            break;

        // Special: 0x7A
        // Undefined: 0x7B
        // Special: 0x7C

        case 0x7D:  // ADC abs, X
            ADC(D_ABSX);
            CLK(4);
            break;

        case 0x7E:  // ROR abs, X
            ROR(A_ABSX);
            CLK(7);
            break;

        // Undefined: 0x7F
        // Special: 0x80

        case 0x81:  // STA (zp, X)
            ST(A, A_IX);
            CLK(6);
            break;

        // Special: 0x82
        // Undefined: 0x83

        case 0x84:  // STY zp
            ST(Y, A_ZP);
            CLK(3);
            break;

        case 0x85:  // STA zp
            ST(A, A_ZP);
            CLK(3);
            break;

        case 0x86:  // STX zp
            ST(X, A_ZP);
            CLK(3);
            break;

        // Undefined: 0x87

        case 0x88:  // DEY
            --Y;
            TEST(Y);
            CLK(2);
            break;

        // Undefined: 0x89

        case 0x8A:  // TXA
            A = X;
            TEST(A);
            CLK(2);
            break;

        // Undefined: 0x8B

        case 0x8C:  // STY abs
            ST(Y, A_ABS);
            CLK(4);
            break;

        case 0x8D:  // STA abs
            ST(A, A_ABS);
            CLK(4);
            break;

        case 0x8E:  // STX abs
            ST(X, A_ABS);
            CLK(4);
            break;

        case 0x90:  // BCC
            BIF(!(P & f6502_Status_C));
            break;

        case 0x91:  // STA (zp), Y
            ST(A, A_IY);
            CLK(6);
            break;

        // Undefined: 0x92 - 0x93

        case 0x94:  // STY zp, X
            ST(Y, A_ZPX);
            CLK(4);
            break;

        case 0x95:  // STA zp, X
            ST(A, A_ZPX);
            CLK(4);
            break;

        case 0x96:  // STX zp, Y
            ST(X, A_ZPY);
            CLK(4);
            break;

        // Undefined: 0x97

        case 0x98:  // TYA
            A = Y;
            TEST(A);
            CLK(2);
            break;

        case 0x99:  //  STA abs, Y
            ST(A, A_ABSY);
            CLK(5);
            break;

        case 0x9A:  // TXS
            S = X;
            CLK(2);
            break;

        // Undefined: 0x9B - 0x9C

        case 0x9D:  // STA abs, X
            ST(A, A_ABSX);
            CLK(5);
            break;

        // Undefined: 0x9E - 0x9F

        case 0xA0:  // LDY imm
            LD(Y, D_IMM);
            CLK(2);
            break;

        case 0xA1:  // LDA (zp, X)
            LD(A, D_IX);
            CLK(6);
            break;

        case 0xA2:  // LDX imm
            LD(X, D_IMM);
            CLK(2);
            break;

        // Undefined: 0xA3

        case 0xA4:  // LDY zp
            LD(Y, D_ZP);
            CLK(3);
            break;

        case 0xA5:  // LDA zp
            LD(A, D_ZP);
            CLK(3);
            break;

        case 0xA6:  // LDX zp
            LD(X, D_ZP);
            CLK(3);
            break;

        // Undefined: 0xA7

        case 0xA8:  // TAY
            Y = A;
            TEST(A);
            CLK(2);
            break;

        case 0xA9:  // LDA imm
            LD(A, D_IMM);
            CLK(2);
            break;

        case 0xAA:  // TAX
            X = A;
            TEST(A);
            CLK(2);
            break;

        // Undefined: 0xAB

        case 0xAC:  // LDY abs
            LD(Y, D_ABS);
            CLK(4);
            break;

        case 0xAD:  // LDA abs
            LD(A, D_ABS);
            CLK(4);
            break;

        case 0xAE:  // LDX abs
            LD(X, D_ABS);
            CLK(4);
            break;

        // Undefined: 0xAF

        case 0xB0:  // BCS
            BIF(P & f6502_Status_C);
            break;

        case 0xB1:  // LDA (zp), Y
            LD(A, D_IY);
            CLK(5);
            break;

        // Undefined: 0xB2 - 0xB3

        case 0xB4:  // LDY zp, X
            LD(Y, D_ZPX);
            CLK(4);
            break;

        case 0xB5:  // LDA zp, X
            LD(A, D_ZPX);
            CLK(4);
            break;

        case 0xB6:  // LDX zp, Y
            LD(X, D_ZPY);
            CLK(4);
            break;

        // Undefined: 0xB7

        case 0xB8:  // CLV
            CLR(P, f6502_Status_V);
            CLK(2);
            break;

        case 0xB9:  // LDA abs, Y
            LD(A, D_ABSY);
            CLK(4);
            break;

        case 0xBA:  // TSX
            X = S;
            TEST(X);
            CLK(2);
            break;

        // Undefined: 0xBB

        case 0xBC:  // LDY abs, X
            LD(Y, D_ABSX);
            CLK(4);
            break;

        case 0xBD:  // LDA abs, X
            LD(A, D_ABSX);
            CLK(4);
            break;

        case 0xBE:  // LDX abs, Y
            LD(X, D_ABSY);
            CLK(4);
            break;

        // Undefined: 0xBF

        case 0xC0:  // CPY imm
            CP(Y, D_IMM);
            CLK(2);
            break;

        case 0xC1:  // CMP (zp, X)
            CP(A, D_IX);
            CLK(6);
            break;

        // Special: 0xC2
        // Undefined: 0xC3

        case 0xC4:  // CPY zp
            CP(Y, D_ZP);
            CLK(3);
            break;

        case 0xC5:  // CMP zp
            CP(A, D_ZP);
            CLK(3);
            break;

        case 0xC6:  // DEC zp
            DEC(A_ZP);
            CLK(5);
            break;

        // Undefined: 0xC7

        case 0xC8:  // INY
            ++Y;
            TEST(Y);
            CLK(2);
            break;

        case 0xC9:  // CMP imm
            CP(A, D_IMM);
            CLK(2);
            break;

        case 0xCA:  // DEX
            --X;
            TEST(X);
            CLK(2);
            break;

        // Undefined: 0xCB

        case 0xCC:  // CPY abs
            CP(Y, D_ABS);
            CLK(4);
            break;

        case 0xCD:  // CMP abs
            CP(A, D_ABS);
            CLK(4);
            break;

        case 0xCE:  // DEC abs
            DEC(A_ABS);
            CLK(6);
            break;

        // Undefined: 0xCF

        case 0xD0:  // BNE
            BIF(!(P & f6502_Status_Z));
            break;

        case 0xD1:  // CMP (zp), Y
            CP(A, D_IY);
            CLK(5);
            break;

        // Undefined: 0xD2 - 0xD3
        // Special: 0xD4

        case 0xD5:  // CMP zp, X
            CP(A, D_ZPX);
            CLK(4);
            break;

        case 0xD6:  // DEC zp, X
            DEC(A_ZPX);
            CLK(6);
            break;

        // Undefined: 0xD7

        case 0xD8:  // CLD
            CLR(P, f6502_Status_D);
            CLK(2);
            break;

        case 0xD9:  // CMP abs, Y
            CP(A, D_ABSY);
            CLK(4);
            break;

        // Special: 0xDA
        // Undefined: 0xDB
        // Special: 0xDC

        case 0xDD:  // CMP abs, X
            CP(A, D_ABSX);
            CLK(4);
            break;

        case 0xDE:  // DEC abs, X
            DEC(A_ABSX);
            CLK(7);
            break;

        // Undefined: 0xDF

        case 0xE0:  // CPX imm
            CP(X, D_IMM);
            CLK(2);
            break;

        case 0xE1:  // SBC (xp, X)
            SBC(D_IX);
            CLK(6);
            break;

        // Special: 0xE2
        // Undefined: 0xE3

        case 0xE4:  // CPX zp
            CP(X, D_ZP);
            CLK(3);
            break;

        case 0xE5:  // SBC zp
            SBC(D_ZP);
            CLK(3);
            break;

        case 0xE6:  // INC zp
            INC(A_ZP);
            CLK(5);
            break;

        case 0xE8:  // INX
            ++X;
            TEST(X);
            CLK(2);
            break;

        case 0xE9:  // SBC imm
            SBC(D_IMM);
            CLK(2);
            break;

        case 0xEA:  // NOP
            CLK(2);
            break;

        // Undefined: 0xEB

        case 0xEC:  // CPX abs
            CP(X, D_ABS);
            CLK(4);
            break;

        case 0xED:  // SBC abs
            SBC(D_ABS);
            CLK(4);
            break;

        case 0xEE:  // INC abs
            INC(A_ABS);
            CLK(6);
            break;

        // Undefined: 0xEF

        case 0xF0:  // BEQ
            BIF(P & f6502_Status_Z);
            break;

        case 0xF1:  // SBC (zp), Y
            SBC(D_IY);
            CLK(5);
            break;

        // Undefined: 0xF2 - 0xF3
        // Special: 0xF4

        case 0xF5:  // SBC zp, X
            SBC(D_ZPX);
            CLK(4);
            break;

        case 0xF6:  // INC zp, X
            INC(A_ZPX);
            CLK(6);
            break;

        // Undefined: 0xF7

        case 0xF8:  // SED
            SET(P, f6502_Status_D);
            CLK(2);
            break;

        case 0xF9:  // SBC abs, Y
            SBC(D_ABSY);
            CLK(4);
            break;

        // Special: 0xFA
        // Undefined: 0xFB
        // Special: 0xFC

        case 0xFD:  // SBC abs, X
            SBC(D_ABSX);
            CLK(4);
            break;

        case 0xFE:  // INC abs, X
            INC(A_ABSX);
            CLK(7);
            break;

        /* "Special" i.e. "unofficial" instructions */
        /* Timing & progression only, no side effects */

        // DOP, 2 Cycles
        case 0x80:
        case 0x82:
        case 0x89:
        case 0xC2:
        case 0xE2:
            ++PC;
            CLK(2);
            break;

        // DOP, 3 Cycles
        case 0x04:
        case 0x44:
        case 0x64:
            ++PC;
            CLK(3);
            break;

        // DOP, 4 Cycles
        case 0x14:
        case 0x34:
        case 0x54:
        case 0x74:
        case 0xD4:
        case 0xF4:
            ++PC;
            CLK(4);
            break;

        // TOP, 4 Cycles
        case 0x1C:
        case 0x3C:
        case 0x5C:
        case 0x7C:
        case 0xDC:
        case 0xFC:
            PC += 2;
            CLK(4);
            break;

        // NOP, 2 Cycles
        default:
            CLK(2);
            break;
        }
    }

#ifdef F6502_TRACE
//    if (int_trig) printf("Possible interrupt trigger\n");
#endif

#ifdef F6502_HCF
step_done:
#endif
    core->registers = (f6502_Registers){
        .PC = PC,
        .S = S,
        .A = A,
        .X = X,
        .Y = Y,
        .P = P,
    };

    return clocks_elapsed;
}

int f6502_step(f6502_Core* core, int clocks) {
    int clocks_elapsed = 0;
    // TODO: Why are we processing seven clocks prior to NMI?
    // This passes the interrupt test with 6 steps
/*
    if (f6502_nmi_ready(core)) {
        clocks_elapsed += f6502_do_step(core, 7);
        clocks -= clocks_elapsed;
    }
*/
    clocks_elapsed += f6502_check_interrupts(core);
    return (    f6502_do_step(core, clocks - clocks_elapsed) +
                clocks_elapsed);
}