#include "cpu.h"
#include <stdio.h>

#define FZ 0x80
#define FN 0x40
#define FH 0x20
#define FC 0x10

// ---- memory access helpers (each ticks 4 T-cycles) ----
static inline u8 rb(GB *gb, u16 a) { u8 v = gb_read(gb, a); gb_tick(gb, 4); return v; }
static inline void wb(GB *gb, u16 a, u8 v) { gb_write(gb, a, v); gb_tick(gb, 4); }
static inline void internal(GB *gb) { gb_tick(gb, 4); }

static inline u8 fetch8(GB *gb) {
    u8 v = gb_read(gb, gb->cpu.pc);
    if (gb->cpu.halt_bug) { gb->cpu.halt_bug = false; }
    else gb->cpu.pc++;
    gb_tick(gb, 4);
    return v;
}
static inline u16 fetch16(GB *gb) {
    u8 lo = fetch8(gb);
    u8 hi = fetch8(gb);
    return lo | (hi << 8);
}

// ---- flag helpers ----
static inline void setf(CPU *c, u8 mask, bool cond) {
    if (cond) c->f |= mask; else c->f &= ~mask;
    c->f &= 0xF0;
}

// ---- 8-bit ALU ----
static u8 alu_inc(CPU *c, u8 v) {
    u8 r = v + 1;
    setf(c, FZ, r == 0);
    setf(c, FN, false);
    setf(c, FH, (v & 0x0F) == 0x0F);
    return r;
}
static u8 alu_dec(CPU *c, u8 v) {
    u8 r = v - 1;
    setf(c, FZ, r == 0);
    setf(c, FN, true);
    setf(c, FH, (v & 0x0F) == 0x00);
    return r;
}
static void alu_add(CPU *c, u8 v) {
    u16 r = c->a + v;
    setf(c, FH, ((c->a & 0xF) + (v & 0xF)) > 0xF);
    setf(c, FC, r > 0xFF);
    c->a = r;
    setf(c, FZ, c->a == 0);
    setf(c, FN, false);
}
static void alu_adc(CPU *c, u8 v) {
    u8 carry = (c->f & FC) ? 1 : 0;
    u16 r = c->a + v + carry;
    setf(c, FH, ((c->a & 0xF) + (v & 0xF) + carry) > 0xF);
    setf(c, FC, r > 0xFF);
    c->a = r;
    setf(c, FZ, c->a == 0);
    setf(c, FN, false);
}
static void alu_sub(CPU *c, u8 v) {
    setf(c, FH, (c->a & 0xF) < (v & 0xF));
    setf(c, FC, c->a < v);
    c->a -= v;
    setf(c, FZ, c->a == 0);
    setf(c, FN, true);
}
static void alu_sbc(CPU *c, u8 v) {
    u8 carry = (c->f & FC) ? 1 : 0;
    int r = c->a - v - carry;
    setf(c, FH, ((c->a & 0xF) - (v & 0xF) - carry) < 0);
    setf(c, FC, r < 0);
    c->a = r;
    setf(c, FZ, c->a == 0);
    setf(c, FN, true);
}
static void alu_and(CPU *c, u8 v) {
    c->a &= v;
    setf(c, FZ, c->a == 0);
    setf(c, FN, false); setf(c, FH, true); setf(c, FC, false);
}
static void alu_or(CPU *c, u8 v) {
    c->a |= v;
    setf(c, FZ, c->a == 0);
    setf(c, FN, false); setf(c, FH, false); setf(c, FC, false);
}
static void alu_xor(CPU *c, u8 v) {
    c->a ^= v;
    setf(c, FZ, c->a == 0);
    setf(c, FN, false); setf(c, FH, false); setf(c, FC, false);
}
static void alu_cp(CPU *c, u8 v) {
    setf(c, FH, (c->a & 0xF) < (v & 0xF));
    setf(c, FC, c->a < v);
    setf(c, FZ, c->a == v);
    setf(c, FN, true);
}

