;TODO, sort out overflow flag
;DEBUG STUFF
.equ ZP_offset = 0x00				;offset for Zero page, set to 0 for non debug
.MACRO cp1

;	clc

	cp		@0,@1


	brcs	PC+3				;invert carry
	sec	
	rjmp	PC+2	
	clc

	in		Pstat1,SREG			;save status bits

.ENDMACRO

.MACRO move_to_reg
					
	mov		r17,Pstat1			;TEST ME
	tst		@0				;all this is need becase the Z and N flags get changed, but dont in a mov operation	
								;add zero should test register
	
	in		r16,SREG

	;N flag
	sbrs	r16,2
	cbr		r17,EXP2(Ns)
	sbrc	r16,2
	sbr		r17,EXP2(Ns)
	;Z flag
	sbrs	r16,1
	cbr		r17,EXP2(Zs)
	sbrc	r16,1
	sbr		r17,EXP2(Zs)

	mov		Pstat1,r17

.ENDMACRO
;-----------01-----------------------------------------------------------
op_ORA:							;OR
	rcall	get_value_01

	out		SREG,Pstat1			;still need to do this to preserve carry
	or		A,retL				;perform OR function
	in		Pstat1,SREG			;save status bits
	rjmp	next_op
op_AND:							;AND
	rcall	get_value_01

	out		SREG,Pstat1	
	and		A,retL
	in		Pstat1,SREG			;save status bits
	rjmp	next_op


op_EOR:							;Exclusive or
	rcall	get_value_01
		
	out		SREG,Pstat1	
	eor		A,retL
	in		Pstat1,SREG			;save status bits
	rjmp	next_op
op_ADC:							;ADD with carry
	rcall	get_value_01
	
	out		SREG,Pstat1			;restor status reg for carry op

	ldi		r17,1

;	in		r16,SREG
;	sbrc	r16,0
;	add		A,r17

	adc		A,retL

	in		r16,SREG

	

	mov		Pstat1,r16			;save status bits

	
	mov		r17,Pstat2

	;sort out O(V) flag
	sbrs	r16,3
	cbr		r17,EXP2(Os)
	sbrc	r16,3
	sbr		r17,EXP2(Os)

	;sbr		r17,EXP2(Os)

	mov		Pstat2,r17			;copy back

	rjmp	next_op

op_STA:							;Store A to mem
	rcall	get_value_01		;Dont acctualy need to get that value, but called to work out address for write
	mov		retL,A
	st_cpuZ	A
	rjmp	next_op

op_LDA:							;Load A with mem
	rcall	get_value_01
	mov		A,retL

	move_to_reg A


	rjmp	next_op

op_CMP:							;Compare A and mem
	rcall	get_value_01
	cp1		A,retL

	rjmp	next_op
op_SBC:							;Subtract A-M with carry
	rcall	get_value_01
	
;	mov		r18,A			;TEST

	out		SREG,Pstat1			;restor status reg for carry op

	brcs	PC+3				;Strange subtraction as works on an inverted carry
	sec	
	rjmp	PC+2	
	clc

;	ldi		r17,1
;---TEST
;	in		r16,SREG
;	sbrc	r16,0
;	add		A,r17
;	SUB		A,retL

	SEZ							;needed because only clears Z if result <> 0!

	sbc		A,retL

	brcs	PC+3				;again..it inverts the carry???
	sec	
	rjmp	PC+2	
	clc

	in		Pstat1,SREG			;save status bits


;	sbc		r18,retL

	in		r16,SREG
	
	mov		r17,Pstat2

	;sort out O(V) flag
	sbrs	r16,3
	cbr		r17,EXP2(Os)
	sbrc	r16,3
	sbr		r17,EXP2(Os)

;	sbr		r17,EXP2(Os)


	mov		Pstat2,r17			;copy back



	rjmp	next_op

;-----------10-----------------------------------------------------------
op_ASL:								;Logcial shift left
	rcall 	get_value_10
	lsl		retL
	in		Pstat1,SREG				;save status bits
	st_cpuZ retL
	rjmp	next_op


