#include "6502.h"

#define STACK_PUSH(v) \
	do { \
		bus_write(0x100 + cpu->sp, v, cycles); \
		cpu->sp--; \
		cycles++; \
	} while (0)

#define STACK_PULL(r, mask) \
	do { \
		bus_read(0x100 + cpu->sp, cycles); \
		cpu->sp++; \
		cycles++; \
		cpu->r = (cpu->r & ~mask) | (bus_read(0x100 + cpu->sp, cycles) & mask); \
		cycles++; \
	} while (0)

#define PUSH_STATE() \
	do { \
		STACK_PUSH((cpu->pc & 0xff00) >> 8); \
		STACK_PUSH(cpu->pc & 0xff); \
		STACK_PUSH(cpu->flags); \
	} while (0)

#define FETCH_PC(addr) \
	do { \
		cpu->pc = 0; \
		cpu->pc |= bus_read(addr, cycles); \
		cycles++; \
		cpu->pc |= bus_read(addr + 1, cycles) << 8; \
		cycles++; \
	} while (0)

#define OP(reg, src, op) \
	do { \
		uint8_t r = cpu->reg; \
		uint16_t ea = 0; \
		uint8_t a; \
		src(); \
		op(); \
		FLAG_SET(N, (r & 0x80) >> 7); \
		FLAG_SET(Z, (r == 0) ? 1 : 0); \
		cpu->reg = r; \
	} while (0); break

#define CMP(reg, src) \
	do { \
		uint8_t r = cpu->reg; \
		uint16_t ea = 0; \
		uint8_t a; \
		src(); \
		uint16_t res = r - a; \
		FLAG_SET(C, (~res & 0x100) >> 8); \
		FLAG_SET(N, (res & 0x80) >> 7); \
		FLAG_SET(Z, ((res & 0xff) == 0) ? 1 : 0); \
	} while (0); break

#define BIT(src) \
	do { \
		uint16_t ea = 0; \
		uint8_t a; \
		src(); \
		FLAG_SET(V, (a & 0x40) >> 6); \
		FLAG_SET(N, (a & 0x80) >> 7); \
		FLAG_SET(Z, ((a & cpu->a) == 0) ? 1 : 0); \
	} while (0); break

#define RMW(src, op, dst) \
	do { \
		uint8_t a; \
		uint16_t ea = 0; \
		src(); \
		dst(); \
		op(); \
		FLAG_SET(N, (a & 0x80) >> 7); \
		FLAG_SET(Z, (a == 0) ? 1 : 0); \
		dst(); \
	} while (0); break

#define STORE(src, dst) \
	do { \
		uint8_t a; \
		src(); \
		dst(); \
	} while (0); break

#define B(f, v) \
	do { \
		uint8_t a; \
		R_IMM(); \
		if (FLAG_GET(f) == v) { \
			uint16_t ea = (cpu->pc & 0xff) + (int8_t)a; \
			uint16_t carry = ea & 0xff00; \
			ea &= 0xff; \
			ea |= cpu->pc & 0xff00; \
			bus_read(ea, cycles); \
			cycles++; \
			if (carry > 0) { \
				bus_read(ea, cycles); \
				ea += carry; \
				cycles++; \
			} \
			cpu->pc = ea; \
		} \
	} while (0); break

#define R_IMP()

#define R_REG(r) \
	do { \
		a = cpu->r; \
	} while (0)
#define R_A() R_REG(a)
#define R_X() R_REG(x)
#define R_Y() R_REG(y)

#define R_IMM() \
	do { \
		cpu->pc++; \
		a = operand; \
	} while (0)

#define R_ZP() \
	do { \
		cpu->pc++; \
		a = bus_read(operand, cycles); \
		ea = operand; \
		cycles++; \
	} while (0)

#define R_ZPX() \
	do { \
		cpu->pc++; \
		bus_read(operand, cycles); \
		cycles++; \
		a = bus_read((operand + cpu->x) & 0xff, cycles); \
		ea = (operand + cpu->x) & 0xff; \
		cycles++; \
	} while (0)

