#include <lcq/6502.h>
#include <lcq/6502/ins.h>

#include <stdlib.h>

static u8 basic_read_handler(void *data, u16 addr) {
    return ((u8 *) data)[addr];
}
static void basic_write_handler(void *data, u16 addr, u8 val) {
    ((u8 *) data)[addr] = val;
}
void l6502_mem_basic_new(l6502_mem *m) {
    m->rh = basic_read_handler;
    m->wh = basic_write_handler;
    m->data = calloc(1 << 16, sizeof(u8));
}

static void flags_check_zn(l6502_emu *e, u8 v) {
    l6502_flag_set(e, L6502_FLAG_ZERO, v == 0);
    l6502_flag_set(e, L6502_FLAG_NEGATIVE, (v >> 7) & 0b1);
}

static u16 amode_immediate(l6502_emu *e) {
    return e->pc++;
}
static u16 amode_absolute(l6502_emu *e) {
    u16 addr = l6502_read_addr(e, e->pc);
    e->pc += 2;
    return addr;
}
static u16 amode_absolute_x(l6502_emu *e) {
    u16 addr = l6502_read_addr(e, e->pc) + (u16) e->regs[L6502_REG_X];
    e->pc += 2;
    return addr;
}
static u16 amode_absolute_y(l6502_emu *e) {
    u16 addr = l6502_read_addr(e, e->pc) + (u16) e->regs[L6502_REG_Y];
    e->pc += 2;
    return addr;
}
static u16 amode_zeropage(l6502_emu *e) {
    return (u16) l6502_read(e, e->pc++);
}
static u16 amode_zeropage_x(l6502_emu *e) {
    return (u16) (l6502_read(e, e->pc++) + e->regs[L6502_REG_X]);
}
static u16 amode_indirect_zeropage_x(l6502_emu *e) {
    return l6502_read_addr(e, l6502_read(e, e->pc++) + (u16) e->regs[L6502_REG_X]);
}
static u16 amode_indirect_zeropage_y(l6502_emu *e) {
    return l6502_read_addr(e, l6502_read(e, e->pc++)) + (u16) e->regs[L6502_REG_Y];
}