op_ROL:								;Left shit with caryy
	rcall 	get_value_10
	out		SREG,Pstat1				;restor status reg for carry op
	rol		retL
	in		Pstat1,SREG				;save status bits
	st_cpuZ retL
	rjmp	next_op

op_LSR:								;Logcial shift right
	rcall 	get_value_10
	lsr		retL
	in		Pstat1,SREG				;save status bits
	st_cpuZ retL
	rjmp	next_op
op_ROR:								;Right shit with caryy
	rcall 	get_value_10
	out		SREG,Pstat1				;restor status reg for carry op
	ror		retL
	in		Pstat1,SREG				;save status bits
	st_cpuZ retL
	rjmp	next_op

op_STX:								;Store X to memory
	rcall 	get_value_10
	mov		retL,Xr
	st_cpuZ retL
	rjmp	next_op

op_LDX:								;Load X with memory
	rcall 	get_value_10
	mov		Xr,retL

	move_to_reg Xr
	rjmp	next_op

op_DEC:								;dec memory
	rcall 	get_value_10
	out		SREG,Pstat1	
	dec		retL
	in		Pstat1,SREG	
	st_cpuZ retL
	rjmp	next_op

op_INC:
	rcall 	get_value_10
	out		SREG,Pstat1	
	inc		retL
	in		Pstat1,SREG	
	st_cpuZ retL
	rjmp	next_op
;-----------00-----------------------------------------------------------
;same as 10 but accum missing
op_BIT:	;This instructions is used to test if one or more bits are set in a target memory location. 
		;The mask pattern in A is ANDed with the value in memory to set or clear the zero flag, 
		;but the result is not kept. Bits 7 and 6 of the value from memory are copied into the N and V flags.
	rcall 	get_value_10
	out		SREG,Pstat1	
	mov		r16,A				;make copy
	and		r16,retL			;perform AND
	in		r16,SREG			;copy SREG for Z flag
	
	;sort out N flag
	sbrs	retL,7
	cbr		r16,EXP2(Ns)
	sbrc	retL,7
	sbr		r16,EXP2(Ns)

	mov		Pstat1,r16			;copy back
	mov		r16,Pstat2

	;sort out O(V) flag
	sbrs	retL,6
	cbr		r16,EXP2(Os)
	sbrc	retL,6
	sbr		r16,EXP2(Os)

	mov		Pstat2,r16			;copy back

	rjmp	next_op

op_JMP:									;Jump absolute
	get_pc_mem r18
	get_pc_mem r17
	
	mov		PL,r18
	mov		PH,r17								
	rjmp	next_op


op_JMPI:								;Jump absolute indirect
	get_pc_mem YL						;get indirect address
	get_pc_mem YH
	ld_cpuY		PL						;load PC with indriect value
	inc			YL						;NOTE, not using Y+ because of CPU bug which does not carry the carry to high byte
	ld_cpuY		PH
	rjmp	next_op

op_STY:									;Store Y to memory						
	rcall 	get_value_10
	mov		retL,Yr
	st_cpuZ retL
	rjmp	next_op
op_LDY:
	rcall 	get_value_10
	mov		Yr,retL

	move_to_reg Yr
	rjmp	next_op
op_CPY:									;Compare Y with memory
	rcall	get_value_10
	cp1		Yr,retL

	rjmp	next_op
op_CPX:									;Compare X with memory
	rcall	get_value_10

	cp1		Xr,retL

	rjmp	next_op
;--BRANCHES---

op_BPL:									;Branch if Positive (N = 0)
	mov		r16,Pstat1
	sbrs	r16,Ns
	rjmp	Branch_take
	adiw	PL,1						;skip over address if no branch
	rjmp	next_op
			
op_BMI:									;Branch if minus (N = 1)
	mov		r16,Pstat1
	sbrc	r16,Ns
	rjmp	Branch_take
	adiw	PL,1				;skip over address if no branch
	rjmp	next_op
op_BVC:							;Branch if (O = 0)
	mov		r16,Pstat2
	sbrs	r16,Os
	rjmp	Branch_take
	adiw	PL,1				;skip over address if no branch
	rjmp	next_op

