#include <libgen.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "cpu.h"
#include "opcodes.h"
#include "rom.h"

#define MAX(a, b)	((a > b) ? a : b)

#define STATUS_UPDATE_ZERO(r)		\
	(regs.status.zero = r == 0)
#define STATUS_UPDATE_NEGATIVE(r)	\
	(regs.status.negative = ((r & (1 << 7)) != 0))
#define STATUS_UPDATE_NZ(r)		\
	{STATUS_UPDATE_ZERO(r);		\
	 STATUS_UPDATE_NEGATIVE(r);}
#define STATUS_TO_INT()			\
	(regs.status.carry | (regs.status.zero << 1) \
	    | (regs.status.interrupt_disable << 2) | (regs.status.decimal_mode << 3) \
	    | (regs.status.brk << 4) | (regs.status.unused << 5) \
	    | (regs.status.overflow << 6) | (regs.status.negative << 7))

#define MEMORY_MIRROR(addr)	\
	if (addr < 0x2000)	\
		addr &= 0x07FF;	\
	else if (addr < 0x4000)	\
		addr &= 0x2007;

#define PUSH(b)	\
	(memwrite(0x0100 + regs.sp--, b))
#define PULL()	\
	(peek(0x0100 + ++regs.sp))

struct cpu_flags {
	uint8_t carry : 1;
	uint8_t zero : 1;
	uint8_t interrupt_disable : 1;
	uint8_t decimal_mode : 1;
	uint8_t brk : 1;
	uint8_t unused : 1;
	uint8_t overflow : 1;
	uint8_t negative : 1;
};

struct registers {
	uint8_t a, x, y, sp;
	struct cpu_flags status;
	uint16_t pc;
};
struct registers regs = {0};

struct Rom rom = {0};

/* 64K address space, 16bit words */
uint8_t memory[0x16000];

uint32_t cycles = 0;

static uint8_t
peek(uint16_t addr)
{
	MEMORY_MIRROR(addr);

	if (addr > 0x7FFF) {
		if (rom.prg_rom_size == 0x4000)
			return rom.prg_rom[(addr - 0x8000) % 0x4000];
		else if (rom.prg_rom_size == 0x8000)
			return rom.prg_rom[addr - 0x8000];
	} else {
		return memory[addr];
	}
}

static uint16_t
peek16(uint16_t addr)
{
	/* bytes are stored in little-endian (low then high) */
	return peek(addr) | (peek(addr + 1) << 8);
}

static void
memwrite(uint16_t addr, uint8_t byte)
{
	MEMORY_MIRROR(addr);

	memory[addr] = byte;
}

static void
memwrite16(uint16_t addr, uint16_t word)
{
	MEMORY_MIRROR(addr);

	/* bytes are stored in little-endian (low then high) */
	memory[addr] = word & 0xFF;
	memory[addr + 1] = (word & 0xFF00) >> 8;
}

static uint8_t
opcode_arg(enum addressing_mode mode)
{
	uint16_t arg, val;

	if (mode != AM_ABS && mode != AM_ABS_X && mode != AM_ABS_Y) {
		arg = peek(regs.pc++);
	} else {
		arg = peek16(regs.pc);
		regs.pc += 2;
	}

	switch (mode) {
	case AM_IMM:
	case AM_REL:
		val = arg;
		break;
	case AM_ZP:
		val = peek(arg % 256);
		break;
	case AM_ZP_X:
		val = peek((arg + regs.x) % 256);
		break;
	case AM_ZP_Y:
		val = peek((arg + regs.y) % 256);
		break;
	case AM_ABS:
		val = peek16(arg);
		break;
	case AM_ABS_X:
		val = peek16(arg + regs.x);
		break;
	case AM_ABS_Y:
		val = peek16(arg + regs.y);
		break;
	case AM_IND_X:
		val = peek(peek((arg + regs.x) % 256) + peek((arg + regs.x + 1) % 256) * 256);
		break;
	case AM_IND_Y:
		val = peek(peek(arg) + peek((arg + 1) % 256) * 256 + regs.y);
		break;
	default:
		fprintf(stderr, "INVALID ADDRESSING MODE %i\n", mode);
		abort();
	}

	return val;
}

