#include <malloc.h>
#include "hs/cpu.h"
#include "bus.h"
#include "6502.h"


#define IS_IRQ_TRIGGER(cpu, t) \
            ((((cpu->irq)>>t)&0x01)==0x01) \

#define CLEAR_IRQ(cpu, t) \
        (cpu->irq = (cpu->irq & (~(1<<t)))); \

#define TRIGGER_IRQ(cpu, t) \
                (cpu->irq = ((cpu->irq)|(1<<t)));\

#define UTF8_CHAR_CACHE (10240)


extern CPU *ines_cpu_new() {
    CPU *cpu = malloc(sizeof(CPU));
    // A, X, Y = 0
    cpu->a = 0;
    cpu->x = 0;
    cpu->y = 0;
    cpu->irq = 0;
    cpu->cycle = 0;
    cpu->suspend = 0;
    cpu->synchronized = 0;
    // S = $FD[3]
    cpu->s = STACK_PTR_SPOS;
    // P = $34 (IRQ disabled)
    cpu->p = DEFAULT_STATUS_VAL;
    TRIGGER_IRQ(cpu, RESET)
    return cpu;
}


static void ines_xy_inc_impl(CPU *cpu, bool x) {
    uint8 tmp;
    if (x) {
        tmp = cpu->x = cpu->x + 1;
    } else {
        tmp = cpu->y = cpu->y + 1;
    }
    INES_NZS(cpu, tmp)
}

static void ines_beq_bne_impl(NesConsole *console, bool bne, AddressMode mode) {
    CPU *cpu = console->cpu;
    bool set = INES_CPU_STATE(cpu, ZERO)
    if ((set && !bne) || (!set && bne)) {
        cpu->cycle += 1;
        uint16 base = cpu->pc + 1;
        byte offset = (byte) ines_ops_operand(console, mode);
        uint16 addr = base + offset;
        INES_CPU_PAGE_CROSS(console, base, addr, 2)
        cpu->pc = addr;
    }
}

static inline void ines_jsr_impl(NesConsole *console) {
    CPU *cpu = console->cpu;
    ines_bus_stack_push0(console, cpu->pc + 1);
    cpu->pc = ines_ops_decode_addr(console, Absolute, False);
}

static inline void ines_rts_impl(NesConsole *console) {
    CPU *cpu = console->cpu;
    cpu->pc = ines_bus_stack_pop0(console) + 1;
}

static inline void ines_logic_impl(NesConsole *console, Instruction is, AddressMode mode) {
    CPU *cpu = console->cpu;
    uint8 a = cpu->a;
    uint8 b = ines_ops_operand0(console, mode, True);
    if (is == AND) {
        a = a & b;
    } else if (is == EOR) {
        a = a ^ b;
    } else {
        a = a | b;
    }
    INES_NZS(cpu, a)
    cpu->a = a;
}

static inline void ines_ld_axy(NesConsole *console, Instruction is, AddressMode mode) {
    CPU *cpu = console->cpu;
    uint8 b = ines_ops_operand0(console, mode, True);
    if (is == LDA) {
        cpu->a = b;
    } else if (is == LDX) {
        cpu->x = b;
    } else {
        cpu->y = b;
    }
    INES_NZS(cpu, b)
}

static inline void ines_tax_impl(CPU *cpu) {
    uint8 x = cpu->a;
    cpu->x = x;
    INES_NZS(cpu, x)
}

static inline void ines_txa_impl(CPU *cpu) {
    uint8 a = cpu->x;
    cpu->a = a;
    INES_NZS(cpu, a)
}

static inline void ines_tay_impl(CPU *cpu) {
    uint8 y = cpu->a;
    cpu->y = y;
    INES_NZS(cpu, y)
}

static inline void ines_tya_impl(CPU *cpu) {
    uint8 a = cpu->y;
    cpu->a = a;
    INES_NZS(cpu, a)
}

static inline void ines_jmp_impl(NesConsole *console, AddressMode mode) {
    uint16 addr = ines_ops_decode_addr(console, mode, False);
    console->cpu->pc = addr;
}

static inline void ines_inc_impl(NesConsole *console, AddressMode mode) {
    uint16 addr = ines_ops_decode_addr(console, mode, True);
    uint8 b = ines_bus_read(console, addr);
    b++;
    INES_NZS(console->cpu, b)
    ines_bus_write(console, addr, b);
}

static inline void ines_asl_lsr_impl(NesConsole *console, Instruction is, AddressMode mode) {
    CPU *cpu = console->cpu;
    uint8 b = ines_ops_operand(console, mode);
    //The bit that was in bit 0 is shifted into the carry flag. Bit 7 is set to zero.
    ines_cpu_state(cpu, CARRY, b & 1);
    if (is == LSR) {
        b >>= 1;
    } else {
        b <<= 1;
    }
    INES_NZS(cpu, b)
    ines_ops_write(console, mode, b);
}


