/* **************************************************************************** 
   * Contains some variable declarations as well as register initializations. * 
   * Also conatins implementations of all the operations codes.	 			  *
   * Authored by Brian Pescatore and Tom Gowing.							  *
   **************************************************************************** */

#define DEBUG
//#define CART_DEBUG

// 6502 register setup
unsigned char	rS;
unsigned char	rA;
unsigned char	rX;
unsigned char	rY;
unsigned char	rSP;
unsigned int	rPC;

// Emulation variables setup
unsigned char  	rOP;
unsigned int   	rOPA;
unsigned char  	rTMP1;
unsigned char  	rTMP2;
unsigned char  	rTMP3;
unsigned char  	rTMP4;

volatile unsigned char 	instr_byte[3];
unsigned char 	status;
unsigned char*	MEMORY;
unsigned char	PPU_register[8];
unsigned char	IO_register[32];

unsigned char*  accessed_reg;

unsigned char	NMI_FLAG;
unsigned char 	SPR_WRITE_FLAG;


/* 
	***************************
	*  Generic Memory Access  *
	*************************** 

	*********** Memory Layout ***********
	$0000-$00FF: Zero Page
	$0100-$01FF: Stack
	$0200-$07FF: RAM
	$0800-$1FFF: Mirrors $0000-$07FF
	$2000-$2007: IO Registers--See below
    $2008-$3FFF: Mirrors $2000-$2007
	$4000-$401F: IO Registers--See below
	$4020-$5FFF: Expansion ROM
	$6000-$7FFF: SRAM (Save RAM)
	$8000-$FFFF: PRG-ROM (Program ROM)
	*************************************

	*********** I/O Registers ***********
	  PPU
	$2000	PPU Control Register 1
	$2001	PPU Control Register 2
	$2002	PPU Status Register
	$2003	SPR-RAM Address Register
	$2004	SPR-RAM I/O Register
	$2005	VRAM Address Register 1
	$2006	VRAM Address Register 2
	$2007	VRAM I/O Register
	  Audio & Other
	$4000	pAPU Pulse 1 Control Register
	$4001	pAPU Pulse 1 Ramp Control Register
	$4002 	pAPU Pulse 1 Fine Tune (FT) Register
	$4003  	pAPU Pulse 1 Coarse Tune (CT) Register
	$4004  	pAPU Pulse 2 Control Register
	$4005  	pAPU Pulse 2 Ramp Control Register
	$4006  	pAPU Pulse 2 Fine Tune Register
	$4007  	pAPU Pulse 2 Coarse Tune Register
	$4008  	pAPU Triangle Control Register 1
	$4009  	pAPU Triangle Control Register 2
	$400A  	pAPU Triangle Frequency Register 1
	$400B  	pAPU Triangle Frequency Register 2
	$400C  	pAPU Noise Control Register 1
	$400E  	pAPU Noise Frequency Register 1
	$400F  	pAPU Noise Frequency Register 2
	$4010  	pAPU Delta Modulation Control Register
	$4011  	pAPU Delta Modulation D/A Register
	$4012  	pAPU Delta Modulation Address Register
	$4013  	pAPU Delta Modulation Data Length Register
	$4014  	Sprite DMA Register
	$4015	pAPU Sound/Vertical Clock Signal Register
	$4016	Controller 1
	$4017	Controller 2

	*************************************

*/

#ifdef CART_DEBUG
	unsigned char PROGRAM[256] = 
	  {
	  	0xA2,0xF0,0x30,0x01,0x7F,0x38,0xB0,0x01,0x7F, // This "Program" will test the bit set functions as well as the branching functions.
	   	0x78,0xD8,0xF8,0x18,0x90,0x01,0x7F,0x08,0x58,
		0x28,0xA9,0xFF,0xA0,0xF1,0x84,0x00,0x65,0x00,
		0x07,0x01,0x7F,0xB8,0x50,0x01,0x7F,0xAA,0xA8,
		0xE8,0x8A,0x88,0x98,0xC8,0xCA,0x48,0x4A,0x68,
		0x38,0x2A,0x2A,0x38,0x6A,0xA9,0x00,0xF0,0x01,
		0x7F,0xA9,0x01,0x0D,0x01,0x7F,0x10,0x01,0x7F,
		0xA9,0x55,0x49,0xAA,0xB8,0xA9,0xF0,0x29,0x55,
		0xBA,0x08,0x9A,0xA9,0x54,0x85,0x01,0xE6,0xA9,
		0x01,0x24,0x01,0xC6,0x01,0xA9,0x01,0x24,0x01
	  };