op_BVS:							;Branch if (O = 1)
	mov		r16,Pstat2
	sbrc	r16,Os
	rjmp	Branch_take
	adiw	PL,1				;skip over address if no branch
	rjmp	next_op
op_BCC:							;Branch is Carry clear
	mov		r16,Pstat1
	sbrs	r16,Cs
	rjmp	Branch_take
	adiw	PL,1				;skip over address if no branch
	rjmp	next_op
op_BCS:
	mov		r16,Pstat1
	sbrc	r16,Cs
	rjmp	Branch_take
	adiw	PL,1				;skip over address if no branch
	rjmp	next_op
op_BNE:							;Branch is (Z=0)
	mov		r16,Pstat1
	sbrs	r16,Zs
	rjmp	Branch_take
	adiw	PL,1				;skip over address if no branch
	rjmp	next_op
op_BEQ:
	mov		r16,Pstat1
	sbrc	r16,Zs
	rjmp	Branch_take
	adiw	PL,1				;skip over address if no branch
	rjmp	next_op

op_BRK:
	;TODO
	ldi		r16,'B'
	rcall	rs232_0_tx
	nop
	nop
	rjmp	next_op
op_JSRA:
	get_pc_mem	r17				;get jump address
	stack_push	PH				;push address to stack, done here because accutaly save PC-1
	stack_push	PL
	get_pc_mem 	r18 			;get high jump address
	
	mov		PL,r17
	mov		PH,r18								
	rjmp	next_op

op_RTI:
	stack_pop	retL
	rcall	stack_decomp
	stack_pop	PL
	stack_pop	PH
	nop	;TODO
	nop
	rjmp	next_op

op_RTS:
	stack_pop	PL				;get return address from stack
	stack_pop	PH

	adiw		PL,1			;add on to PC

	rjmp	next_op


;--------SPECIAL CASES and operations only on the accumulator---------
op_ASL_A:							;logical shift left
	lsl		A

	in		Pstat1,SREG				;save status bits
	rjmp	next_op

op_ROL_A:							;shift left with carry
	out		SREG,Pstat1				;restor status reg for carry op

	rol		A

	in		Pstat1,SREG				;save status bits
	rjmp	next_op


op_LSR_A:
	lsr		A

	in		Pstat1,SREG				;save status bits
	rjmp	next_op
op_ROR_A:
	out		SREG,Pstat1				;restor status reg for carry op

	ror		A

	in		Pstat1,SREG				;save status bits
	rjmp	next_op

op_STX_ZPY:										;Store X to memory
	rcall	get_value_ZP_idx_Y

	mov		retL,Xr
	st_cpuZ retL
	rjmp	next_op

op_LDX_ZPY:
	rcall	get_value_ZP_idx_Y
	mov		Xr,retL

	move_to_reg Xr
	rjmp	next_op

op_LDX_XY:
	rcall	get_value_ZP_abs_idx_Y
	mov		Xr,retL

	move_to_reg Xr
	rjmp	next_op
op_PHP:
	rcall	stack_build
	stack_push retL
	rjmp	next_op

op_PLP:
	stack_pop	retL
	rcall	stack_decomp
	rjmp	next_op
op_PHA:
	stack_push A
	rjmp	next_op
op_PLA:
	stack_pop A
	move_to_reg A
	rjmp	next_op
op_DEY:
	out		SREG,Pstat1				;restor SREG so carry isnt changed
	dec		Yr						;perform dec
	in		Pstat1,SREG				;save changed N and Z
	rjmp	next_op

op_TAY:								;Transfer A to Y

	mov		Yr,A				;

	move_to_reg Yr
	rjmp	next_op

op_INY:
	out		SREG,Pstat1	
	inc		Yr
	in		Pstat1,SREG	
	rjmp	next_op

op_INX:
	out		SREG,Pstat1	
	inc		Xr
	in		Pstat1,SREG	
	rjmp	next_op

op_CLC:
	mov		r16,Pstat1
	cbr		r16,EXP2(Cs)
	mov		Pstat1,r16
	rjmp	next_op
op_SEC:
	mov		r16,Pstat1
	sbr		r16,EXP2(Cs)
	mov		Pstat1,r16
	rjmp	next_op