#define R_ZPY() \
	do { \
		cpu->pc++; \
		bus_read(operand, cycles); \
		cycles++; \
		a = bus_read((operand + cpu->y) & 0xff, cycles); \
		ea = (operand + cpu->y) & 0xff; \
		cycles++; \
	} while (0)

#define R_ABS() \
	do { \
		cpu->pc++; \
		ea = operand; \
		ea |= bus_read(cpu->pc, cycles) << 8; \
		cpu->pc++; \
		cycles++; \
		a = bus_read(ea, cycles); \
		cycles++; \
	} while (0)

#define R_ABSI(i) \
	do { \
		cpu->pc++; \
		ea = operand; \
		ea += cpu->i; \
		uint16_t carry = ea & 0x0100; \
		ea &= 0xff; \
		ea |= bus_read(cpu->pc, cycles) << 8; \
		cpu->pc++; \
		cycles++; \
		a = bus_read(ea, cycles); \
		cycles++; \
		if (carry) { \
			ea += 0x100; \
			a = bus_read(ea, cycles); \
			cycles++; \
		} \
	} while (0)

#define R_ABSX() R_ABSI(x)
#define R_ABSY() R_ABSI(y)

#define R_INDX() \
	do { \
		cpu->pc++; \
		bus_read(operand, cycles); \
		cycles++; \
		ea = bus_read((operand + cpu->x) & 0xff, cycles); \
		cycles++; \
		ea |= bus_read((operand + cpu->x + 1) & 0xff, cycles) << 8; \
		cycles++; \
		a = bus_read(ea, cycles); \
		cycles++; \
	} while (0)

#define R_INDY() \
	do { \
		cpu->pc++; \
		ea = bus_read(operand, cycles); \
		cycles++; \
		ea += cpu->y; \
		uint16_t carry = ea & 0x0100; \
		ea &= 0xff; \
		ea |= bus_read((operand + 1) & 0xff, cycles) << 8; \
		cycles++; \
		a = bus_read(ea, cycles); \
		cycles++; \
		if (carry) { \
			ea += 0x100; \
			a = bus_read(ea, cycles); \
			cycles++; \
		} \
	} while (0)

#define W_R(r) \
	do { \
		cpu->r = a; \
	} while (0)
#define W_A() W_R(a)
#define W_X() W_R(x)
#define W_Y() W_R(y)

#define W_ZP() \
	do { \
		cpu->pc++; \
		bus_write(operand, a, cycles); \
		cycles++; \
	} while (0)

#define W_ZPX() \
	do { \
		cpu->pc++; \
		bus_read(operand, cycles); \
		cycles++; \
		bus_write((operand + cpu->x) & 0xff, a, cycles); \
		cycles++; \
	} while (0)

#define W_ZPY() \
	do { \
		cpu->pc++; \
		bus_read(operand, cycles); \
		cycles++; \
		bus_write((operand + cpu->y) & 0xff, a, cycles); \
		cycles++; \
	} while (0)

#define W_ABS() \
	do { \
		cpu->pc++; \
		uint16_t ea = operand; \
		ea |= bus_read(cpu->pc, cycles) << 8; \
		cpu->pc++; \
		cycles++; \
		bus_write(ea, a, cycles); \
		cycles++; \
	} while (0)

#define W_ABSI(i) \
	do { \
		cpu->pc++; \
		uint16_t ea = operand; \
		ea += cpu->i; \
		uint16_t carry = ea & 0x0100; \
		ea &= 0xff; \
		ea |= bus_read(cpu->pc, cycles) << 8; \
		cpu->pc++; \
		cycles++; \
		if (!carry) { \
			bus_write(ea, a, cycles); \
			cycles++; \
		} else { \
			bus_read(ea, cycles); \
			cycles++; \
			ea += 0x100; \
			bus_write(ea, a, cycles); \
			cycles++; \
		} \
	} while (0)