#endif

#ifdef DEBUG
#define SET_ADDR_HI(x)		PORTC = (x)
#define SET_ADDR_LO(x)		PORTB = (x)
#else
#define SET_ADDR_HI(x)		PORTC = (x)
#define SET_ADDR_LO(x)		PORTD = (x)
#endif

#define READ_DATA_PORT()	PINA

#ifndef CART_DEBUG

#define READ_PC(x)			SET_ADDR_HI(rPC >> 8);	 	\
							SET_ADDR_LO(rPC & 0xFF); 	\
							rPC++;					 	\
							instr_byte[x] = READ_DATA_PORT()

#define READ_PRGROM(x)		SET_ADDR_HI(x >> 8);		\
							SET_ADDR_LO(x & 0xFF);		\
							return READ_DATA_PORT()
#else

#define READ_PC(x)			instr_byte[x] = PROGRAM[rPC & 0xFF]; rPC++

#define READ_PRGROM(x)		return PROGRAM[x & 0xFF]

#endif

//#define ENABLE_APU_SPI		PORTB &= 0x10
//#define ENABLE_PPU_SPI		asm("nop\n\r") 

#define SPI_WAIT			while (!(SPSR & (1<<SPIF)));   

#define SPI_PPU_SEND(c,d)	asm("nop");

#define SPI_APU_SEND(c,d)	asm("nop");

#define NMI_ACTIVATE		((NMI_FLAG==0) && ((PINB & 0x01)==1))

#define NMI_DEACTIVATE		((NMI_FLAG==1) && ((PINB & 0x01)==0))

#define NMI_PC				SET_ADDR_HI(0xFFFA >> 8);	 	\
							SET_ADDR_LO(0xFFFA & 0xFF); 	\
							rPC = READ_DATA_PORT() << 8;	\
							SET_ADDR_HI(0xFFFB >> 8);		\
							SET_ADDR_LO(0xFFFB & 0xFF);		\
							rPC |= READ_DATA_PORT()

#define READ_SAVE_RAM(x)	return 0

#define READ_EXP_ROM(x) 	return 0

// Likely need to switch to another jumplist to deal with each register
#define READ_IO_REGISTER(x)							\
		rTMP3 = (x & 0x1F);							\
		if (rTMP3 > 0x15) {							\
			rTMP2 = IO_register[rTMP3];				\
			IO_register[rTMP3] >>= 1;				\
			return rTMP2;							\
		}											\
		accessed_reg = &(IO_register[x & 0x001F]);	\
		return *accessed_reg

// Likely need to switch to another jumplist to deal with each register
#define READ_PPU_REGISTER(x) 						\
		accessed_reg = &(PPU_register[x & 0x0007]);	\
		return *accessed_reg

// We already know that x is less than 0x2000
#define READ_RAM(x) return (MEMORY[ x & 0x07FF])


inline char mem(unsigned int x) {
	if 	(x >= 0x8000) {	
		READ_PRGROM(x); 
	} else if (x >= 0x6000) { 
		READ_SAVE_RAM(x);
	} else if (x >= 0x4020) {
		READ_EXP_ROM(x);
	} else if (x >= 0x4000) {
		READ_IO_REGISTER(x);
	} else if (x >= 0x2000) {
		READ_PPU_REGISTER(x);
	} else {
		READ_RAM(x);
	}
}

#define PRG_MEM_WRITE		//FIXME

#define DATA_OUT(d)			DDRA = 0xFF;				\
							PORTA = d;					\
							DDRA = 0x00

#define WRITE_PRGROM(d,a)  	PRG_MEM_WRITE;				\
							SET_ADDR_HI(a >> 8);		\
							SET_ADDR_LO(a & 0xFF);		\
							DATA_OUT(d)

#define WRITE_SAVE_RAM(d,a)

#define WRITE_EXP_ROM(d,a)	/* HOW? */

#define WRITE_IO_REGISTER(d,a)					\
	rTMP3 = (a & 0x1F);							\
	IO_register[rTMP3] = d						