void l6502_emulate(l6502_emu *e) {
    u8 ins = l6502_read(e, e->pc++);
    u8 val = 0;
    u16 res = 0;
    bool set_flags = true;
    bool set_result = true;
    l6502_reg set_reg = L6502_REG__MARKER;
    u16 addr = 0;
    switch (ins) { /* identify some special-case instructions */
    case BRK:
        l6502_push_addr(e, e->pc);
        l6502_push(e, e->regs[L6502_REG_FLAGS]);
        e->pc = l6502_read_addr(e, 0xfffe) - 1;
        l6502_flag_set(e, L6502_FLAG_BREAK_COMMAND, true);
        break;
    case PHP: l6502_push(e, e->regs[L6502_REG_FLAGS]); break;
    case CLC: l6502_flag_set(e, L6502_FLAG_CARRY, false); break;
    case JSR:
        addr = l6502_read_addr(e, e->pc);
        l6502_push_addr(e, e->pc + 1);
        e->pc = addr;
        break;
    case PLP: e->regs[L6502_REG_FLAGS] = l6502_pop(e); break;
    case SEC: l6502_flag_set(e, L6502_FLAG_CARRY, true); break;
    case RTI:
        e->regs[L6502_REG_FLAGS] = l6502_pop(e);
        e->pc = l6502_pop_addr(e);
        break;
    case PHA: l6502_push(e, e->regs[L6502_REG_A]); break;
    case JMP_ABSOLUTE:
        addr = l6502_read_addr(e, e->pc);
        e->pc = addr;
        break;
    case CLI: l6502_flag_set(e, L6502_FLAG_INTERRUPT_DISABLE, false); break;
    case RTS: e->pc = l6502_pop_addr(e) + 1; break;
    case PLA: e->regs[L6502_REG_A] = l6502_pop(e); break;
    case JMP_INDIRECT:
        addr = l6502_read_addr(e, l6502_read_addr(e, e->pc));
        e->pc = addr;
        break;
    case SEI: l6502_flag_set(e, L6502_FLAG_INTERRUPT_DISABLE, true); break;
    case DEY: e->regs[L6502_REG_Y] -= 1; flags_check_zn(e, e->regs[L6502_REG_Y]); break;
    case TXA: e->regs[L6502_REG_A] = e->regs[L6502_REG_X]; flags_check_zn(e, e->regs[L6502_REG_A]); break;
    case TYA: e->regs[L6502_REG_A] = e->regs[L6502_REG_Y]; flags_check_zn(e, e->regs[L6502_REG_A]); break;
    case TXS: e->regs[L6502_REG_SP] = e->regs[L6502_REG_X]; break; /* note TXS does not set flags */
    case TAY: e->regs[L6502_REG_Y] = e->regs[L6502_REG_A]; flags_check_zn(e, e->regs[L6502_REG_Y]); break;
    case TAX: e->regs[L6502_REG_X] = e->regs[L6502_REG_A]; flags_check_zn(e, e->regs[L6502_REG_X]); break;
    case CLV: l6502_flag_set(e, L6502_FLAG_OVERFLOW, false); break;
    case TSX: e->regs[L6502_REG_X] = e->regs[L6502_REG_SP]; flags_check_zn(e, e->regs[L6502_REG_X]); break;
    case INY: e->regs[L6502_REG_Y] += 1; flags_check_zn(e, e->regs[L6502_REG_Y]); break;
    case DEX: e->regs[L6502_REG_X] -= 1; flags_check_zn(e, e->regs[L6502_REG_X]); break;
    case CLD: l6502_flag_set(e, L6502_FLAG_DECIMAL, false); break;
    case INX: e->regs[L6502_REG_X] += 1; flags_check_zn(e, e->regs[L6502_REG_X]); break;
    case NOP: break;
    case SED: l6502_flag_set(e, L6502_FLAG_DECIMAL, true); break;
    }
    if ((ins & 0b11111) == 0b10000) { /* identify conditional branches */
        l6502_flag flags[4] = { L6502_FLAG_NEGATIVE, L6502_FLAG_OVERFLOW, L6502_FLAG_CARRY, L6502_FLAG_ZERO };
        u8 off = l6502_read(e, e->pc++);
        if (l6502_flag_get(e, flags[(ins >> 6) & 0b11]) == ((ins >> 5) & 0b1)) {
            e->pc = (u16) ((i16) e->pc + (i16) (i8) off);
        }
        return;
    }
    switch (ins & 0b11) { /* identify instructions in common forms */
    case 0b01: { /* group one */
        set_reg = L6502_REG_A;
        switch ((ins >> 2) & 0b111) {
        case G1_ADDRESSING_MODE_INDIRECT_ZEROPAGE_X: addr = amode_indirect_zeropage_x(e); break;
        case G1_ADDRESSING_MODE_ZEROPAGE: addr = amode_zeropage(e); break;
        case G1_ADDRESSING_MODE_IMMEDIATE: addr = amode_immediate(e); break;
        case G1_ADDRESSING_MODE_ABSOLUTE: addr = amode_absolute(e); break;
        case G1_ADDRESSING_MODE_INDIRECT_ZEROPAGE_Y: addr = amode_indirect_zeropage_y(e); break;
        case G1_ADDRESSING_MODE_ZEROPAGE_X: addr = amode_zeropage_x(e); break;
        case G1_ADDRESSING_MODE_ABSOLUTE_Y: addr = amode_absolute_y(e); break;
        case G1_ADDRESSING_MODE_ABSOLUTE_X: addr = amode_absolute_x(e); break;
        }
        val = l6502_read(e, addr);
        switch ((ins >> 5) & 0b111) {
        case G1_OPCODE_ORA: res = (u16) e->regs[L6502_REG_A] | (u16) val; break;
        case G1_OPCODE_AND: res = (u16) e->regs[L6502_REG_A] & (u16) val; break;
        case G1_OPCODE_EOR: res = (u16) e->regs[L6502_REG_A] ^ (u16) val; break;
        case G1_OPCODE_ADC:
            res = (u16) e->regs[L6502_REG_A] + (u16) val;
            res += (u16) l6502_flag_get(e, L6502_FLAG_CARRY);
            l6502_flag_set(e, L6502_FLAG_OVERFLOW, ((res >> 8) != 0) != ((res >> 7) & 0b1));
            break;
        case G1_OPCODE_STA: l6502_write(e, addr, e->regs[L6502_REG_A]); set_flags = set_result = false; break;
        case G1_OPCODE_LDA: res = (u16) l6502_read(e, addr); break;
        case G1_OPCODE_CMP:
            set_result = false;
            res = (u16) e->regs[L6502_REG_A] - (u16) val;
            break;
        case G1_OPCODE_SBC:
            res = (u16) e->regs[L6502_REG_A] - (u16) val;
            res -= (u16) (1 - l6502_flag_get(e, L6502_FLAG_CARRY));
            l6502_flag_set(e, L6502_FLAG_OVERFLOW, ((res >> 8) != 0) != ((res >> 7) & 0b1));
            break;
        }
        break;
    }
    case 0b10: { /* group two */
        switch ((ins >> 2) & 0b111) {
        case G2_ADDRESSING_MODE_IMMEDIATE: addr = amode_immediate(e); break;
        case G2_ADDRESSING_MODE_ZEROPAGE: addr = amode_zeropage(e); break;
        case G2_ADDRESSING_MODE_ACCUMULATOR: addr = (u16) e->regs[L6502_REG_A]; set_reg = L6502_REG_A; break;
        case G2_ADDRESSING_MODE_ABSOLUTE: addr = amode_absolute(e); break;
        case G2_ADDRESSING_MODE_ZEROPAGE_X: addr = amode_zeropage_x(e); break;
        case G2_ADDRESSING_MODE_ABSOLUTE_X: addr = amode_absolute_x(e); break;
        }
        val = set_reg ? (u8) addr : l6502_read(e, addr);
        switch ((ins >> 5) & 0b111) {
        case G2_OPCODE_ASL: res = ((u16) val) << 1; break;
        case G2_OPCODE_ROL: res = ((u16) val) << 1 | (u16) l6502_flag_get(e, L6502_FLAG_CARRY); break;
        case G2_OPCODE_LSR:
            res = ((u16) val) >> 1
                | (u16) (val & 0b1) << 8; /* place the previous low bit in carry */
            break;
        case G2_OPCODE_ROR:
            res = ((u16) val) >> 1
                | (u16) l6502_flag_get(e, L6502_FLAG_CARRY) << 7
                | (u16) (val & 0b1) << 8; /* same as above */
            break;
        case G2_OPCODE_STX: l6502_write(e, addr, e->regs[L6502_REG_X]); set_flags = set_result = false; break;
        case G2_OPCODE_LDX: res = (u16) l6502_read(e, addr); set_reg = L6502_REG_X; break;
        case G2_OPCODE_DEC: res = (u16) val - 1; break;
        case G2_OPCODE_INC: res = (u16) val + 1; break;
        }
        break;
    }
    case 0b00: { /* group three */
        switch ((ins >> 2) & 0b111) {
        case G3_ADDRESSING_MODE_IMMEDIATE: addr = amode_immediate(e); break;
        case G3_ADDRESSING_MODE_ZEROPAGE: addr = amode_zeropage(e); break;
        case G3_ADDRESSING_MODE_ABSOLUTE: addr = amode_absolute(e); break;
        case G3_ADDRESSING_MODE_ZEROPAGE_X: addr = amode_zeropage_x(e); break;
        case G3_ADDRESSING_MODE_ABSOLUTE_X: addr = amode_absolute_x(e); break;
        }
        val = l6502_read(e, addr);
        switch ((ins >> 5) & 0b111) {
        case G3_OPCODE_BIT:
            set_flags = set_result = false;
            l6502_flag_set(e, L6502_FLAG_ZERO, (e->regs[L6502_REG_A] & val) == 0);
            l6502_flag_set(e, L6502_FLAG_OVERFLOW, (val >> 6) & 0b1);
            l6502_flag_set(e, L6502_FLAG_NEGATIVE, (val >> 7) & 0b1);
            break;
        case G3_OPCODE_STY: l6502_write(e, addr, e->regs[L6502_REG_Y]); set_flags = set_result = false; break;
        case G3_OPCODE_LDY: res = (u16) l6502_read(e, addr); set_reg = L6502_REG_Y; break;
        case G3_OPCODE_CPY:
            set_result = false;
            res = (u16) e->regs[L6502_REG_Y] - (u16) val;
            break;
        case G3_OPCODE_CPX:
            set_result = false;
            res = (u16) e->regs[L6502_REG_X] - (u16) val;
            break;
        }
    }
    }
    if (set_flags) {
        l6502_flag_set(e, L6502_FLAG_ZERO, res == 0);
        l6502_flag_set(e, L6502_FLAG_NEGATIVE, (res >> 7) & 0b1);
        l6502_flag_set(e, L6502_FLAG_CARRY, (res >> 8) != 0);
    }
    if (set_result) {
        if (set_reg != L6502_REG__MARKER) e->regs[set_reg] = (u8) res;
        else l6502_write(e, addr, (u8) res);
    }
}