static uint16_t
opcode_mem(enum addressing_mode mode)
{
	uint16_t arg, val;

	if (mode == AM_ACC || mode == AM_NONE) {
		return 0;
	} else if (mode != AM_ABS && mode != AM_ABS_X && mode != AM_ABS_Y && mode != AM_IND && mode) {
		arg = peek(regs.pc++);
	} else {
		arg = peek16(regs.pc);
		regs.pc += 2;
	}

	switch (mode) {
	case AM_ZP:
		val = arg % 256;
		break;
	case AM_ZP_X:
		val = (arg + regs.x) % 256;
		break;
	case AM_ZP_Y:
		val = (arg + regs.y) % 256;
		break;
	case AM_IMM:
	case AM_REL:
	case AM_ABS:
		val = arg;
		break;
	case AM_ABS_X:
		val = arg + regs.x;
		break;
	case AM_ABS_Y:
		val = arg + regs.y;
		break;
	case AM_IND:
		val = peek(arg) | (peek(((arg + 1) & 0xFF) | (arg & 0xFF00)) << 8);
		break;
	case AM_IND_X:
		val = peek((arg + regs.x) % 256) + peek((arg + regs.x + 1) % 256) * 256;
		break;
	case AM_IND_Y:
		val = peek(arg) + peek((arg + 1) % 256) * 256 + regs.y;
		break;
	default:
		fprintf(stderr, "opcode_mem INVALID ADDRESSING MODE %i\n", mode);
		abort();
	}

	return val;
}

void
ADC(uint16_t arg)
{
	uint16_t sum;	// 16-bit sum makes it easier to determine carry flag

	sum = regs.a + arg + regs.status.carry;

	regs.status.carry = sum > 0xFF;
	/* overflow flag formula: https://stackoverflow.com/a/29224684 */
	regs.status.overflow = (~(regs.a ^ arg) & (regs.a ^ sum) & 0x80) != 0;
	regs.a = sum & 0xFF;

	STATUS_UPDATE_NZ(regs.a);
}

void
AND(uint16_t arg)
{
	regs.a &= arg;

	STATUS_UPDATE_NZ(regs.a);
}

void
ASL_acc(uint16_t arg)
{
	uint16_t tmp;

	tmp = regs.a << 1;
	regs.a = tmp & 0xFF;

	regs.status.carry = tmp > 0xFF;
	STATUS_UPDATE_NZ(regs.a);
}

void
ASL(uint16_t mem)
{
	uint16_t tmp;

	tmp = peek(mem) << 1;
	memwrite(mem, tmp & 0xFF);

	regs.status.carry = tmp > 0xFF;
	STATUS_UPDATE_NZ(tmp);
}

void
BCC(uint16_t arg)
{
	if (regs.status.carry == 0)
		regs.pc += arg, cycles++;
}

void
BCS(uint16_t arg)
{
	if (regs.status.carry == 1)
		regs.pc += arg, cycles++;
}

void
BEQ(uint16_t arg)
{
	if (regs.status.zero == 1)
		regs.pc += arg, cycles++;
}

void
BIT(uint16_t arg)
{
	uint8_t tmp = peek(arg);
	regs.status.zero = (regs.a & tmp) == 0;
	regs.status.overflow = (tmp & (1 << 6)) != 0;
	STATUS_UPDATE_NEGATIVE(tmp);
}

void
BMI(uint16_t arg)
{
	if (regs.status.negative == 1)
		regs.pc += arg, cycles++;
}

void
BNE(uint16_t arg)
{
	if (regs.status.zero == 0)
		regs.pc += arg, cycles++;
}

void
BPL(uint16_t arg)
{
	if (regs.status.negative == 0)
		regs.pc += arg, cycles++;
}