/*	switch (rTMP3) {							
		case 0x14:	SPR_WRITE_FLAG = 255; 		\
					rTMP2 = mem(d << 8);		\
					SPI_PPU_SEND(0x14,rTMP2);	\
					break;						\
		case 0x16:	ENABLE_APU_SPI;				\
					SPDR = 0x16;				\
					SPI_WAIT;					\
					IO_register[0x16] = SPDR;	\
					SPDR = 0x17;				\
					IO_register[0x17] = SPDR;	\
					break;						\
	} */											

	
// $2005, $2006 latching need to be done on the PPU side of things
#define WRITE_PPU_REGISTER(d,a) 	\
	rTMP3 = (a & 0x07);				\
	PPU_register[rTMP3] = d

#define WRITE_RAM(d,a)		MEMORY[a & 0x07FF] = d

#define write_mem(d,a) 							\
	if 	(((unsigned int)a) >= 0x8000) {			\
		WRITE_PRGROM(d,((unsigned int)a)); 		\
	} else if (((unsigned int)a) >= 0x6000) { 	\
		WRITE_SAVE_RAM(d,((unsigned int)a));	\
	} else if (((unsigned int)a) >= 0x4020) {	\
		WRITE_EXP_ROM(d,((unsigned int)a));		\
	} else if (((unsigned int)a) >= 0x4000) {	\
		WRITE_IO_REGISTER(d,((unsigned int)a));	\
	} else if (((unsigned int)a) >= 0x2000) {	\
		WRITE_PPU_REGISTER(d,((unsigned int)a));\
	} else {									\
		WRITE_RAM(d,((unsigned int)a));			\
	}							

#define ONE_BYTE 		asm("nop\n\r");\
						fprintf(stdout,"One Byte Operation.\n\r")						

#define TWO_BYTE		READ_PC(1);\
						fprintf(stdout,"Two Byte Operation.\n\r")
						

#define THREE_BYTE		READ_PC(1);	\
						READ_PC(2);	\
						fprintf(stdout,"Three Byte Operation.\n\r")


#define PUSH_STACK(v)	MEMORY[0x100 + rSP] = v; rSP++		
#define POP_STACK(r)	rSP--; r = MEMORY[0x100+rSP]		


/* **********************
   *  Addressing Modes  *
   ********************** */

/** - Implicit -
  * Memory address is implied by the instruction
  * Example: RTI (return from subroutine) has an immplied address use 
  */

  // Handled by individual instructions.
  
/** - Immediate -
  * Allows direct access addressed using 8 bits
  * Indicated by # followed by a number ( #10 or #$0A ) 
  */

#define IMMEDIATE		\
						fprintf(stdout,"Immediate Addressing.\n\r");\
						rOP  = instr_byte[1]

/** - Accumulator -
  * Operate directly upon the accumulator register
  * Specified using the operand value A
  */

#define ACCUM			\
						fprintf(stdout,"Accumulator Addressing.\n\r");\
						rOP  = rA
  
/** - Zero Page -
  * Allows immediate addressing into the zero page of memory
  * Specified with a numeric 8 bit operand
  */

#define ZPAGE_ADDR		\
						fprintf(stdout,"Zero Page Addressing.\n\r");\
						rOPA = ((unsigned int)instr_byte[1])
#define ZPAGE			ZPAGE_ADDR;		\
						rOP  = mem(rOPA)
  
/** - Zero Page X -
  * Allows the addition of a 8 bit offset to the current X value for access
  * Specified with a numeric 8 bit operand followed by an X ($0C,X)
  * NOTE: The wraparound only occurs on base ($0080+$00FF -> $007F)
  */

#define ZPAGE_X_ADDR		\
						fprintf(stdout,"Zero Page X Addressing.\n\r");\
						rOPA = ((unsigned int)instr_byte[1] + rX)
#define ZPAGE_X			ZPAGE_X_ADDR;			\
						rOP  = mem(rOPA)

/** - Zero Page Y -
  * Allows the addition of a 8 bit offset to the current Y value for access
  * Specified with a numeric 8 bit operand followed by an Y ($0C,Y)
  * NOTE: The wraparound only occurs on base ($0080+$00FF -> $007F)
  */

#define ZPAGE_Y_ADDR	fprintf(stdout,"Zero Page Y Addressing.\n\r");\
						rOPA = ((unsigned int)instr_byte[1] + rY)
#define ZPAGE_Y			ZPAGE_Y_ADDR;				\
						rOP  = mem(rOPA)

/** - Relative -
  * Used for branching by an 8 bit offset
  * The offset is added to an updated Program Counter
  */