static inline void ines_ror_rol_impl(NesConsole *console, Instruction is, AddressMode mode) {
    CPU *cpu = console->cpu;
    bool carry;
    uint8 c = cpu->p & 0x01;
    uint8 b = ines_ops_operand(console, mode);
    if (is == ROL) {
        // Move each of the bits in either A or M one place to the left. Bit 0 is filled with the
        // current value of the carry flag whilst the old bit 7 becomes the new carry flag value.
        carry = (b & 0x80) == 0x80;
        b >>= 1;
        b = b & (c | 0xFE);
    } else {
        // Move each of the bits in either A or M one place to the right. Bit 7 is filled with the
        // current value of the carry flag whilst the old bit 0 becomes the new carry flag value.
        carry = (b & 0x01) == 0x01;
        b <<= 1;
        b = (b & 0xEF) | (c << 7);
    }
    ines_ops_write(console, mode, b);
    ines_cpu_state(cpu, CARRY, carry);
    INES_CPU_STATE_SET_NEG(cpu, b)
}

static void inline ines_xycmp_impl(NesConsole *console, Instruction is, AddressMode mode) {
    uint8 a;
    CPU *cpu = console->cpu;
    uint8 b = ines_ops_operand(console, mode);
    if (is == CPX) {
        a = cpu->x;
    } else {
        a = cpu->y;
    }
    uint8 t = a - b;
    ines_cpu_state(cpu, CARRY, a >= b);
    INES_NZS(cpu, t)
}

static void inline ines_dec_impl(NesConsole *console, AddressMode mode) {
    uint8 b = ines_ops_operand(console, mode);
    b = b - 1;
    INES_NZS(console->cpu, b);
}

static void inline ines_xydec_impl(NesConsole *console, Instruction is) {
    uint8 b;
    CPU *cpu = console->cpu;
    if (is == DEX) {
        b = --cpu->x;
    } else {
        b = --cpu->y;
    }
    INES_NZS(cpu, b)
}

static void inline ines_bit_impl(NesConsole *console, AddressMode mode) {
    CPU *cpu = console->cpu;
    uint8 b = ines_ops_operand(console, mode);
    uint8 r = b & cpu->a;
    ines_cpu_state(cpu, ZERO, r == 0);
    ines_cpu_state(cpu, OVERFLOW, (b >> 6) & 0x01);
    ines_cpu_state(cpu, NEGATIVE, (b >> 7) & 0x01);
}

static void inline ines_branch_impl(NesConsole *console, CPU *cpu) {
    cpu->cycle = cpu->cycle + 1;
    uint16 base = cpu->pc + 1;
    byte offset = (byte) ines_ops_operand(console, Relative);
    uint16 addr = base + offset;
    INES_CPU_PAGE_CROSS(console, base, addr, 2)
    cpu->pc = addr;
}

static void inline ines_bmi_bpl_impl(NesConsole *console, Instruction is) {
    CPU *cpu = console->cpu;
    bool s = INES_CPU_STATE(cpu, NEGATIVE)
    if ((s && is == BMI) || (!s && is == BPL)) {
        ines_branch_impl(console, cpu);
    }
}

static void inline ines_bvs_bvc_impl(NesConsole *console, Instruction is) {
    CPU *cpu = console->cpu;
    bool s = INES_CPU_STATE(cpu, OVERFLOW)
    if ((s && is == BVS) || (!s && is == BVC)) {
        ines_branch_impl(console, cpu);
    }
}

static void inline ines_cmp_impl(NesConsole *console, AddressMode mode) {
    CPU *cpu = console->cpu;
    uint8 a = cpu->a;
    uint8 b = ines_ops_operand0(console, mode, True);
    ines_cpu_state(cpu, CARRY, a >= b);
    ines_cpu_state(cpu, ZERO, a == b);
    INES_CPU_STATE_SET_NEG(cpu, a - b)
}


static void inline ines_pha_pla_impl(NesConsole *console, Instruction is) {
    CPU *cpu = console->cpu;
    if (is == PHA) {
        uint8 a = cpu->a;
        ines_bus_stack_push(console, a);
    } else {
        uint8 a = ines_bus_stack_pop(console);
        cpu->a = a;
    }
}

static void inline ines_php_plp_impl(NesConsole *console, Instruction is) {
    CPU *cpu = console->cpu;
    if (is == PHP) {
        uint8 P = cpu->a;
        ines_bus_stack_push(console, P);
    } else {
        uint8 p = ines_bus_stack_pop(console);
        cpu->p = p;
    }
}