#define W_ABSX() W_ABSI(x)
#define W_ABSY() W_ABSI(y)

#define W_INDX() \
	do { \
		cpu->pc++; \
		bus_read(operand, cycles); \
		cycles++; \
		uint16_t ea = bus_read((operand + cpu->x) & 0xff, cycles); \
		cycles++; \
		ea |= bus_read((operand + cpu->x + 1) & 0xff, cycles) << 8; \
		cycles++; \
		bus_write(ea, a, cycles); \
		cycles++; \
	} while (0)

#define W_INDY() \
	do { \
		cpu->pc++; \
		uint16_t ea = bus_read(operand, cycles); \
		cycles++; \
		ea += cpu->y; \
		uint16_t carry = ea & 0x0100; \
		ea &= 0xff; \
		ea |= bus_read((operand + 1) & 0xff, cycles) << 8; \
		cycles++; \
		if (!carry) { \
			bus_write(ea, a, cycles); \
			cycles++; \
		} else { \
			bus_read(ea, cycles); \
			cycles++; \
			ea += 0x100; \
			bus_write(ea, a, cycles); \
			cycles++; \
		} \
	} while (0)

#define W_EA() \
	do { \
		bus_write(ea, a, cycles); \
		cycles++; \
	} while (0)

#define ADC() \
	do { \
		uint16_t res = r + a + FLAG_GET(C); \
		FLAG_SET(C, (res & 0x100) >> 8); \
		FLAG_SET(V, ((r ^ res) & (a ^ res) & 0x80) >> 7); \
		r = res & 0xff; \
	} while (0)

#define SBC() \
	do { \
		a = ~a; \
		ADC(); \
	} while (0)

#define ASL() \
	do { \
		FLAG_SET(C, (a & 0x80) >> 7); \
		a = a << 1; \
	} while (0)

#define LSR() \
	do { \
		FLAG_SET(C, a & 1); \
		a = a >> 1; \
	} while (0)

#define ROL() \
	do { \
		uint8_t c = FLAG_GET(C); \
		FLAG_SET(C, (a & 0x80) >> 7); \
		a = (a << 1) | c; \
	} while (0)

#define ROR() \
	do { \
		uint8_t c = FLAG_GET(C); \
		FLAG_SET(C, a & 1); \
		a = (a >> 1) | (c << 7); \
	} while (0)

#define AND() r = r & a
#define EOR() r = r ^ a
#define ORA() r = r | a

#define INC() a = a + 1
#define DEC() a = a - 1

#define LOAD() r = a

void cpu_reset(cpu_t *cpu) {
	cpu->a = 0;
	cpu->x = 0;
	cpu->y = 0;
	cpu->sp = 0xff;
	cpu->flags = 0;
	cpu->irq = false;
	cpu->nmi = false;
	cpu->jammed = false;

	uint8_t cycles = 0;
	FETCH_PC(0xfffc);
}