#define RELATIVE		\
						fprintf(stdout,"Relative Addressing.\n\r");\
						rOPA = instr_byte[1]

/** - Absolute -
  * A full 16 bit specified address 
  * Indicated by a full 16 bit number
  */

#define ABS_ADDR		\
						fprintf(stdout,"Absolute Addressing.\n\r");\
						rOPA = instr_byte[1]; 						\
						((unsigned char*) &rOPA)[1] = instr_byte[2]
#define ABS 			ABS_ADDR;									\
						rOP = mem(rOPA)

/** - Absolute X -
  * A full 16 bit specified address added to the X register
  * Indicated by a full 16 bit number with a following X
  */

#define ABS_X_ADDR		\
						fprintf(stdout,"Absolute X Addressing.\n\r");\
						rOPA = instr_byte[1];					\
						((unsigned char*) &rOPA)[1] = instr_byte[2]; \
						rOPA = rOPA + rX
#define ABS_X 			ABS_X_ADDR;				\
						rOPA += rX;				\
						rOP = mem(rOPA)

/** - Absolute Y -
  * A full 16 bit specified address added to the Y register
  * Indicated by a full 16 bit number with a following Y
  */
\
#define ABS_Y_ADDR		\
						fprintf(stdout,"Absolute Y Addressing.\n\r");\
						rOPA = instr_byte[1]; 						\
						((unsigned char*) &rOPA)[1] = instr_byte[2]; \
						rOPA = rOPA + rY
#define ABS_Y			ABS_Y_ADDR;			\
						rOPA += rY;			\
						rOP = mem(rOPA)

/** - Indirect - 
  * A full 16 bit address which indicates LSB of a 16 bit address to jump to
  * Indicated as such: ($4000) -- Only used by jmp
  */

#define INDIRECT		\
						fprintf(stdout,"Indirect Addressing.\n\r");	\
						((unsigned char*)&rOPA)[0] = instr_byte[1];	\
						((unsigned char*)&rOPA)[1] = instr_byte[2];	\
						rOP  = mem(rOPA + 1); 						\
						rOPA = mem(rOPA);							\
						((unsigned char*)&rOPA)[1] = rOP

/** - Indexed Indirect -
  * Same as Indirect except we add the X value to the offset
  * Indicated as such: ($40,X) -- $FF max offset
  */

#define IND_X			\
						fprintf(stdout,"Indirect X Addressing.\n\r");				\
						rOP  = instr_byte[1] + rX;									\
						rOPA = mem((unsigned int)rOP);								\
						((unsigned char*)&rOPA)[1] = mem((unsigned int)rOP + 1);	\
						rOP  = mem(rOPA)
#define IND_X_ADDR		\
						fprintf(stdout,"Indirect X Addressing.\n\r");				\
						rOP  = instr_byte[1] + rX;									\
						rOPA = mem((unsigned int)rOP);								\
						((unsigned char*)&rOPA)[1] = mem((unsigned int)rOP + 1)

/** - Indirect Indexed -
  * Same as Indirect except we add the Y value to the resulting address
  * Indicated as such: ($40),Y 
  */

#define IND_Y			\
						fprintf(stdout,"Indirect Y Addressing.\n\r");				\
						rOPA = mem((unsigned int)instr_byte[1]);						 \
						((unsigned char*)&rOPA)[1] = mem((unsigned int)instr_byte[1]+1); \
						rOP  = mem(rOPA + rY) 
#define IND_Y_ADDR		\
						fprintf(stdout,"Indirect Y Addressing.\n\r");				\
						rOPA = mem((unsigned int)instr_byte[1]);						 \
						((unsigned char*)&rOPA)[1] = mem((unsigned int)instr_byte[1]+1); \
						rOPA = rOPA + rY 

/* *********************
   *  STATUS REGISTER  *
   ********************* */

// 6502 Status Bits
#define C_BIT			0 // Clear
#define Z_BIT			1 // Zero
#define I_BIT			2 // Interrupt Disable
#define D_BIT			3 // Decimal mode
#define B_BIT			4 // Break Command
#define V_BIT			6 // Overflow
#define N_BIT			7 // Negative

#define C_MASK			0x01
#define Z_MASK			0x02
#define I_MASK			0x04
#define D_MASK			0x08
#define B_MASK			0x10
#define V_MASK			0x40
#define N_MASK			0x80