op_CLI:
	mov		r16,Pstat2
	cbr		r16,EXP2(Is)
	mov		Pstat2,r16
	rjmp	next_op
op_SEI:
	mov		r16,Pstat2
	sbr		r16,EXP2(Is)
	mov		Pstat2,r16
	rjmp	next_op
op_TYA:							;Transfer Y to A
	mov		A,Yr				;

	move_to_reg A
	rjmp	next_op
op_CLV:
	mov		r16,Pstat2
	cbr		r16,EXP2(Os)
	mov		Pstat2,r16
	rjmp	next_op

op_CLD:
	mov		r16,Pstat2
	cbr		r16,EXP2(Ds)
	mov		Pstat2,r16
	rjmp	next_op
op_SED:
	mov		r16,Pstat2
	sbr		r16,EXP2(Ds)
	mov		Pstat2,r16
	rjmp	next_op
op_TXA:							;TRans X to A
	mov		A,Xr				;

	move_to_reg A
	rjmp	next_op
op_TXS:							;transfer X to stack pointer
	mov		stack,Xr
	rjmp	next_op

	
op_TAX:							;Transfer A to X
	mov		Xr,A				;

	move_to_reg Xr
	rjmp	next_op

op_TSX:
	mov		Xr,stack				;

	move_to_reg Xr
	rjmp	next_op

op_DEX:
	out		SREG,Pstat1				;restor SREG so carry isnt changed
	dec		Xr						;perform dec
	in		Pstat1,SREG				;save changed N and Z
	rjmp	next_op
op_NOP:								;nopys
	nop
	nop
	rjmp	next_op
;---------------------------
;---------------------------
;---------------------------
get_value_01:						;MEMORY type decoder for xxxxxx01 opcodes
	andi	retL,0b00111000			;retL still contains opcode, but lsl by one, get mask of memory access type
									;should be a fatser way of doing this with jump tables
									;for now, order with most frequently used at top

	cpi		retL,0b00000000
	breq	get_value_ZP_inx_X_indirj 	

	cpi		retL,0b00001000
	breq	get_value_ZPj

	cpi		retL,0b00010000
	breq	get_value_imedj

	cpi		retL,0b00011000
	breq	get_value_absj

	cpi		retL,0b00100000
	breq	get_value_ZP_indir_idx_Yj

	cpi		retL,0b00101000
	breq	get_value_ZP_idx_Xj


	cpi		retL,0b00110000
	breq	get_value_ZP_abs_idx_Yj

	cpi		retL,0b00111000
	breq	get_value_ZP_abs_idx_Xj
;---------------------------------
get_value_10:						;MEMORY type decoder for xxxxxx10 opcodes
	andi	retL,0b00111000			
								
	cpi		retL,0b00000000
	breq	get_value_imedj
	
	cpi		retL,0b00001000
	breq	get_value_ZPj

	cpi		retL,0b00010000
	breq	get_value_accumj

	cpi		retL,0b00011000
	breq	get_value_absj

	cpi		retL,0b00101000
	breq	get_value_ZP_idx_Xj

	cpi		retL,0b00111000
	breq	get_value_ZP_abs_idx_Xj

get_value_ZP_inx_X_indirj:
	rjmp	get_value_ZP_inx_X_indir
get_value_ZPj:
	rjmp	get_value_ZP
get_value_imedj:
	rjmp	get_value_imed
get_value_absj:
	rjmp	get_value_abs
get_value_ZP_indir_idx_Yj:
	rjmp	get_value_ZP_indir_idx_Y
get_value_ZP_idx_Xj:
	rjmp	get_value_ZP_idx_X
get_value_ZP_idx_Yj:
	rjmp	get_value_ZP_idx_Y
get_value_ZP_abs_idx_Yj:
	rjmp	get_value_ZP_abs_idx_Y
get_value_ZP_abs_idx_Xj:
	rjmp	get_value_ZP_abs_idx_X
get_value_accumj:
	rjmp	get_value_accum