static void inline ines_rti_impl(NesConsole *console) {
    CPU *cpu = console->cpu;
    cpu->p = ines_bus_stack_pop(console);
    cpu->pc = ines_bus_stack_pop0(console);
}

static void inline ines_adc_sbc_impl(NesConsole *console, Instruction is, AddressMode model) {
    CPU *cpu = console->cpu;
    uint8 b = ines_ops_operand0(console, model, True);
    if (is == SBC) {
        b = (-b - 1);
    }
    uint16 tmp = cpu->a + b + INES_CPU_STATE(cpu, CARRY)
    ines_cpu_state(cpu, CARRY, tmp > 0xFF);
    uint8 sum = (uint8) tmp;
    ines_cpu_state(cpu, OVERFLOW, (((b & 0xff ^ sum) & (sum ^ cpu->a)) & 0x80) != 0);
    cpu->a = sum;
    INES_NZS(cpu, sum)
}

static void inline ines_tsx_txs_impl(NesConsole *console, Instruction is) {
    uint8 b;
    CPU *cpu = console->cpu;
    if (is == TSX) {
        b = cpu->p;
        cpu->x = b;
    } else {
        b = cpu->x;
        cpu->p = b;
    }
    INES_NZS(cpu, b)
}

static void inline ines_bcc_bcs_impl(NesConsole *console, Instruction is) {
    CPU *cpu = console->cpu;
    bool s = INES_CPU_STATE(cpu, CARRY)
    if ((s && is == BCS) || (!s && is == BCC)) {
        ines_branch_impl(console, cpu);
    }
}

/**
 * Private debug instruction implement most program language `print` function
 */
static void inline ines_log_impl(NesConsole *console) {
    CPU *cpu = console->cpu;
    uint16 pc = cpu->pc;
    uint8 arr[UTF8_CHAR_CACHE];
    int length = ines_bus_read_utf8char(console, arr, UTF8_CHAR_CACHE);
    if (length > 0) {
        printf("($%04x)Emulator log:%s\n", pc - 1, arr);
        cpu->pc = pc + length;
    }
}