void
BRK(uint16_t arg)
{
	/* TODO: push regs.pc and regs.status to stack and load IRQ vector */
	regs.status.brk = 1;
	// exit(0);
}

void
BVC(uint16_t arg)
{
	//regs.status.overflow = (STATUS_TO_INT() & (1 << 6)) == 0;
	//if (regs.status.overflow == 0)
	if ((STATUS_TO_INT() & (1 << 6)) == 0)
		regs.pc += arg, cycles++;
}

void
BVS(uint16_t arg)
{
	regs.status.overflow = (STATUS_TO_INT() & (1 << 6)) != 0;
	if (regs.status.overflow == 1)
		regs.pc += arg, cycles++;
}

void
CLC(uint16_t arg)
{
	regs.status.carry = 0;
}

void
CLD(uint16_t arg)
{
	regs.status.decimal_mode = 0;
}

void
CLI(uint16_t arg)
{
	regs.status.interrupt_disable = 0;
}

void
CLV(uint16_t arg)
{
	regs.status.overflow = 0;
}

void
CMP(uint16_t arg)
{
	uint8_t tmp;

	tmp = regs.a - arg;

	regs.status.carry = regs.a >= arg;
	regs.status.zero = regs.a == arg;
	STATUS_UPDATE_NEGATIVE(tmp);
}

void
CPX(uint16_t arg)
{
	uint8_t tmp;

	tmp = regs.x - arg;

	regs.status.carry = regs.x >= arg;
	regs.status.zero = regs.x == arg;
	STATUS_UPDATE_NEGATIVE(tmp);
}

void
CPY(uint16_t arg)
{
	uint8_t tmp;

	tmp = regs.y - arg;

	regs.status.carry = regs.y >= arg;
	regs.status.zero = regs.y == arg;
	STATUS_UPDATE_NEGATIVE(tmp);
}

void
DEC(uint16_t mem)
{
	memwrite(mem, peek(mem) - 1);

	STATUS_UPDATE_NZ(peek(mem));
}

void
DEX(uint16_t arg)
{
	regs.x--;

	STATUS_UPDATE_NZ(regs.x);
}

void
DEY(uint16_t arg)
{
	regs.y--;

	STATUS_UPDATE_NZ(regs.y);
}

void
EOR(uint16_t arg)
{
	regs.a ^= arg;

	STATUS_UPDATE_NZ(regs.a);
}

void
INC(uint16_t mem)
{
	memwrite(mem, peek(mem) + 1);

	STATUS_UPDATE_NZ(peek(mem));
}

void
INX(uint16_t arg)
{
	regs.x++;

	STATUS_UPDATE_NZ(regs.x);
}

void
INY(uint16_t arg)
{
	regs.y++;

	STATUS_UPDATE_NZ(regs.y);
}

void
JMP(uint16_t arg)
{
	regs.pc = arg;
}

void
JSR(uint16_t arg)
{
	uint16_t tmp = regs.pc - 1;

	/*
	 * first push high-byte of return address then low-byte
	 * https://www.masswerk.at/6502/6502_instruction_set.html
	 */
	PUSH((tmp & 0xFF00) >> 8);
	PUSH(tmp & 0xFF);
	regs.pc = arg;
}

void
LDA(uint16_t arg)
{
	regs.a = arg;

	STATUS_UPDATE_NZ(regs.a);
}

void
LDX(uint16_t arg)
{
	regs.x = arg;

	STATUS_UPDATE_NZ(regs.x);
}

void
LDY(uint16_t arg)
{
	regs.y = arg;

	STATUS_UPDATE_NZ(regs.y);
}

void
LSR_acc(uint16_t arg)
{
	regs.status.carry = regs.a & 1;	// bit 0 in carry
	regs.a >>= 1;
	regs.a &= ~(1 << 7);		// bit 7 cleared

	STATUS_UPDATE_NZ(regs.a);
}