#define nC_MASK			0xFE
#define nZ_MASK			0xFD
#define nI_MASK			0xFB
#define nD_MASK			0xF7
#define nB_MASK			0xEF
#define nV_MASK			0xBF
#define nN_MASK			0x7F

// 644 Status Bits (that we care about)
#define C_BIT_644		0 // Clear
#define Z_BIT_644		1 // Zero
#define N_BIT_644		2 // Negative
#define V_BIT_644		3 // Overflow
#define I_BIT_644		7 // Interrupt Enable

#define C_MASK_644		0x01
#define Z_MASK_644		0x02
#define N_MASK_644		0x04
#define V_MASK_644		0x08
#define I_MASK_644		0x80

#define nC_MASK_644		0xFE
#define nZ_MASK_644		0xFD
#define nN_MASK_644		0xFB
#define nV_MASK_644		0xF7
#define nI_MASK_644		0x7F


// To keep all SET commands stable in instruction count,
// set each flag to be changed to zero, then ORing will always work.
// Note: the incoming 'status' is in the order of 644 SREG

#define SET_SBIT(nX,X,mY,Y)	rS &= nX;									\
							rS |= ((((status & mY))>>Y)<<X)

#define SET_CLEAR			SET_SBIT(nC_MASK,C_BIT,C_MASK_644,C_BIT_644)
#define GET_CLEAR			(rS & C_MASK)
#define SET_ZERO			SET_SBIT(nZ_MASK,Z_BIT,Z_MASK_644,Z_BIT_644)

#define SET_INTERRUPT		SET_SBIT(nI_MASK,I_BIT,I_MASK_644,I_BIT_644)

#define SET_DECIMAL			SET_SBIT(nD_MASK,D_BIT,D_MASK_644,D_BIT_644)

#define SET_BREAK			SET_SBIT(nB_MASK,B_BIT,B_MASK_644,B_BIT_644)

#define SET_OVERFLOW		SET_SBIT(nV_MASK,V_BIT,V_MASK_644,V_BIT_644)

#define SET_NEGATIVE		SET_SBIT(nN_MASK,N_BIT,N_MASK_644,N_BIT_644)

// Some common pairings of status flags that are set by instructions.
#define NZ_MASK				0x82
#define nNZ_MASK			0x7D

#define STATUS_SET_NZ		rS &= nNZ_MASK;						\
							rS |= ((status & N_MASK_644)<<5);	\
							rS |= ((status & Z_MASK_644))
						

/* ***********************
   *  6502 Instructions  *
   *********************** */

#define EXECUTE_ORA		fprintf(stdout,"ORA\n\r");\
						rA |= rOP;			\
						status = SREG;		\
						STATUS_SET_NZ

#define EXECUTE_AND		fprintf(stdout,"AND\n\r");\
						rA &= rOP;			\
						status = SREG;		\
						STATUS_SET_NZ

#define EXECUTE_EOR		fprintf(stdout,"EOR\n\r");\
						rA ^= rOP;			\
						status = SREG;		\
						STATUS_SET_NZ

#define EXECUTE_BIT		fprintf(stdout,"BIT\n\r");\
						status = rA & rOP;	\
						rS |= status & 0xC0;\
						SET_ZERO

#define EXE_LD(r)		r = mem(rOPA);		\
						status = SREG;		\
						STATUS_SET_NZ		

#define EXECUTE_LDA		fprintf(stdout,"LDA\n\r");\
						EXE_LD(rA)
#define EXECUTE_LDX		fprintf(stdout,"LDX\n\r");\
						EXE_LD(rX)
#define EXECUTE_LDY		fprintf(stdout,"LDY\n\r");\
						EXE_LD(rY)

#define EXECUTE_STA		fprintf(stdout,"STA\n\r");\
						write_mem(rA,rOPA)
#define EXECUTE_STX		fprintf(stdout,"STX\n\r");\
						write_mem(rX,rOPA)
#define EXECUTE_STY		fprintf(stdout,"STY\n\r");\
						write_mem(rY,rOPA)

#define EXE_T(f,t)		t = f;				\
						status = SREG;		\
						STATUS_SET_NZ

#define EXECUTE_TAX		fprintf(stdout,"TAX\n\r");\
						EXE_T(rA,rX)
#define EXECUTE_TAY		fprintf(stdout,"TAY\n\r");\
						EXE_T(rA,rY)
