#include <u.h>
#include <libc.h>
#include "6502.h"

byte regA, regX, regY, regP, regS;
word regPC;
byte mem[65536], memt[65536];

enum {
	flagC = 1,
	flagZ = 2,
	flagI = 4,
	flagD = 8,
	flagB = 16,
	flagV = 64,
	flagN = 128
};


int readhandler(int addr);
void writehandler(int val, int addr);
void undefined(void);

void push(byte b) {
	mem[0x100 + regS--] = b;
}

void pop(byte *b) {
	*b = mem[0x100 + ++regS];
}

word adimm(void) {
	return regPC++;
}

word adabs(void) {
	word r = mem[regPC] | (mem[regPC+1] << 8);
	regPC += 2;
	return r;
}

word adabsx(void) {
	return adabs() + regX;
}

word adabsy(void) {
	return adabs() + regY;
}

word adzp(void) {
	return mem[regPC++];
}

word adzpx(void) {
	return (adzp() + regX) & 0xFF;
}

word adindzpx(void) {
	word a = adzpx();
	word b = mem[a] | (mem[a+1] << 8);
	return b;
}

word adindzpy(void) {
	word a = adzp();
	word b = (mem[a] | (mem[a+1] << 8)) + regY;
	return b;
}

void branch(void) {
	signed char b = mem[adimm()];
	regPC += b;	
}

byte memread(word w) {
	if(memt[w] & 1) return mem[w];
	return readhandler(w);
}

void memwrite(word w, byte b) {
	if(memt[w] & 2) mem[w] = b;
	else writehandler(b, w);
}

void szflag(byte v) {
	regP &= ~(flagN | flagZ);
	regP |= v & flagN;
	if(v == 0) regP |= flagZ;
}

void setcarry(byte i) {
	regP &= ~flagC;
	if(i) regP |= flagC;
}

void compare(byte a, byte b) {
	regP &= ~(flagN | flagZ | flagC);
	if(a == b) regP |= flagZ;
	regP |= a & 0x80;
	if(a >= b) regP |= flagC;
}

void step(void) {
	byte opcode = mem[regPC++], a, b, c, r;
	word w;
	switch(opcode) {
		case 0x00: regP |= flagB; irq(); break;
		case 0x08: push(regP); break;
		case 0x0A: setcarry(regA & 128); regA <<= 1; szflag(regA); break;
		case 0x10: if(!(regP & flagN)) branch(); else regPC++; break;
		case 0x18: regP &= ~flagC; break;
		case 0x20: 	
			   w = adabs();
			   regPC--;
			   push(regPC >> 8);
			   push(regPC & 0xFF);
			   regPC = w;
			   break;
		case 0x28: pop(&regP); break;
		case 0x30: if(regP & flagN) branch(); else regPC++; break;
		case 0x38: regP |= flagC; break;
		case 0x48: push(regA); break;
		case 0x4A: setcarry(regA & 1); regA >>= 1; szflag(regA); break;
		case 0x4C: regPC = adabs(); break;
		case 0x50: if(!(regP & flagV)) branch(); else regPC++; break;
		case 0x60:
			   pop((byte*) (&regPC));
			   pop((byte*) (&regPC) + 1);
			   regPC++;
			   break;
		case 0x58: regP &= ~flagI; break;
		case 0x68: pop(&regA); break;
		case 0x70: if(regP & flagV) branch(); else regPC++; break;
		case 0x78: regP |= flagI; break;
		case 0x88: regY--; break;
		case 0x89: undefined(); break; /* would be considered STA # */
		case 0x8A: regA = regX; break;
		case 0x90: if(!(regP & flagC)) branch(); else regPC++; break;
		case 0x98: regA = regY; break;
		case 0x9A: regS = regX; break;
		case 0xA2: regX = memread(adimm()); szflag(regX); break;
		case 0xA8: regY = regA; break;
		case 0xAA: regX = regA; break;
		case 0xB0: if(regP & flagC) branch(); else regPC++; break;
		case 0xB8: regP &= ~flagV; break;
		case 0xBA: regX = regS; break;
		case 0xC8: regY++; break;
		case 0xCA: regX--; break;
		case 0xD0: if(!(regP & flagZ)) branch(); else regPC++; break;
		case 0xD4: undefined(); break; /* would be considered CPY zp, X */
		case 0xD8: regP &= ~flagD; break;
		case 0xDC: undefined(); break; /* would be considered CPY abs, X */
		case 0xE8: regX++; break;
		case 0xEA: break;
		case 0xF0: if(regP & flagZ) branch(); else regPC++; break;
		case 0xF4: undefined(); break; /* would be considered CPX zp, X */
		case 0xFC: undefined(); break; /* would be considered CPX abs, X */
		case 0xF8: regP |= flagD; break;
		default: goto undecoded;
	}
	return;
undecoded:
	a = opcode >> 5;
	b = (opcode >> 2) & 7;;
	c = opcode & 3;
	if(c == 1) {
		word (*ad[8])(void) = {adindzpx, adzp, adimm, adabs, adindzpy, adzpx, adabsy, adabsx};
		switch(a) {
			case 0: regA |= memread(ad[b]()); szflag(regA); break;
			case 1: regA &= memread(ad[b]()); szflag(regA); break;
			case 2: regA ^= memread(ad[b]()); szflag(regA); break;
			case 4: memwrite(ad[b](), regA); break;
			case 5: regA = memread(ad[b]()); szflag(regA); break;
			case 6: compare(regA, memread(ad[b]())); break;
			case 7:
				w = regA - memread(ad[b]()) - 1 + (regP & flagC);
				regP &= ~flagC;
				if(w < 0x100) regP |= flagC;
				regA = w & 0xFF;
				szflag(regA);
				break;
			default: undefined();
		}
	}
	else if(c == 2) {
		word (*ad[8])(void) = {0, adzp, 0, adabs, 0, adzpx, 0, adabsx};
		if(ad[b] == 0) undefined();
		switch(a) {
			case 5: regX = memread(ad[b]()); szflag(regX); break;
			case 6: w = ad[b](); 
				r = memread(w) - 1;
				memwrite(w, r);
				szflag(r);
				break;
			case 7: w = ad[b]();
				r = memread(w) + 1;
				memwrite(w, r);
				szflag(r);
				break;
			default: undefined();
		}
	}
	else if(c == 0) {
		word (*ad[8])(void) = {adimm, adzp, 0, adabs, 0, adzpx, 0, adabsx};
		if(ad[b] == 0) undefined();
		switch(a) {
			case 5: regY = memread(ad[b]()); szflag(regY); break;
			case 6: compare(regY, memread(ad[b]())); break;
			case 7: compare(regX, memread(ad[b]())); break;
			default: undefined();
		}
	}
	else undefined();
}

void nmi(void) {
	push(regPC & 0xFF);
	push(regPC >> 8);
	push(regP);
	regPC = mem[0xFFFA] | (mem[0xFFFB] << 8);
}

void irq(void) {
	push(regPC & 0xFF);
	push(regPC >> 8);
	push(regP);
	regPC = mem[0xFFFE] | (mem[0xFFFF] << 8);
}