;----------------------------
get_value_ZP_inx_X_indir:			;Zero page index X, indirect

	ldi		YH,ZP_offset					;Zero page, so high byte 0
	get_pc_mem YL					;get next byte into YL
	add		YL,Xr					
	adc		YH,zero_reg

	ld_cpuYp ZL						;now load Z with indrected address Y
	ld_cpuY	 ZH

	ld_cpuZ	retL					;load actual value!
	ret


get_value_ZP:						;Zero Page
	
	ldi		ZH,ZP_offset			;Zero page..
	get_pc_mem ZL					;Get pos in ZP

	ld_cpuZ	retL					;load value
	ret


get_value_imed:						;immediate mode
	get_pc_mem retL
	ret

get_value_abs:						;get absolute (one byte)
	get_pc_mem ZL
	get_pc_mem ZH
	
	ld_cpuZ	retL
	ret

get_value_ZP_indir_idx_Y:			;Zero page indirect, indexed Y
	ldi		YH,ZP_offset
	get_pc_mem YL

	ld_cpuYp ZL						;now load Z with indrected address Y
	ld_cpuY	 ZH

	add		ZL,Yr					;add Y reg for indexing
	adc		ZH,zero_reg				;no wrap around on this one

	ld_cpuZ	retL					;load value
	ret

get_value_ZP_idx_X:					;Zero page indexd X
	ldi		ZH,ZP_offset
	get_pc_mem ZL

	add		ZL,Xr					;add index, 
	adc		ZH,zero_reg

	ld_cpuZ	retL					;get value
	ret


get_value_ZP_idx_Y:					;Zero page indexd Y (only used with LDX and STX)
	ldi		ZH,ZP_offset
	get_pc_mem ZL

	add		ZL,Yr					;add index, with wrap around..humm TODO..?
	adc		ZH,zero_reg

	ld_cpuZ	retL					;get value
	ret

get_value_ZP_abs_idx_Y:				;absolute, indexd with Y
	get_pc_mem ZL					;get absolut value
	get_pc_mem ZH

	add		ZL,Yr
	adc		ZH,zero_reg
	
	ld_cpuZ	retL
	ret
	
get_value_ZP_abs_idx_X:				;absolute, indexd with X
	get_pc_mem ZL					;get absolut value
	get_pc_mem ZH

	add		ZL,Xr
	adc		ZH,zero_reg
	
	ld_cpuZ	retL
	ret

get_value_accum:					;accumulator,	Should not use this, there should be special cases for A ops

	mov		retL,A
	ret



;----------------------------------------------------------------------
;----------------------------------------------------------------------
;Branch_take
	
;purpose: 	if a branch is sucessfull
;Takes: 	
;Give:		
;----------------------------------------------------------------------
;----------------------------------------------------------------------


Branch_take:
	get_pc_mem r18					;get branch modifiyer

	sbrc	r18,7					;gotta go signed 8 bit X 16 bit unsigned
	ldi		r17,0xff				;todo FIX, DONT USE R16
	sbrs	r18,7
	ldi		r17,0
	
	add		PL,r18
	adc		PH,r17
	rjmp	next_op


;----------------------------------------------------------------------
;----------------------------------------------------------------------
;stack_build TODO: make a macro
	
;purpose: 	Here teh stack is stored as 2 bytes for speed, in order to store on the stack
			;it must be built into one byte
;Takes: 	
;Give:		retL as built stack
;----------------------------------------------------------------------
;----------------------------------------------------------------------
stack_build:

	mov		r16,Pstat1					;copy byte for bottom 3 bits (C,Z,N)
	andi	r16,0b00000111				;clear top bits
	mov		r17,Pstat2					
	andi	r17,0b11111000
	or		r16,r17
	mov		retL,r16
	ret


;----------------------------------------------------------------------
;----------------------------------------------------------------------
;stack_decomp TODO: make a macro
	
;purpose: 	reverse of stack_build
			
;Takes: 	retL 
;Give:		
;----------------------------------------------------------------------
;----------------------------------------------------------------------
stack_decomp:
	mov		Pstat1,retL
	mov		Pstat2,retL

	ret
op_INVALID:
	ldi		r16,'V'
	rcall	rs232_0_tx
	rjmp	next_op

p:	rjmp p