static void add_hl(CPU *c, u16 v) {
    u32 r = c->hl + v;
    setf(c, FN, false);
    setf(c, FH, ((c->hl & 0x0FFF) + (v & 0x0FFF)) > 0x0FFF);
    setf(c, FC, r > 0xFFFF);
    c->hl = r;
}

static u16 add_sp_e(GB *gb, s8 e) {
    CPU *c = &gb->cpu;
    u16 sp = c->sp;
    setf(c, FZ, false); setf(c, FN, false);
    setf(c, FH, ((sp & 0xF) + (e & 0xF)) > 0xF);
    setf(c, FC, ((sp & 0xFF) + (e & 0xFF)) > 0xFF);
    return sp + e;
}

static void daa(CPU *c) {
    int a = c->a;
    if (!(c->f & FN)) {
        if ((c->f & FH) || (a & 0x0F) > 9) a += 0x06;
        if ((c->f & FC) || a > 0x9F) { a += 0x60; setf(c, FC, true); }
    } else {
        if (c->f & FH) a = (a - 6) & 0xFF;
        if (c->f & FC) a -= 0x60;
    }
    c->a = a;
    setf(c, FZ, c->a == 0);
    setf(c, FH, false);
}

// ---- CB rotate/shift ----
static u8 cb_rlc(CPU *c, u8 v){ u8 r=(v<<1)|(v>>7); setf(c,FC,v&0x80); setf(c,FZ,r==0); setf(c,FN,false); setf(c,FH,false); return r; }
static u8 cb_rrc(CPU *c, u8 v){ u8 r=(v>>1)|(v<<7); setf(c,FC,v&0x01); setf(c,FZ,r==0); setf(c,FN,false); setf(c,FH,false); return r; }
static u8 cb_rl (CPU *c, u8 v){ u8 cy=(c->f&FC)?1:0; u8 r=(v<<1)|cy; setf(c,FC,v&0x80); setf(c,FZ,r==0); setf(c,FN,false); setf(c,FH,false); return r; }
static u8 cb_rr (CPU *c, u8 v){ u8 cy=(c->f&FC)?0x80:0; u8 r=(v>>1)|cy; setf(c,FC,v&0x01); setf(c,FZ,r==0); setf(c,FN,false); setf(c,FH,false); return r; }
static u8 cb_sla(CPU *c, u8 v){ u8 r=v<<1; setf(c,FC,v&0x80); setf(c,FZ,r==0); setf(c,FN,false); setf(c,FH,false); return r; }
static u8 cb_sra(CPU *c, u8 v){ u8 r=(v>>1)|(v&0x80); setf(c,FC,v&0x01); setf(c,FZ,r==0); setf(c,FN,false); setf(c,FH,false); return r; }
static u8 cb_swap(CPU *c,u8 v){ u8 r=(v>>4)|(v<<4); setf(c,FC,false); setf(c,FZ,r==0); setf(c,FN,false); setf(c,FH,false); return r; }
static u8 cb_srl(CPU *c, u8 v){ u8 r=v>>1; setf(c,FC,v&0x01); setf(c,FZ,r==0); setf(c,FN,false); setf(c,FH,false); return r; }

// r index: 0=B 1=C 2=D 3=E 4=H 5=L 6=(HL) 7=A
static u8 reg_get(GB *gb, int i) {
    CPU *c = &gb->cpu;
    switch (i) {
        case 0: return c->b; case 1: return c->c; case 2: return c->d;
        case 3: return c->e; case 4: return c->h; case 5: return c->l;
        case 6: return rb(gb, c->hl); case 7: return c->a;
    }
    return 0;
}
static void reg_set(GB *gb, int i, u8 v) {
    CPU *c = &gb->cpu;
    switch (i) {
        case 0: c->b=v; break; case 1: c->c=v; break; case 2: c->d=v; break;
        case 3: c->e=v; break; case 4: c->h=v; break; case 5: c->l=v; break;
        case 6: wb(gb, c->hl, v); break; case 7: c->a=v; break;
    }
}