#define EXECUTE_TXA		fprintf(stdout,"TXA\n\r");\
						EXE_T(rX,rA)
#define EXECUTE_TYA		fprintf(stdout,"TYA\n\r");\
						EXE_T(rY,rA)

#define EXECUTE_TSX		fprintf(stdout,"TSX\n\r");\
						rS = rX
#define EXECUTE_TXS		fprintf(stdout,"TXS\n\r");\
						rX = rS

#define EXE_PUSH(r)		PUSH_STACK(r)

#define EXECUTE_PHA		fprintf(stdout,"PHA\n\r");\
						EXE_PUSH(rA)
#define EXECUTE_PHP		fprintf(stdout,"PHP\n\r");\
						EXE_PUSH(rS)

#define EXECUTE_PLA		rA = POP_STACK;	\
						status = SREG;		\
						STATUS_SET_NZ

#define EXECUTE_PLP		fprintf(stdout,"POP\n\r");\
						POP_STACK(rS);

#define EXECUTE_ADC		fprintf(stdout,"ADC\n\r");\
						rTMP1 = rOP + GET_CLEAR;	\
						rTMP2 = SREG;				\
						rA += rTMP1;				\
						status = SREG;				\
						STATUS_SET_NZ;				\
						rS &= nZ_MASK;				\
						rS |= (status|rTMP2) & C_MASK

#define EXECUTE_SBC		fprintf(stdout,"SBA\n\r");\
						rTMP1 = rOP + GET_CLEAR;	\
						rTMP2 = SREG;				\
						rA -= rTMP1 - 1;			\
						status = SREG;				\
						STATUS_SET_NZ;				\
						rS &= nZ_MASK;				\
						rS |= (status|rTMP2) & C_MASK

#define EXECUTE_CMP		fprintf(stdout,"CMP\n\r");\
						rTMP1 = rA - rOP;			\
						status = SREG;				\
						STATUS_SET_NZ;				\
						status &= nC_MASK;			\
						status |= (rTMP1>0) ? 1 : 0

#define EXECUTE_CPX		fprintf(stdout,"CPX\n\r");\
						rTMP1 = rX - rOP;			\
						status = SREG;				\
						STATUS_SET_NZ;				\
						status &= nC_MASK;			\
						status |= (rTMP1>0) ? 1 : 0

#define EXECUTE_CPY		fprintf(stdout,"CPY\n\r");\
						rTMP1 = rY - rOP;			\
						status = SREG;				\
						STATUS_SET_NZ;				\
						status &= nC_MASK;			\
						status |= (rTMP1>0) ? 1 : 0

#define EXECUTE_INC		fprintf(stdout,"INC\n\r");\
						rOP+=1;						\
						status = SREG;				\
						write_mem(rOP,rOPA);		\
						STATUS_SET_NZ

#define EXECUTE_DEC		fprintf(stdout,"DEC\n\r");\
						rOP-=1;						\
						status = SREG;				\
						write_mem(rOP,rOPA);		\
						STATUS_SET_NZ

#define EXE_INCR(r)		r += 1;						\
						status = SREG;				\
						STATUS_SET_NZ

#define EXECUTE_INX		fprintf(stdout,"INX\n\r");\
						EXE_INCR(rX)
#define EXECUTE_INY		fprintf(stdout,"INY\n\r");\
						EXE_INCR(rY)

#define EXE_DECR(r)		r -= 1;						\
						status = SREG;				\
						STATUS_SET_NZ			

#define EXECUTE_DEX		fprintf(stdout,"DEX\n\r");\
						EXE_DECR(rX)
#define EXECUTE_DEY		fprintf(stdout,"DEY\n\r");\
						EXE_DECR(rY)

// Note: ASL/LSR/ROL/ROR will store in different places based on the addressing mode
#define EXECUTE_ASL		fprintf(stdout,"ASL\n\r");\
						rTMP1 = rOP;				\
						rOP <<= 1;					\
						status = SREG;				\
						STATUS_SET_NZ;				\
						rS &= nC_MASK;				\
						rS |= (rTMP1 & 0x80) ? 1 : 0

#define EXECUTE_LSR		fprintf(stdout,"LSR\n\r");\
						rTMP1 = rOP;				\
						rOP >>= 1;					\
						status = SREG;				\
						STATUS_SET_NZ;				\
						rS &= nC_MASK;				\
						rS |= (rTMP1 & 0x01)