extern uint32 ines_console_execute(NesConsole *console) {
    CPU *cpu = console->cpu;
    uint32 suspend = cpu->suspend;
    if (suspend != 0) {
        INES_CPU_CLOCK_SYNC(console, cpu->suspend)
        cpu->suspend = 0;
        return suspend;
    }
    uint32 cycle = 0;
    uint16 pc = cpu->pc;
    cpu->synchronized = 0;
    if (IS_IRQ_TRIGGER(cpu, RESET)) {
        CLEAR_IRQ(cpu, RESET)
        pc = ines_bus_read_uint16(console, RESET_VECTOR);
    } else if (IS_IRQ_TRIGGER(cpu, NMI)) {
        CLEAR_IRQ(cpu, NMI)
        cycle = 7;
        pc = ines_bus_read_uint16(console, NMI_VECTOR);
    } else if (IS_IRQ_TRIGGER(cpu, IRQ)) {
        cycle = 7;
        CLEAR_IRQ(cpu, IRQ)
        pc = ines_bus_read_uint16(console, IRQ_BRK_VECTOR);
    } else if (IS_IRQ_TRIGGER(cpu, BRK)) {
        cycle = 2;
        CLEAR_IRQ(cpu, BRK)
        pc = ines_bus_read_uint16(console, IRQ_BRK_VECTOR);
    }
    if (cycle > 0) {
        INES_CPU_CLOCK_SYNC(console, cycle)
    }
    cpu->cycle = cycle;
    uint8 opcode = ines_bus_read(console, pc);
    uint8 *opWrap = ops[opcode];
    Instruction is = opWrap[0];
    AddressMode mode = opWrap[1];

    uint16 state = pc + 1;

    cpu->pc = state;
    cpu->cycle = opWrap[3];

    if (ines_debug_enable() && is != UWN) {
        ines_debug("($%04x) %s m=%s,a=$%02x,x=$%02x,y=$%02x,p=$%02x",
                   state - 1, IStr[is], AMStr[mode], cpu->a, cpu->x, cpu->y, cpu->p);
    }
    switch (is) {
        case ADC:
            ines_adc_sbc_impl(console, ADC, mode);
            break;
        case SBC:
            ines_adc_sbc_impl(console, SBC, mode);
            break;
        case INX:
            ines_xy_inc_impl(cpu, True);
            break;
        case INY:
            ines_xy_inc_impl(cpu, False);
            break;
        case SEC:
            ines_cpu_state(cpu, DECIMAL, True);
            break;
        case CLC:
            ines_cpu_state(cpu, CARRY, False);
            break;
        case SED:
            ines_cpu_state(cpu, DECIMAL, True);
            break;
        case CLD:
            ines_cpu_state(cpu, DECIMAL, False);
            break;
        case SEI:
            ines_cpu_state(cpu, INTERRUPT_DISABLE, True);
            break;
        case CLI:
            ines_cpu_state(cpu, INTERRUPT_DISABLE, False);
            break;
        case BEQ:
            ines_beq_bne_impl(console, False, mode);
            break;
        case BNE:
            ines_beq_bne_impl(console, True, mode);
            break;
        case JMP:
            ines_jmp_impl(console, mode);
            break;
        case JSR:
            ines_jsr_impl(console);
            break;
        case RTS:
            ines_rts_impl(console);
            break;
        case AND:
            ines_logic_impl(console, AND, mode);
            break;
        case ORA:
            ines_logic_impl(console, ORA, mode);
            break;
        case EOR:
            ines_logic_impl(console, EOR, mode);
            break;
        case LDA:
            ines_ld_axy(console, LDA, mode);
            break;
        case LDX:
            ines_ld_axy(console, LDX, mode);
            break;
        case LDY:
            ines_ld_axy(console, LDY, mode);
            break;
        case STA:
            ines_ops_write(console, mode, cpu->a);
            break;
        case STX:
            ines_ops_write(console, mode, cpu->x);
            break;
        case STY:
            ines_ops_write(console, mode, cpu->y);
            break;
        case TAX:
            ines_tax_impl(cpu);
            break;
        case TXA:
            ines_txa_impl(cpu);
            break;
        case TAY:
            ines_tay_impl(cpu);
            break;
        case TYA:
            ines_tya_impl(cpu);
        case INC:
            ines_inc_impl(console, mode);
            break;
        case NOP:
        case BRK0:
            ines_console_irq(cpu, BRK);
            break;
        case LSR:
            ines_asl_lsr_impl(console, LSR, mode);
            break;
        case ASL:
            ines_asl_lsr_impl(console, ASL, mode);
            break;
        case ROL:
            ines_ror_rol_impl(console, ROL, mode);
            break;
        case ROR:
            ines_ror_rol_impl(console, ROR, mode);
            break;
        case CPY:
            ines_xycmp_impl(console, CPY, mode);
            break;
        case CPX:
            ines_xycmp_impl(console, CPX, mode);
            break;
        case DEC:
            ines_dec_impl(console, mode);
            break;
        case DEX:
            ines_xydec_impl(console, DEX);
            break;
        case DEY:
            ines_xydec_impl(console, DEY);
            break;
        case BIT:
            ines_bit_impl(console, mode);
            break;
        case BMI:
            ines_bmi_bpl_impl(console, BMI);
            break;
        case BPL:
            ines_bmi_bpl_impl(console, BPL);
            break;
        case BVC:
            ines_bvs_bvc_impl(console, BVC);
            break;
        case BVS:
            ines_bvs_bvc_impl(console, BVS);
            break;
        case CLV:
            ines_cpu_state(cpu, OVERFLOW, False);
            break;
        case CMP:
            ines_cmp_impl(console, mode);
            break;
        case PHA:
            ines_pha_pla_impl(console, PHA);
            break;
        case PLA:
            ines_pha_pla_impl(console, PLA);
            break;
        case PHP:
            ines_php_plp_impl(console, PHP);
            break;
        case PLP:
            ines_php_plp_impl(console, PLP);
            break;
        case RTI:
            ines_rti_impl(console);
            break;
        case TSX:
            ines_tsx_txs_impl(console, TSX);
            break;
        case TXS:
            ines_tsx_txs_impl(console, TXS);
            break;
        case BCS:
            ines_bcc_bcs_impl(console, BCS);
            break;
        case BCC:
            ines_bcc_bcs_impl(console, BCC);
            break;
        case LOG:
            ines_log_impl(console);
            break;
        default:
            ines_warn("Unknown open code $%02x", opcode);
            break;
    }
    uint16 tmp = cpu->pc;
    if (tmp == state) {
        cpu->pc = state + (opWrap[2] - 1);
    }
    uint8 left = cpu->cycle - cpu->synchronized;
    if (left != 0) {
        INES_CPU_CLOCK_SYNC(console, left)
    }
    cycle = cpu->cycle;
    console->masterCycle += cycle;
    return cycle;
}

extern void ines_cpu_dispose(CPU *cpu) {
    if (cpu == NULL) {
        return;
    }
    free(cpu);
}

extern void ines_console_irq(CPU *cpu, ExternalIRQ external) {
    bool irqDisable = INES_CPU_STATE(cpu, INTERRUPT_DISABLE);
    if (irqDisable && (external == IRQ || external == BRK)) {
        return;
    }
    TRIGGER_IRQ(cpu, external)
}