static void push16(GB *gb, u16 v) {
    CPU *c = &gb->cpu;
    wb(gb, --c->sp, v >> 8);
    wb(gb, --c->sp, v & 0xFF);
}
static u16 pop16(GB *gb) {
    CPU *c = &gb->cpu;
    u8 lo = rb(gb, c->sp++);
    u8 hi = rb(gb, c->sp++);
    return lo | (hi << 8);
}

// ---- CB prefix ----
static void do_cb(GB *gb) {
    CPU *c = &gb->cpu;
    u8 op = fetch8(gb);
    int r = op & 7;
    int y = (op >> 3) & 7;
    if (op < 0x40) {
        u8 v = reg_get(gb, r);
        u8 res;
        switch (y) {
            case 0: res = cb_rlc(c, v); break;
            case 1: res = cb_rrc(c, v); break;
            case 2: res = cb_rl(c, v);  break;
            case 3: res = cb_rr(c, v);  break;
            case 4: res = cb_sla(c, v); break;
            case 5: res = cb_sra(c, v); break;
            case 6: res = cb_swap(c, v);break;
            default:res = cb_srl(c, v); break;
        }
        reg_set(gb, r, res);
    } else if (op < 0x80) { // BIT
        u8 v = reg_get(gb, r);
        setf(c, FZ, !(v & (1 << y)));
        setf(c, FN, false);
        setf(c, FH, true);
    } else if (op < 0xC0) { // RES
        u8 v = reg_get(gb, r);
        reg_set(gb, r, v & ~(1 << y));
    } else { // SET
        u8 v = reg_get(gb, r);
        reg_set(gb, r, v | (1 << y));
    }
}

// ---- interrupt handling ----
static bool handle_interrupts(GB *gb) {
    CPU *c = &gb->cpu;
    u8 pending = gb->iff & gb->ie & 0x1F;
    if (!pending) return false;

    if (c->halted) c->halted = false;
    if (!c->ime) return false;

    c->ime = false;
    internal(gb); internal(gb);           // 2 internal cycles
    push16(gb, c->pc);                     // 2 memory cycles
    // determine vector after push (IE may change but pending latched)
    pending = gb->iff & gb->ie & 0x1F;
    int bit = 0;
    for (; bit < 5; bit++) if (pending & (1 << bit)) break;
    if (bit == 5) { c->pc = 0; internal(gb); return true; } // cancelled
    gb->iff &= ~(1 << bit);
    c->pc = 0x40 + bit * 8;
    internal(gb);
    return true;
}