#define EXECUTE_ROL		fprintf(stdout,"ROL\n\r");\
						rTMP1 = rOP;				\
						rOP <<= 1;					\
						rOP |= (rS & 0x01);			\
						status = SREG;				\
						STATUS_SET_NZ;				\
						rS &= nC_MASK;				\
						rS |= (rTMP1 & 0x80) ? 1 : 0

#define EXECUTE_ROR		fprintf(stdout,"ROR\n\r");\
						rTMP1 = rOP;				\
						rOP >>= 1;					\
						rOP |= (rS & 0x01) << 7;	\
						status = SREG;				\
						STATUS_SET_NZ;				\
						rS &= nC_MASK;				\
						rS |= (rTMP1 & 0x01)

#define EXECUTE_JMP		fprintf(stdout,"JMP\n\r");\
						rPC = rOPA

// Assumes that rPC has already been incremented twice (Needs to be next instruction -1) Push: MSB first| Pull: LSB first
#define EXECUTE_JSR		fprintf(stdout,"JSR\n\r");	\
						PUSH_STACK(rPC & 0xFF);		\
						PUSH_STACK(rPC>>8);			\
						rPC = rOPA

#define EXECUTE_RTS		fprintf(stdout,"RTS\n\r");\
						POP_STACK(((unsigned char*)&rPC)[1]);	\
						POP_STACK(((unsigned char*)&rPC)[0]);	\
						rPC+=1

#define EXECUTE_BCC		fprintf(stdout,"BCC\n\r");\
						rPC += (rS & C_MASK) ?	0 : rOP
#define EXECUTE_BCS		fprintf(stdout,"BCS\n\r");\
						rPC += (rS & C_MASK) ?	rOP : 0
#define EXECUTE_BMI 	fprintf(stdout,"BMI\n\r");\
						rPC += (rS & Z_MASK) ?	rOP : 0
#define EXECUTE_BEQ		fprintf(stdout,"BEQ\n\r");\
						rPC += (rS & Z_MASK) ?  0 : rOP
#define EXECUTE_BNE		fprintf(stdout,"BNE\n\r");\
						rPC += (rS & N_MASK) ?	rOP : 0
#define EXECUTE_BPL		fprintf(stdout,"BPL\n\r");\
						rPC += (rS & N_MASK) ?	0 : rOP
#define EXECUTE_BVC		fprintf(stdout,"BVC\n\r");\
						rPC += (rS & V_MASK) ?	0 : rOP
#define EXECUTE_BVS		fprintf(stdout,"BVS\n\r");\
						rPC += (rS & V_MASK) ?	rOP : 0

#define EXECUTE_CLC		fprintf(stdout,"CLC\n\r");\
						rS &= nC_MASK
#define EXECUTE_CLD		fprintf(stdout,"CLD\n\r");\
						rS &= nD_MASK
#define EXECUTE_CLI		fprintf(stdout,"CLI\n\r");\
						rS &= nI_MASK
#define EXECUTE_CLV		fprintf(stdout,"CLV\n\r");\
						rS &= nV_MASK
#define EXECUTE_SEC		fprintf(stdout,"SEC\n\r");\
						rS |= C_MASK
#define EXECUTE_SED		fprintf(stdout,"SED\n\r");\
						rS |= D_MASK
#define EXECUTE_SEI		fprintf(stdout,"SEI\n\r");\
						rS |= I_MASK

#define EXECUTE_BRK		fprintf(stdout,"BRK\n\r");\
						PUSH_STACK((unsigned char)(rPC&0xFF));					\
						PUSH_STACK((unsigned char)(rPC>>8));					\
						PUSH_STACK(rS);											\
						rPC = (mem(0xFFFF) << 8);								\
						rPC |= mem(0xFFFE);										\
						rS |= B_MASK

#define EXECUTE_NOP		fprintf(stdout,"NOP\n\r");\
						asm volatile (		\
							"NOP\n\r"		\
							"NOP\n\r"		\
							"NOP\n\r"		\
							"NOP\n\r"		\
							"NOP\n\r"		\
							"NOP\n\r"		\
							"NOP\n\r"		\
							)

#define EXECUTE_RTI		fprintf(stdout,"RTI\n\r");\
						POP_STACK(rS);							\
						POP_STACK(((unsigned char*)&rPC)[1]);	\
						POP_STACK(((unsigned char*)&rPC)[0])