void
LSR(uint16_t mem)
{
	uint8_t tmp;

	tmp = peek(mem);

	regs.status.carry = tmp & 1;	// bit 0 in carry
	tmp >>= 1;
	tmp &= ~(1 << 7);		// bit 7 cleared

	memwrite(mem, tmp);

	STATUS_UPDATE_NZ(tmp);
}

void
NOP(uint16_t arg)
{
	return;
}

void
ORA(uint16_t arg)
{
	regs.a |= arg;

	STATUS_UPDATE_NZ(regs.a);
}

void
PHA(uint16_t arg)
{
	PUSH(regs.a);
}

void
PHP(uint16_t arg)
{
	PUSH(STATUS_TO_INT() | (1 << 4));
}

void
PLA(uint16_t arg)
{
	regs.a = PULL();

	STATUS_UPDATE_NZ(regs.a);
}

void
PLP(uint16_t arg)
{
	uint8_t status;

	status = PULL();

	regs.status.carry = (status & 1) != 0;
	regs.status.zero = (status & (1 << 1)) != 0;
	regs.status.interrupt_disable = (status & (1 << 2)) != 0;
	regs.status.decimal_mode = (status & (1 << 3)) != 0;
	regs.status.brk = 0;
	regs.status.unused = 1;
	regs.status.overflow = (status & (1 << 6)) != 0;
	regs.status.negative = (status & (1 << 7)) != 0;
}

void
ROL_acc(uint16_t arg)
{
	uint8_t carry;
	carry = (regs.a & (1 << 7)) != 0;

	regs.a <<= 1;
	regs.a |= regs.status.carry;

	regs.status.carry = carry;
	STATUS_UPDATE_NZ(regs.a);
}

void
ROL(uint16_t mem)
{
	uint8_t carry, tmp;
	carry = (peek(mem) & (1 << 7)) != 0;

	tmp = (peek(mem) << 1) | regs.status.carry;
	memwrite(mem, tmp);

	regs.status.carry = carry;
	STATUS_UPDATE_NZ(regs.a);
}

void
ROR_acc(uint16_t arg)
{
	uint8_t carry;
	carry = regs.a & 1;

	regs.a >>= 1;
	regs.a |= regs.status.carry << 7;

	regs.status.carry = carry;
	STATUS_UPDATE_NZ(regs.a);
}

void
ROR(uint16_t mem)
{
	uint8_t carry, tmp;
	carry = peek(mem) & 1;

	tmp = (peek(mem) >> 1) | (regs.status.carry << 7);
	memwrite(mem, tmp);

	regs.status.carry = carry;
	STATUS_UPDATE_NZ(tmp);
}

void
RTI(uint16_t arg)
{
	PLP(0);
	regs.pc = PULL() | (PULL() << 8);
}

void
RTS(uint16_t arg)
{
	regs.pc = (PULL() | (PULL() << 8)) + 1;
}

void
SBC(uint16_t arg)
{
	/* SBC is described online as ADC with argument as two's complement */
	ADC(~(uint8_t)arg);
}

void
SEC(uint16_t arg)
{
	regs.status.carry = 1;
}

void
SED(uint16_t arg)
{
	regs.status.decimal_mode = 1;
}

void
SEI(uint16_t arg)
{
	regs.status.interrupt_disable = 1;
}

void
STA(uint16_t mem)
{
	uint8_t tmp = peek(mem);
	memwrite(mem, regs.a);
}

void
STX(uint16_t mem)
{
	uint8_t tmp = peek(mem);
	memwrite(mem, regs.x);
}

void
STY(uint16_t mem)
{
	uint8_t tmp = peek(mem);
	memwrite(mem, regs.y);
}

void
TAX(uint16_t arg)
{
	regs.x = regs.a;

	STATUS_UPDATE_NZ(regs.x);
}

void
TAY(uint16_t arg)
{
	regs.y = regs.a;

	STATUS_UPDATE_NZ(regs.y);
}

void
TSX(uint16_t arg)
{
//	regs.x = PULL();
	regs.x = regs.sp;

	STATUS_UPDATE_NZ(regs.x);
}