int cpu_step(GB *gb) {
    CPU *c = &gb->cpu;
    u64 start = gb->cycles;

    // EI takes effect after the following instruction
    bool ei_was_pending = c->ime_pending;

    if (handle_interrupts(gb)) {
        if (ei_was_pending) { /* handled below */ }
        return (int)(gb->cycles - start);
    }

    if (c->halted) {
        gb_tick(gb, 4);
        return (int)(gb->cycles - start);
    }

    u8 op = fetch8(gb);

    switch (op) {
    case 0x00: break;                                   // NOP
    case 0x10: fetch8(gb);                              // STOP
        if (gb->cgb_mode && (gb->key1 & 0x01)) {
            gb->double_speed = !gb->double_speed;
            gb->key1 = (gb->double_speed ? 0x80 : 0x00);
        }
        break;
    case 0x76:                                          // HALT
        if (!c->ime && (gb->iff & gb->ie & 0x1F)) c->halt_bug = true;
        else c->halted = true;
        break;

    // 16-bit loads
    case 0x01: c->bc = fetch16(gb); break;
    case 0x11: c->de = fetch16(gb); break;
    case 0x21: c->hl = fetch16(gb); break;
    case 0x31: c->sp = fetch16(gb); break;
    case 0x08: { u16 a = fetch16(gb); wb(gb, a, c->sp & 0xFF); wb(gb, a+1, c->sp >> 8); } break;
    case 0xF9: c->sp = c->hl; internal(gb); break;
    case 0xF8: { s8 e = (s8)fetch8(gb); c->hl = add_sp_e(gb, e); internal(gb); } break;

    // 8-bit loads (immediate)
    case 0x06: c->b = fetch8(gb); break;
    case 0x0E: c->c = fetch8(gb); break;
    case 0x16: c->d = fetch8(gb); break;
    case 0x1E: c->e = fetch8(gb); break;
    case 0x26: c->h = fetch8(gb); break;
    case 0x2E: c->l = fetch8(gb); break;
    case 0x36: wb(gb, c->hl, fetch8(gb)); break;
    case 0x3E: c->a = fetch8(gb); break;

    // LD (rr),A and A,(rr)
    case 0x02: wb(gb, c->bc, c->a); break;
    case 0x12: wb(gb, c->de, c->a); break;
    case 0x22: wb(gb, c->hl++, c->a); break;
    case 0x32: wb(gb, c->hl--, c->a); break;
    case 0x0A: c->a = rb(gb, c->bc); break;
    case 0x1A: c->a = rb(gb, c->de); break;
    case 0x2A: c->a = rb(gb, c->hl++); break;
    case 0x3A: c->a = rb(gb, c->hl--); break;

    // 16-bit inc/dec
    case 0x03: c->bc++; internal(gb); break;
    case 0x13: c->de++; internal(gb); break;
    case 0x23: c->hl++; internal(gb); break;
    case 0x33: c->sp++; internal(gb); break;
    case 0x0B: c->bc--; internal(gb); break;
    case 0x1B: c->de--; internal(gb); break;
    case 0x2B: c->hl--; internal(gb); break;
    case 0x3B: c->sp--; internal(gb); break;

    // 8-bit inc
    case 0x04: c->b = alu_inc(c, c->b); break;
    case 0x0C: c->c = alu_inc(c, c->c); break;
    case 0x14: c->d = alu_inc(c, c->d); break;
    case 0x1C: c->e = alu_inc(c, c->e); break;
    case 0x24: c->h = alu_inc(c, c->h); break;
    case 0x2C: c->l = alu_inc(c, c->l); break;
    case 0x34: { u8 v = rb(gb, c->hl); wb(gb, c->hl, alu_inc(c, v)); } break;
    case 0x3C: c->a = alu_inc(c, c->a); break;
    // 8-bit dec
    case 0x05: c->b = alu_dec(c, c->b); break;
    case 0x0D: c->c = alu_dec(c, c->c); break;
    case 0x15: c->d = alu_dec(c, c->d); break;
    case 0x1D: c->e = alu_dec(c, c->e); break;
    case 0x25: c->h = alu_dec(c, c->h); break;
    case 0x2D: c->l = alu_dec(c, c->l); break;
    case 0x35: { u8 v = rb(gb, c->hl); wb(gb, c->hl, alu_dec(c, v)); } break;
    case 0x3D: c->a = alu_dec(c, c->a); break;

    // ADD HL,rr
    case 0x09: add_hl(c, c->bc); internal(gb); break;
    case 0x19: add_hl(c, c->de); internal(gb); break;
    case 0x29: add_hl(c, c->hl); internal(gb); break;
    case 0x39: add_hl(c, c->sp); internal(gb); break;

    // rotates on A
    case 0x07: { u8 v=c->a; c->a=(v<<1)|(v>>7); setf(c,FC,v&0x80); setf(c,FZ,false);setf(c,FN,false);setf(c,FH,false);} break;
    case 0x0F: { u8 v=c->a; c->a=(v>>1)|(v<<7); setf(c,FC,v&0x01); setf(c,FZ,false);setf(c,FN,false);setf(c,FH,false);} break;
    case 0x17: { u8 cy=(c->f&FC)?1:0; u8 v=c->a; c->a=(v<<1)|cy; setf(c,FC,v&0x80); setf(c,FZ,false);setf(c,FN,false);setf(c,FH,false);} break;
    case 0x1F: { u8 cy=(c->f&FC)?0x80:0; u8 v=c->a; c->a=(v>>1)|cy; setf(c,FC,v&0x01); setf(c,FZ,false);setf(c,FN,false);setf(c,FH,false);} break;

    case 0x27: daa(c); break;
    case 0x2F: c->a = ~c->a; setf(c,FN,true); setf(c,FH,true); break;
    case 0x37: setf(c,FC,true); setf(c,FN,false); setf(c,FH,false); break;
    case 0x3F: setf(c,FC,!(c->f&FC)); setf(c,FN,false); setf(c,FH,false); break;

    // control flow
    case 0xC3: { u16 a=fetch16(gb); c->pc=a; internal(gb);} break;
    case 0xE9: c->pc = c->hl; break;
    case 0x18: { s8 e=(s8)fetch8(gb); c->pc+=e; internal(gb);} break;
    case 0x20: { s8 e=(s8)fetch8(gb); if(!(c->f&FZ)){c->pc+=e; internal(gb);} } break;
    case 0x28: { s8 e=(s8)fetch8(gb); if( (c->f&FZ)){c->pc+=e; internal(gb);} } break;
    case 0x30: { s8 e=(s8)fetch8(gb); if(!(c->f&FC)){c->pc+=e; internal(gb);} } break;
    case 0x38: { s8 e=(s8)fetch8(gb); if( (c->f&FC)){c->pc+=e; internal(gb);} } break;
    case 0xC2: { u16 a=fetch16(gb); if(!(c->f&FZ)){c->pc=a; internal(gb);} } break;
    case 0xCA: { u16 a=fetch16(gb); if( (c->f&FZ)){c->pc=a; internal(gb);} } break;
    case 0xD2: { u16 a=fetch16(gb); if(!(c->f&FC)){c->pc=a; internal(gb);} } break;
    case 0xDA: { u16 a=fetch16(gb); if( (c->f&FC)){c->pc=a; internal(gb);} } break;

    case 0xCD: { u16 a=fetch16(gb); internal(gb); push16(gb,c->pc); c->pc=a; } break;
    case 0xC4: { u16 a=fetch16(gb); if(!(c->f&FZ)){internal(gb);push16(gb,c->pc);c->pc=a;} } break;
    case 0xCC: { u16 a=fetch16(gb); if( (c->f&FZ)){internal(gb);push16(gb,c->pc);c->pc=a;} } break;
    case 0xD4: { u16 a=fetch16(gb); if(!(c->f&FC)){internal(gb);push16(gb,c->pc);c->pc=a;} } break;
    case 0xDC: { u16 a=fetch16(gb); if( (c->f&FC)){internal(gb);push16(gb,c->pc);c->pc=a;} } break;

    case 0xC9: c->pc = pop16(gb); internal(gb); break;
    case 0xD9: c->pc = pop16(gb); internal(gb); c->ime = true; break; // RETI
    case 0xC0: internal(gb); if(!(c->f&FZ)){c->pc=pop16(gb);internal(gb);} break;
    case 0xC8: internal(gb); if( (c->f&FZ)){c->pc=pop16(gb);internal(gb);} break;
    case 0xD0: internal(gb); if(!(c->f&FC)){c->pc=pop16(gb);internal(gb);} break;
    case 0xD8: internal(gb); if( (c->f&FC)){c->pc=pop16(gb);internal(gb);} break;

    case 0xC7: internal(gb); push16(gb,c->pc); c->pc=0x00; break;
    case 0xCF: internal(gb); push16(gb,c->pc); c->pc=0x08; break;
    case 0xD7: internal(gb); push16(gb,c->pc); c->pc=0x10; break;
    case 0xDF: internal(gb); push16(gb,c->pc); c->pc=0x18; break;
    case 0xE7: internal(gb); push16(gb,c->pc); c->pc=0x20; break;
    case 0xEF: internal(gb); push16(gb,c->pc); c->pc=0x28; break;
    case 0xF7: internal(gb); push16(gb,c->pc); c->pc=0x30; break;
    case 0xFF: internal(gb); push16(gb,c->pc); c->pc=0x38; break;

    // push/pop
    case 0xC1: c->bc = pop16(gb); break;
    case 0xD1: c->de = pop16(gb); break;
    case 0xE1: c->hl = pop16(gb); break;
    case 0xF1: c->af = pop16(gb) & 0xFFF0; break;
    case 0xC5: internal(gb); push16(gb, c->bc); break;
    case 0xD5: internal(gb); push16(gb, c->de); break;
    case 0xE5: internal(gb); push16(gb, c->hl); break;
    case 0xF5: internal(gb); push16(gb, c->af); break;

    // high memory / misc loads
    case 0xE0: { u8 n=fetch8(gb); wb(gb, 0xFF00+n, c->a); } break;
    case 0xF0: { u8 n=fetch8(gb); c->a = rb(gb, 0xFF00+n); } break;
    case 0xE2: wb(gb, 0xFF00+c->c, c->a); break;
    case 0xF2: c->a = rb(gb, 0xFF00+c->c); break;
    case 0xEA: { u16 a=fetch16(gb); wb(gb, a, c->a); } break;
    case 0xFA: { u16 a=fetch16(gb); c->a = rb(gb, a); } break;

    // ADD SP,e
    case 0xE8: { s8 e=(s8)fetch8(gb); c->sp = add_sp_e(gb, e); internal(gb); internal(gb); } break;

    // interrupts
    case 0xF3: c->ime = false; c->ime_pending = false; break;
    case 0xFB: c->ime_pending = true; break;

    // immediate ALU
    case 0xC6: alu_add(c, fetch8(gb)); break;
    case 0xCE: alu_adc(c, fetch8(gb)); break;
    case 0xD6: alu_sub(c, fetch8(gb)); break;
    case 0xDE: alu_sbc(c, fetch8(gb)); break;
    case 0xE6: alu_and(c, fetch8(gb)); break;
    case 0xEE: alu_xor(c, fetch8(gb)); break;
    case 0xF6: alu_or(c, fetch8(gb)); break;
    case 0xFE: alu_cp(c, fetch8(gb)); break;

    case 0xCB: do_cb(gb); break;

    case 0xED: gb->poweroff = true;         // (illegal on HW) clean-exit signal
        fprintf(stderr, "[sl0pboy] poweroff opcode $ED at pc=%04X (af=%04X bc=%04X "
                "de=%04X hl=%04X sp=%04X)\n", (unsigned)(c->pc - 1), c->af, c->bc,
                c->de, c->hl, c->sp);
        break;

    default:
        // 0x40-0xBF block: LD r,r' and ALU A,r
        if (op >= 0x40 && op < 0x80) {
            int dst = (op >> 3) & 7;
            int src = op & 7;
            u8 v = reg_get(gb, src);
            reg_set(gb, dst, v);
        } else if (op >= 0x80 && op < 0xC0) {
            int kind = (op >> 3) & 7;
            u8 v = reg_get(gb, op & 7);
            switch (kind) {
                case 0: alu_add(c, v); break;
                case 1: alu_adc(c, v); break;
                case 2: alu_sub(c, v); break;
                case 3: alu_sbc(c, v); break;
                case 4: alu_and(c, v); break;
                case 5: alu_xor(c, v); break;
                case 6: alu_or(c, v);  break;
                case 7: alu_cp(c, v);  break;
            }
        }
        // else: illegal opcode (D3,DB,DD,E3,E4,EB,EC,ED,F4,FC,FD) -> NOP-ish
        break;
    }

    // apply delayed EI (after this instruction completed, if it was EI)
    if (c->ime_pending && op == 0xFB) {
        // becomes active before next instruction; leave pending flag,
        // but we consumed it: enable now so next handle_interrupts sees ime.
    }
    if (ei_was_pending && op != 0xFB) {
        c->ime = true;
        c->ime_pending = false;
    }

    return (int)(gb->cycles - start);
}