uint8_t cpu_step(cpu_t *cpu) {
	uint8_t cycles = 0;
	if (cpu->jammed) {
		return cycles;
	}

	// Service interrupts
	if (cpu->nmi) {
		cycles += 2; // ?
		PUSH_STATE();
		FETCH_PC(0xfffa);
		FLAG_SET(I, 1);
	} else if (!FLAG_GET(I) && cpu->irq) {
		cycles += 2; // ?
		PUSH_STATE();
		FETCH_PC(0xfffe);
		FLAG_SET(I, 1);
	}

	// Fetch opcode
	uint8_t opcode = bus_read(cpu->pc, cycles);
	cpu->pc++;
	cycles++;

	// Always fetch next byte
	uint8_t operand = bus_read(cpu->pc, cycles);
	cycles++;

	// Execute opcode
	switch (opcode) {
		// LDA
		case 0xa9: OP(a, R_IMM, LOAD);
		case 0xa5: OP(a, R_ZP, LOAD);
		case 0xb5: OP(a, R_ZPX, LOAD);
		case 0xad: OP(a, R_ABS, LOAD);
		case 0xbd: OP(a, R_ABSX, LOAD);
		case 0xb9: OP(a, R_ABSY, LOAD);
		case 0xa1: OP(a, R_INDX, LOAD);
		case 0xb1: OP(a, R_INDY, LOAD);

		// LDX
		case 0xa2: OP(x, R_IMM, LOAD);
		case 0xa6: OP(x, R_ZP, LOAD);
		case 0xb6: OP(x, R_ZPY, LOAD);
		case 0xae: OP(x, R_ABS, LOAD);
		case 0xbe: OP(x, R_ABSY, LOAD);

		// LDY
		case 0xa0: OP(y, R_IMM, LOAD);
		case 0xa4: OP(y, R_ZP, LOAD);
		case 0xb4: OP(y, R_ZPX, LOAD);
		case 0xac: OP(y, R_ABS, LOAD);
		case 0xbc: OP(y, R_ABSX, LOAD);

		// STA
		case 0x85: STORE(R_A, W_ZP);
		case 0x95: STORE(R_A, W_ZPX);
		case 0x8d: STORE(R_A, W_ABS);
		case 0x9d: STORE(R_A, W_ABSX);
		case 0x99: STORE(R_A, W_ABSY);
		case 0x81: STORE(R_A, W_INDX);
		case 0x91: STORE(R_A, W_INDY);

		// STX
		case 0x86: STORE(R_X, W_ZP);
		case 0x96: STORE(R_X, W_ZPY);
		case 0x8e: STORE(R_X, W_ABS);

		// STY
		case 0x84: STORE(R_Y, W_ZP);
		case 0x94: STORE(R_Y, W_ZPX);
		case 0x8c: STORE(R_Y, W_ABS);

		// ADC
		case 0x69: OP(a, R_IMM, ADC);
		case 0x65: OP(a, R_ZP, ADC);
		case 0x75: OP(a, R_ZPX, ADC);
		case 0x6d: OP(a, R_ABS, ADC);
		case 0x7d: OP(a, R_ABSX, ADC);
		case 0x79: OP(a, R_ABSY, ADC);
		case 0x61: OP(a, R_INDX, ADC);
		case 0x71: OP(a, R_INDY, ADC);

		// SBC
		case 0xe9: OP(a, R_IMM, SBC);
		case 0xe5: OP(a, R_ZP, SBC);
		case 0xf5: OP(a, R_ZPX, SBC);
		case 0xed: OP(a, R_ABS, SBC);
		case 0xfd: OP(a, R_ABSX, SBC);
		case 0xf9: OP(a, R_ABSY, SBC);
		case 0xe1: OP(a, R_INDX, SBC);
		case 0xf1: OP(a, R_INDY, SBC);

		// AND
		case 0x29: OP(a, R_IMM, AND);
		case 0x25: OP(a, R_ZP, AND);
		case 0x35: OP(a, R_ZPX, AND);
		case 0x2d: OP(a, R_ABS, AND);
		case 0x3d: OP(a, R_ABSX, AND);
		case 0x39: OP(a, R_ABSY, AND);
		case 0x21: OP(a, R_INDX, AND);
		case 0x31: OP(a, R_INDY, AND);

		// EOR
		case 0x49: OP(a, R_IMM, EOR);
		case 0x45: OP(a, R_ZP, EOR);
		case 0x55: OP(a, R_ZPX, EOR);
		case 0x4d: OP(a, R_ABS, EOR);
		case 0x5d: OP(a, R_ABSX, EOR);
		case 0x59: OP(a, R_ABSY, EOR);
		case 0x41: OP(a, R_INDX, EOR);
		case 0x51: OP(a, R_INDY, EOR);

		// ORA
		case 0x09: OP(a, R_IMM, ORA);
		case 0x05: OP(a, R_ZP, ORA);
		case 0x15: OP(a, R_ZPX, ORA);
		case 0x0d: OP(a, R_ABS, ORA);
		case 0x1d: OP(a, R_ABSX, ORA);
		case 0x19: OP(a, R_ABSY, ORA);
		case 0x01: OP(a, R_INDX, ORA);
		case 0x11: OP(a, R_INDY, ORA);

		// CMP
		case 0xc9: CMP(a, R_IMM);
		case 0xc5: CMP(a, R_ZP);
		case 0xd5: CMP(a, R_ZPX);
		case 0xcd: CMP(a, R_ABS);
		case 0xdd: CMP(a, R_ABSX);
		case 0xd9: CMP(a, R_ABSY);
		case 0xc1: CMP(a, R_INDX);
		case 0xd1: CMP(a, R_INDY);

		// CPX
		case 0xe0: CMP(x, R_IMM);
		case 0xe4: CMP(x, R_ZP);
		case 0xec: CMP(x, R_ABS);

		// CPY
		case 0xc0: CMP(y, R_IMM);
		case 0xc4: CMP(y, R_ZP);
		case 0xcc: CMP(y, R_ABS);

		// JMP
		case 0x4c:
			do {
				uint8_t a;
				R_IMM();
				uint8_t pch = bus_read(cpu->pc, cycles);
				cycles++;
				cpu->pc = a;
				cpu->pc |= pch << 8;
			} while (0);
			break;

		case 0x6c:
			do {
				uint8_t a;
				R_IMM();
				uint16_t ea = a | (bus_read(cpu->pc, cycles) << 8);
				cycles++;
				cpu->pc = bus_read(ea, cycles);
				cycles++;
				uint8_t ea2 = (ea & 0xff) + 1;
				cpu->pc |= bus_read((ea & 0xff00) | ea2, cycles) << 8;
				cycles++;
			} while (0);
			break;

		// JSR
		case 0x20:
			do {
				uint8_t a;
				R_IMM();
				bus_read(0x100 + cpu->sp, cycles);
				cycles++;
				STACK_PUSH((cpu->pc & 0xff00) >> 8); \
				STACK_PUSH(cpu->pc & 0xff); \
				uint16_t ea = a | (bus_read(cpu->pc, cycles) << 8);
				cycles++;
				cpu->pc = ea;
			} while (0);
			break;

		// RTI
		case 0x40:
			do {
				cpu->sp++;
				cycles++;
				cpu->flags = (cpu->flags & ~0b11001111) | (bus_read(0x100 + cpu->sp, cycles) & 0b11001111);
				cpu->sp++;
				cycles++;
				cpu->pc = bus_read(0x100 + cpu->sp, cycles);
				cpu->sp++;
				cycles++;
				cpu->pc |= bus_read(0x100 + cpu->sp, cycles) << 8;
				cycles++;
			} while(0);
			break;

		// RTS
		case 0x60:
			do {
				cpu->sp++;
				cycles++;
				cpu->pc = bus_read(0x100 + cpu->sp, cycles);
				cpu->sp++;
				cycles++;
				cpu->pc |= bus_read(0x100 + cpu->sp, cycles) << 8;
				cycles++;
				cpu->pc++;
				cycles++;

			} while(0);
			break;

		// Branches
		case 0x50: B(V, 0);
		case 0x70: B(V, 1);
		case 0x10: B(N, 0);
		case 0x30: B(N, 1);
		case 0xd0: B(Z, 0);
		case 0xf0: B(Z, 1);
		case 0x90: B(C, 0);
		case 0xb0: B(C, 1);

		// BRK
		case 0x00:
			cpu->pc++;
			FLAG_SET(B, 1);
			FLAG_SET(U, 1);
			PUSH_STATE();
			FLAG_SET(I, 1);
			FETCH_PC(0xfffe);
			break;

		// Flag manipulation
		case 0x18: FLAG_SET(C, 0); break;
		case 0x38: FLAG_SET(C, 1); break;
		case 0x58: FLAG_SET(I, 0); break;
		case 0x78: FLAG_SET(I, 1); break;
		case 0xd8: FLAG_SET(D, 0); break;
		case 0xf8: FLAG_SET(D, 1); break;
		case 0xb8: FLAG_SET(V, 0); break;

		// ASL
		case 0x0a: RMW(R_A, ASL, W_A);
		case 0x06: RMW(R_ZP, ASL, W_EA);
		case 0x16: RMW(R_ZPX, ASL, W_EA);
		case 0x0e: RMW(R_ABS, ASL, W_EA);
		case 0x1e: RMW(R_ABSX, ASL, W_EA);

		// LSR
		case 0x4a: RMW(R_A, LSR, W_A);
		case 0x46: RMW(R_ZP, LSR, W_EA);
		case 0x56: RMW(R_ZPX, LSR, W_EA);
		case 0x4e: RMW(R_ABS, LSR, W_EA);
		case 0x5e: RMW(R_ABSX, LSR, W_EA);

		// ROL
		case 0x2a: RMW(R_A, ROL, W_A);
		case 0x26: RMW(R_ZP, ROL, W_EA);
		case 0x36: RMW(R_ZPX, ROL, W_EA);
		case 0x2e: RMW(R_ABS, ROL, W_EA);
		case 0x3e: RMW(R_ABSX, ROL, W_EA);

		// ROR
		case 0x6a: RMW(R_A, ROR, W_A);
		case 0x66: RMW(R_ZP, ROR, W_EA);
		case 0x76: RMW(R_ZPX, ROR, W_EA);
		case 0x6e: RMW(R_ABS, ROR, W_EA);
		case 0x7e: RMW(R_ABSX, ROR, W_EA);

		// Increment / decrement
		case 0xe6: RMW(R_ZP, INC, W_EA);
		case 0xf6: RMW(R_ZPX, INC, W_EA);
		case 0xee: RMW(R_ABS, INC, W_EA);
		case 0xfe: RMW(R_ABSX, INC, W_EA);
		case 0xe8: RMW(R_X, INC, W_X);
		case 0xc8: RMW(R_Y, INC, W_Y);

		case 0xc6: RMW(R_ZP, DEC, W_EA);
		case 0xd6: RMW(R_ZPX, DEC, W_EA);
		case 0xce: RMW(R_ABS, DEC, W_EA);
		case 0xde: RMW(R_ABSX, DEC, W_EA);
		case 0xca: RMW(R_X, DEC, W_X);
		case 0x88: RMW(R_Y, DEC, W_Y);

		// Stack operations
		case 0x48: STACK_PUSH(cpu->a); break;
		case 0x08: STACK_PUSH(cpu->flags | 0b00110000); break;
		case 0x68: 
			STACK_PULL(a, 0xff);
			FLAG_SET(N, (cpu->a & 0x80) >> 7);
			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
			break;
		case 0x28: STACK_PULL(flags, 0b11001111); break;

		// Transfers
		case 0xaa:
			cpu->x = cpu->a;
			FLAG_SET(N, (cpu->a & 0x80) >> 7);
			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
			break;
		case 0xa8:
			cpu->y = cpu->a;
			FLAG_SET(N, (cpu->a & 0x80) >> 7);
			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
			break;
		case 0x8a:
			cpu->a = cpu->x;
			FLAG_SET(N, (cpu->a & 0x80) >> 7);
			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
			break;
		case 0x98:
			cpu->a = cpu->y;
			FLAG_SET(N, (cpu->a & 0x80) >> 7);
			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
			break;
		case 0xba:
			cpu->x = cpu->sp;
			FLAG_SET(N, (cpu->x & 0x80) >> 7);
			FLAG_SET(Z, (cpu->x == 0) ? 1 : 0);
			break;

		case 0x9a: cpu->sp = cpu->x; break;

		// NOP
		case 0xea: break;

		// BIT
		case 0x24: BIT(R_ZP);
		case 0x2C: BIT(R_ABS);

		// Jam on everything else
		default:
			cpu->jammed = true;
			break;
	}

	return cycles;
}