void
TXA(uint16_t arg)
{
	regs.a = regs.x;

	STATUS_UPDATE_NZ(regs.a);
}

void
TXS(uint16_t arg)
{
//	PUSH(regs.x);
	regs.sp = regs.x;
}

void
TYA(uint16_t arg)
{
	regs.a = regs.y;

	STATUS_UPDATE_NZ(regs.a);
}

static void
interpret(void)
{
	uint8_t op;
	uint16_t arg, mem, deref;
	enum addressing_mode mode;

	for (;;) {
		printf("%04X  ", regs.pc);

		op = peek(regs.pc++);

		printf("%02X", op);
		for (uint8_t i = 0; i < opcodes[op].bytes - 1; i++)
			printf(" %02X", peek(regs.pc + i));
		printf("\t%s ", opcodes[op].name);

		mode = opcodes[op].mode;
		arg = opcode_mem(mode);

		switch (mode) {
		case AM_IMM:
			printf("#$%02X", arg);
			break;
		case AM_ZP:
			printf("$%02X", arg);
			break;
		case AM_ZP_X:
			printf("$%02X,X", arg);
			break;
		case AM_ZP_Y:
			printf("$%02X,Y", arg);
			break;
		case AM_REL:
			printf("$%04X", regs.pc + arg);
			break;
		case AM_ABS:
			printf("$%04X", arg);
			break;
		case AM_ABS_X:
			printf("$%04X,X", arg);
			break;
		case AM_ABS_Y:
			printf("$%04X,Y", arg);
			break;
		case AM_IND:
			printf("($%04X) = %04X\t\t", arg, peek16(arg));
			break;
		case AM_ACC:
		case AM_NONE:
			putchar('\t');
			break;
		default:
			printf("\t\t\t\t");
			break;
		}

		if (opcodes[op].didmemory) printf(" = %02X", peek(arg));
		printf("\t\t\t");

		printf("A:%02X X:%02X Y:%02X P:%02X SP:%02X CYC:%d %08b\n",
		    regs.a, regs.x, regs.y, STATUS_TO_INT(), regs.sp, cycles, STATUS_TO_INT());

		opcodes[op].instr(arg);
		cycles += opcodes[op].cycles;
	}
loop_exit:
}

/* https://www.nesdev.org/wiki/CPU_power_up_state */
void
cpu_init(void)
{
	regs.a = regs.x = regs.y = 0;
	regs.pc = 0xFFFC;
	regs.sp = 0xFD;

	//memset(&regs.status, 0, sizeof(regs.status));
	regs.status.interrupt_disable = 1;
	regs.status.unused = 1;

	cycles += 7;
}

int
main(int argc, char *argv[])
{
	FILE *fp;
	uint8_t *buf;
	size_t buflen;

	if (argc != 2) {
		fprintf(stderr, "Usage: %s rom.nes\n", basename(argv[0]));
		return 1;
	}

	fp = fopen(argv[1], "r");
	fseek(fp, 0, SEEK_END);
	buflen = ftell(fp);
	buf = calloc(1, buflen);
	fseek(fp, 0, SEEK_SET);
	if (fread(buf, 1, buflen, fp) != buflen && ferror(fp)) {
		fprintf(stderr, "file %s was not read properly\n", argv[1]);
		clearerr(fp);
		fclose(fp);
		return 1;
	}
	fclose(fp);

	parse_rom(buf, buflen, &rom);
	free(buf);

	cpu_init();

	/* TODO: move to separate file? */
	if (rom.mapper != 0) {
		fprintf(stderr, "Only iNES ROMs using Mapper 0 are supported for now.\n");
		return 1;
	}

	memwrite16(0xFFFC, 0xC000);
	regs.pc = 0xC000;

	interpret();

	printf("\n$02 = %02X\n", memory[0x02]);
	printf("$03 = %02X\n", memory[0x03]);

	free_rom(&rom);

	return 0;
}