JoeyLib IIgs ASM Blitter Work

jIIgs.asm

;----------------------------------------
;  JoeyLib
;  Copyright (C) 2018-2019 Scott Duensing <[email protected]>
;
;  This software is provided 'as-is', without any express or implied
;  warranty.  In no event will the authors be held liable for any damages
;  arising from the use of this software.
;
;  Permission is granted to anyone to use this software for any purpose,
;  including commercial applications, and to alter it and redistribute it
;  freely, subject to the following restrictions:
;
;  1. The origin of this software must not be misrepresented; you must not
;     claim that you wrote the original software. If you use this software
;     in a product, an acknowledgment in the product documentation would be
;     appreciated but is not required.
;  2. Altered source versions must be plainly marked as such, and must not be
;     misrepresented as being the original software.
;  3. This notice may not be removed or altered from any source distribution.
;----------------------------------------


	mcopy 13:ORCAInclude:m16.ORCA
	mcopy jIIgs.macro
	case on
	gen on

GlobalData	data
ScTable	ds	400	; 400 bytes for 200 scanline offsets
StTable	ds	50	; 50 bytes for 25 stencil table offsets
MaTable	ds	256	; 4 bit stencil to mask word table
VblRate	ds	2	; Either 5 or 6 depending on PAL or NTSC.
VblTime	ds	2	; Integer Counter
	end

;----------------------------------------
; Blit an 8x8 block to (almost) anywhere on a surface.
; Optimized by qkumba - thanks!
;----------------------------------------
asmB88	start

source	equ	1	; Source Pixels Offset
target	equ	3	; Target Pixels Offset
t	equ	5	; Temp
xc	equ	7	; X Counter
yc	equ	9	; Y Counter

	jsubroutine (4:surface,4:tiles,2:sx,2:sy,2:tx,2:ty),10
	using GlobalData

; Find offset into tile memory
	lda	sy	; Load sy to get index into scanline table
	asl	a	; Multiply by two to get offset into scanline table (two bytes per entry in table)
	tay
; Divide source horizontal position by two since there are two pixels per byte
	lda	sx	; Load X source position into accumulator
	lsr	a
	clc
	adc	ScTable,y	; Add to byte offset of y position from table
	sta	source	; Offset to start of source pixels

; Find offset into target surface memory
	lda	ty	; Load ty to get index into scanline table
	asl	a	; Multiply by two to get offset into scanline table (two bytes per entry in table)
	tay

; Divide target horizontal position by two since there are two pixels per byte
	lda	tx	; Load X target position into accumulator
	lsr	a	; Shift Right - divide by 2

	clc
	adc	ScTable,y	; Add x position to byte offset of y position from table
	sta	target	; Offset to start of target pixels

	stz	xc	; Load 0 into X, 8 into Y counters
	lda	#8
	sta	yc

blitTop	ldy	source	; Load Y register with source pixel offset
	lda	[tiles],y	; Load 4 tile pixels
	tyx
	ldy	target	; Load Y register with target pixel offset
	sta	[surface],y	; Store 4 pixels into screen

	iny		; Increment to next pixel target quad
	iny
	sty	target
	inx		; Increment to next pixel source quad
	inx
	stx	source

	lda	xc	; Increment X counter
	inc	a
	and	#1	; count to 2 and auto-reset
	sta	xc
;;cmp	#2	; End of X pixels?
	bne	blitTop	; Nope!

	clc		; Increment target offset
	tya
	adc	#156
	sta	target
	clc		; Increment source offset
	txa
	adc	#156
	sta	source
	dec	yc	; Decrement Y counter
;;cmp	#0	; End of Y pixels?
	bne	blitTop	; Nope!

	jreturn
	end


;----------------------------------------
; Blit an 8x8 block with stencil to (almost) anywhere on a surface.
; (Thanks dwsJason!)
;----------------------------------------
asmB88a start

source	equ 1	; Source Pixels Offset
target	equ 3	; Target Pixels Offset
xc	equ 5	; X Counter
yc	equ 7	; Y Counter
mask0123	equ 9
stoffset	equ 11
tile_pix	equ 13

	jsubroutine (4:surface,4:tiles,4:stencil,2:sx,2:sy,2:tx,2:ty),16

	using GlobalData
; Find offset into tile memory
; Divide source horizontal position by two since there are two pixels per byte
	lda	sx	; Load X source position into accumulator
	lsr	a	; Shift Right - divide by 2
	tax		; save in temp

; c=? but does not matter
	lda	sy	; Load sy to get index into scanline table
	asl	a	; Multiply by two to get offset into scanline table (two bytes per entry in table)
	tay
	txa
	adc	ScTable,y	; Load byte offset of y position from table
; c=0 from the asl above here
	sta	source	; Offset to start of source pixels; Find offset into target surface memory

; Divide target horizontal position by two since there are two pixels per byte
	lsr	tx

	lda	ty	; Load ty to get index into scanline table
	asl	a	; Multiply by two to get offset into scanline table (two bytes per entry in table)
	tay
	lda	ScTable,y	; Load byte offset of y position from table
; c=0 from the asl above
	adc	tx	; Add x position to scanline offset
	sta	target	; Offset to start of target pixels; Find the upper left corner of the stencil

	lda	sy
	asl	a
	tay
	lda	ScTable,y   ; look up in the pixel scanline table
	lsr	a
	lsr	a	   		; we're 4 times smaller, so just divide it
	sta	stoffset

	lda	#0	; Load 0 into X and Y counters
	sta	xc
	sta	yc

blitTop anop
; build the mask
	lda	#$FFFF
	sta	mask0123   ; default all 4 pixels visible

	lda	sx     	   ; add in stencil X position
	lsr	a
	lsr	a
	lsr 	a	   		; div by 8 to get to byte index
	clc
	adc	stoffset
	tay

	lda	sx
	and	#7
; A = number of initial bits to shift out
; to get to the mask
    	tax			   ; save in X

	lda [stencil],y ; load the stencil

	xba             ; since we divided by 8, need to get our bits up high
skipbit anop 		; burn bits we don't care about
	asl	a
	dex
	bpl	skipbit

	tax
	bcc	mask0   	; bit clear, leave mask alone
	lda	#$00F0  	; bit set, set the mask bits
	trb	mask0123
mask0 anop
	txa
	asl	a
	tax
	bcc	mask1       ; bit clear, leave mask alone
	lda	#$000F      ; bit set, set the mask bits
	trb	mask0123
mask1	anop
	txa
	asl	a
	tax
	bcc	mask2       ; bit clear, leave mask alone
	lda	#$F000      ; bit set, set the mask bits
	trb	mask0123
mask2 anop
	txa
	asl	a
	bcc	mask3       ; bit clear, leave mask alone
	lda	#$0F00      ; bit set, set the mask bits
	trb	mask0123
mask3 anop
; c=?
	clc		; c=0
	lda	sx
	adc	#4 	; Add 4 here because we're moving left to right
	sta	sx

	ldy	source	; Load Y register with source pixel offset
	lda	[tiles],y	; Load 4 tile pixels
	sta	tile_pix

	ldy	target	; Load Y register with target pixel offset
	lda	[surface],y
 	and	mask0123
	ora	tile_pix

	sta	[surface],y	; Store 4 pixels into screen
;clc		; Increment to next pixel target quad
	lda	#2
	adc	target
	sta	target
; c=0 here as long as target didn't go over $FFFF
;clc		; Increment to next pixel source quad
	lda	#2
	adc	source
	sta	source	;clc  ; c=0 here as long as source didn't go above $FFFF
	lda	xc	; Increment X counter
	adc	#1
	sta	xc
	cmp	#2	; End of X pixels?
	bcc	blitTop	; Nope!	lda	#0	; Reset X counter

	stz	xc	; reset xcounter to 0
; c = 1
; reset stencil X
	lda	sx
	sbc	#8
	sta	sx

; c=0
	clc
	lda	stoffset   ;Increment Stencil offset
	adc	#40     ; 160 / 4
	sta	stoffset	; c=0		; Increment target offset

	lda	#156    ; c=1, so instead of doing clc, adc 156, just do adc 155
	adc	target
	sta	target
;clc		; Increment source offset
; c=0 unless target goes above $FFFF
	lda	#156
	adc	source
	sta	source
; clc   c=0 unless source goes above $FFFF
	lda	yc	; Increment Y counter
	adc	#1
	sta	yc
	cmp	#8	; End of Y pixels?
	bcs	maskend	; Yep!
	jmp	blitTop
maskend	anop
	jreturn
	end


;----------------------------------------
; Set Border Color
;----------------------------------------
asmBorder	start
	jsubroutine (2:color)
	short	m
	lda	>$E0C034
	and	#$F0
	ora	color
	sta	>$E0C034
	long	m
	jreturn
	end


;----------------------------------------
; Draw a block of tiles from a region of tile data
;
; ***FIX*** This no longer does what we want since
; blitting source addresses are now in pixels.  Fix!
;----------------------------------------
asmDrawBM	start

tileX	equ	1
tileY	equ	3
offset	equ	5
loopX	equ	7
loopY	equ	9

	jsubroutine (4:surface,2:startX,2:startY,2:width,2:height,4:mapData,2:stride,4:tiles),10

	lda	#0	; Zero some starting values
	sta	offset

	clc		; Find loop ending values
	lda	width	; Load width of map to draw, in tiles
	asl	a	; Multiply by 8 to get pixels
	asl	a
	asl	a
	adc	startX	; Add starting position in pixels
	sta	loopX	; Store it
	clc
	lda	height	; Load height of map to draw, in tiles
	asl	a	; Multiply by 8 to get pixels
	asl	a
	asl	a
	adc	startY	; Add starting position in pixels
	sta	loopY	; Store it

	ldx	startX	; Initialize loop counters
	ldy	startY

drawTop	phy		; Keep Y for later
	ldy	offset	; Y is now offset into map data
	lda	[mapData],y
	and	#$00ff	; We only want 8 bits
	sta	tileX
	iny
	lda	[mapData],y
	and	#$00ff	; We only want 8 bits
	sta	tileY
	iny
	sty	offset
	ply		; Y is our loop counter again

	phx		; Protect from clobbering
	phy

	phy		; Push parameters in reverse for asmB88
	phx
	ph2	tileY
	ph2	tileX
	ph4	tiles
	ph4	surface
	jsl	asmB88	; Call tile drawing code

	ply		; Unclobber
	plx

	txa		; Increment x loop
	adc	#8	; Increment in pixels
	tax
	cpx	loopX	; Loop until 'width'
	bcc	drawTop

	ldx	startX	; Reset x loop
	clc
	lda	offset	; Add stride to offset
	adc	stride
	sta	offset
	tya		; Increment y loop
	adc	#8	; Increment in pixels
	tay
	cpy	loopY	; Loop until 'height'
	bcc	drawTop

	jreturn
	end


;----------------------------------------
; Arc3D line-drawing code
; By Andy McFadden
; Adapted from code by the FTA.
;
; Draws from (clpx0,clpy0) to (clpx1,clpy1) (output of a clipping routine).
;----------------------------------------
asmDrawLine	start

x0	equ	1
y0	equ	3
x1	equ	5
y1	equ	7
offset	equ	9
deltax	equ	11
deltay	equ	13
diff	equ	15
even_c	equ	17
odd_c	equ	19

	jsubroutine (4:surface,2:penColor,2:clpx0,2:clpy0,2:clpx1,2:clpy1),20

;			Say surface = $012000...

	lda	surface	; Should be $2000
	sta	__e0	; set "even" addresses
	sta	__e1
	sta	__e2
	sta	__e3
	sta	__e4
	sta	__e5
	sta	__e6
	sta	__e7
	sta	__e8
	sta	__e9
	sta	__e10
	sta	__e11
	sta	__e12
	sta	__e13

	dea		; Should be $1fff
	sta	__o0	; set "odd" addresses
	sta	__o1
	sta	__o2
	sta	__o3
	sta	__o4
	sta	__o5
	sta	__o6
	sta	__o7
	sta	__o8
	sta	__o9
	sta	__o10
	sta	__o11
	sta	__o12
	sta	__o13

	phb		; keep original bank
	lda	surface+1	; Should be $0120
	pha		; switch to bank we're pointing into
	plb		; B is now $20 - bad!
	plb		; B is now $01 - good!

DoDrawLine	anop
	lda	<clpy1	; 4 is y1 < y0?
	cmp	<clpy0	; 4
	blt	in_order	; 2/3 yes, so leave alone

;	lda	<clpy1	; we want y0 to be the largest,
	sta	y0	; so switch
	lda	<clpx1
	sta	x0
	lda	<clpy0
	sta	y1
	lda	<clpx0
	sta	x1
	bra	copy_done

in_order	anop
;	lda	<clpy1
	sta	y1
	lda	<clpx1
	sta	x1
	lda	<clpy0
	sta	y0
	lda	<clpx0
	sta	x0
; 39/41 cycles to here

; setup offset and deltas
copy_done	anop
	lda	y1
	asl	A
	asl	A
	adc	y1
	asl	A
	asl	A
	asl	A
	asl	A
	asl	A
	asl	A
	adc	x1
	sta	offset	; 16+16=32

; penColor must be between 0 and 15
	lda	<penColor	; setup pixel images
	sta	even_c	; 0x000f
	xba
	asl	A
	asl	A
	asl	A
	asl	A
	and	#$f000
	sta	odd_c	; 0xf000

	lda	y0
	sec
	sbc	y1
	sta	deltay	; we know y0 is larger...
	beq	Horizontal	; +2 do as special case

	lda	x1
	cmp	x0
	beq	Vertical	; +2 do as special case
;	bcc	Rev	; x1 is smaller; go other way
	bcs	Forw
	brl	Rev
Forw	sbc	x0
	sta	deltax

	cmp	deltay
	bcc	Forw_bigy_j
	brl	Forw_bigx
Forw_bigy_j	anop
	brl	Forw_bigy

;
; Handle special cases (horizontal/vertical/point)
;

; handle vertical (deltax=0) lines
Vertical	lda	deltay
	tay		; count down

	lda	offset
	lsr	A
	tax
	bcc	vf_odd2
	clc
;	bra	vf_even2

; loop below
vf_even2	lda	$dead,x
__e0	equ	*-2
	and	#$fff0
	ora	even_c
	sta	$dead,x
__e1	equ	*-2

	txa
	adc	#160
	tax
	dey
	bpl	vf_even2
	bra	vf_done

vf_odd2	lda	$beef,x
__o0	equ	*-2
	and	#$0fff
	ora	odd_c
	sta	$beef,x
__o1	equ	*-2

	txa
	adc	#160
	tax
	dey
	bpl	vf_odd2
;	bra	vf_done

vf_done	brl	Exit

; handle horizontal (deltay=0) lines
Horizontal	anop
	lda	x1
	cmp	x0
	bcc	h_rev	; x1 is smaller; go other way
	sbc	x0
	sta	deltax

; horizontal, moving forward
h_forw	lda	deltax
	tay		; count down

	lda	offset
	lsr	A
	tax
	bcc	hf_odd2
	clc
;	bra	hf_even2

; loop below
hf_even2	lda	$dead,x
__e2	equ	*-2
	and	#$fff0
	ora	even_c
	sta	$dead,x
__e3	equ	*-2

	dey
	bmi	hf_done

hf_odd2	lda	$beef,x
__o2	equ	*-2
	and	#$0fff
	ora	odd_c
	sta	$beef,x
__o3	equ	*-2

	dex
	dey
	bpl	hf_even2

hf_done	brl	Exit

; horizontal, reverse direction
h_rev	sec
	lda	x0
	sbc	x1
	sta	deltax

	lda	deltax
	tay		; count down

	lda	offset
	lsr	A
	tax
	bcc	hr_odd2
	clc
;	bra	hr_even2

; loop below
hr_even2	lda	$dead,x
__e4	equ	*-2
	and	#$fff0
	ora	even_c
	sta	$dead,x
__e5	equ	*-2

	dey
	bmi	hr_done
	inx

hr_odd2	lda	$beef,x
__o4	equ	*-2
	and	#$0fff
	ora	odd_c
	sta	$beef,x
__o5	equ	*-2

	dey
	bpl	hr_even2
;	bra	hr_done

hr_done	brl	Exit


;
; Standard cases
;

; forward direction, deltay is bigger than deltax
Forw_bigy	anop
	lda	deltay
	tay		; count down
	lsr	A
	eor	#$ffff
	inc	A
	sta	diff

	lda	offset
	lsr	A
	tax
	bcc	fy_odd2
	clc
	bra	fy_even2

; loop below
fy_even1	sta	diff
fy_even2	lda	$dead,x
__e6	equ	*-2
	and	#$fff0
	ora	even_c
	sta	$dead,x
__e7	equ	*-2

	dey
	bmi	fy_done
	txa
	adc	#160
	tax
	lda	deltax
	adc	diff
	bmi	fy_even1
	sbc	deltay
fy_odd1	sta	diff
fy_odd2	lda	$beef,x
__o6	equ	*-2
	and	#$0fff
	ora	odd_c
	sta	$beef,x
__o7	equ	*-2

	dey
	bmi	fy_done
	txa
	adc	#160
	tax
	lda	deltax
	adc	diff
	bmi	fy_odd1
	sbc	deltay
	dex
	bra	fy_even1

fy_done	brl	Exit

; reverse direction
Rev	sec
	lda	x0
	sbc	x1
	sta	deltax
	cmp	deltay
	bcs	Rev_bigx

; reverse direction, deltay is bigger than deltax
Rev_bigy	lda	deltay
	tay		; count down
	inc	A
	lsr	A
	eor	#$ffff
	inc	A
	sta	diff

	lda	offset
	lsr	A
	tax
	bcc	ry_odd2
	clc
	bra	ry_even2

; loop below
ry_even1	sta	diff
ry_even2	lda	$dead,x
__e8	equ	*-2
	and	#$fff0
	ora	even_c
	sta	$dead,x
__e9	equ	*-2

	dey
	bmi	ry_done
	txa
	adc	#160
	tax
	lda	deltax
	adc	diff
	bmi	ry_even1
	sbc	deltay
	inx
ry_odd1	sta	diff
ry_odd2	lda	$beef,x
__o8	equ	*-2
	and	#$0fff
	ora	odd_c
	sta	$beef,x
__o9	equ	*-2

	dey
	bmi	ry_done
	txa
	adc	#160
	tax
	lda	deltax
	adc	diff
	bmi	ry_odd1
	sbc	deltay
	bra	ry_even1

ry_done	brl	Exit

; reverse direction, deltax is bigger than deltay
Rev_bigx	lda	deltax
	tay		; count down
	lsr	A
	eor	#$ffff
	inc	A
	sta	diff

	lda	offset
	lsr	A
	tax
	bcc	rx_odd2
	clc
	bra	rx_even2

; loop below
rx_even1	sta	diff
rx_even2	lda	$dead,x
__e10	equ	*-2
	and	#$fff0
	ora	even_c
	sta	$dead,x
__e11	equ	*-2

	dey
	bmi	rx_done
	inx
	lda	deltay
	adc	diff
	bmi	rx_odd1
	sbc	deltax
	sta	diff

	txa
	adc	#160
	tax
	bra	rx_odd2

rx_odd1	sta	diff
rx_odd2	lda	$beef,x
__o10	equ	*-2
	and	#$0fff
	ora	odd_c
	sta	$beef,x
__o11	equ	*-2

	dey
	bmi	rx_done
	lda	deltay
	adc	diff
	bmi	rx_even1
	sbc	deltax
	sta	diff
	txa
	adc	#160
	tax
	bra	rx_even2

rx_done	brl	Exit

; forward direction, deltax is bigger than deltay
Forw_bigx	anop
	lda	deltax
	tay		; count down
	lsr	A
	eor	#$ffff
	inc	A
	sta	diff

	lda	offset
	lsr	A
	tax
	bcc	fx_odd2
	clc
	bra	fx_even2

; loop below
fx_even1	sta	diff
fx_even2	lda	$dead,x
__e12	equ	*-2
	and	#$fff0
	ora	even_c
	sta	$dead,x
__e13	equ	*-2

	dey
	bmi	fx_done
	lda	deltay
	adc	diff
	bmi	fx_odd1
	sbc	deltax
	sta	diff

	txa
	adc	#160
	tax
	bra	fx_odd2

fx_odd1	sta	diff
fx_odd2	lda	$beef,x
__o12	equ	*-2
	and	#$0fff
	ora	odd_c
	sta	$beef,x
__o13	equ	*-2

	dey
	bmi	fx_done
	dex
	lda	deltay
	adc	diff
	bmi	fx_even1
	sbc	deltax
	sta	diff
	txa
	adc	#160
	tax
	bra	fx_even2

fx_done	brl	Exit

;
; common exit point
;
Exit	plb		; return to original bank
	jreturn
	end


;----------------------------------------
; Returns the color of a given point
;
; Psychotically commented to help learn ASM  :-)
;----------------------------------------
asmGetPoint	start

temp1	equ	1

	jsubroutine (4:surface,2:Xp,2:Yp),2
	using GlobalData

	lda	Yp	; Load accumulator with Y location
	asl	a	; Shift accumulator left (multiply by 2) for word offset into table
	tay		; Transfer accumulator to y register
	lda	ScTable,y	; Look up scan line address offset
	sta	temp1	; Store accumulator (row offset into SHR memory) into temp1
	clc		; Clear carry flag
	lda	Xp	; Place X position in accumulator
	lsr	a	; Logical Shift Right (divide by 2, two pixels per byte) - bit shifted off the end goes into carry flag
	php		; Save our carry flag for later!
	clc		; Clear carry
	adc	temp1	; Add X position to SHR address with carry - a now contains address of color pair we want
	plp		; Restore our carry flag
	tay		; Transfer accumulator to y register
	lda	[surface],y	; Load pixel pair into accumulator
	bcc	GEven	; Branch if Carry Clear - Determines Even/Odd pixel
	and	#$F	; AND accumulator with $F to mask off unwanted pixel
	bra	GBoth

GEven	and	#$F0	; AND accumulator with $F0 to mask off unwanted pixel
	lsr	a	; Shift the result down a nibble
	lsr	a
	lsr	a
	lsr	a

GBoth	sta	temp1	; Store accumulator in temp1

	jreturn 2:temp1
	end


;----------------------------------------
; Return the current 1/10th second timer value
;----------------------------------------
asmGetVbl	start
	using GlobalData
	lda	>VblTime
	rtl
	end


;----------------------------------------
; Turns off SHR Graphics
;----------------------------------------
asmGrOff	start
	jsubroutine
	short	m
	lda	>$E1C029
	and	#$7F
	sta	>$E1C029
	long	m
	jreturn
	end


;----------------------------------------
; Turns on SHR Graphics
;----------------------------------------
asmGrOn	start
	jsubroutine
	short	m
	lda	>$E1C029
	ora	#$C1
	sta	>$E1C029
	long	m
	jreturn
	end


;----------------------------------------
; Reads controller axis
; Thank you to John Brooks for this code!
;----------------------------------------
asmJoy	start

pret0	equ	1
pret1	equ	2

speed	equ	$00C036
pdl0	equ	$00C064
pdl1	equ	$00C065
ptrig	equ	$00C070

	jsubroutine ,2

	php		; Save mx reg size bits
	phb		; Save data bank
	short	m,i	; 8-bit m,x
	sei		; disable interrupts

	ldx	#0	; Zero paddle 0 counter
	phx
	plb		; Set DBR = 0

	ldy	speed	; Read current system speed
	phy		; Save it
	lda	#$80	; Fast/Slow bit
	trb	speed	; Force 1 Mhz

	txy		; Zero paddle 1 counter

	pha		; Dummy data for wait pla
	lda	ptrig	; Start paddle timers. Wait 11 cycles then read pdl0
	pla		; 4c

gotpdl1	xba		; 3c
chkpdl0	lda	pdl0	; 4c. Read pdl0. 10c until pdl1 read
	bpl	gotpdl0	; 2/3c taken if pdl0 is done
	inx		; 2c
	inx		; 2c
	lda	pdl1	; 4c. Read pdl1. 12c until pdl0 read
	bmi	nogots	; 2/3c
	bpl	gotpdl1	; 3c always taken
nogots	iny		; 2c
	jmp	chkpdl0	; 3c
gotpdl0	xba		; 3c
	lda	pdl1	; 4c. Read pdl1. 12c until pdl0 read
	bmi	nogots	; 3c if pdl1 not done

	pla		; Reload original speed
	sta	speed	; Restore it

	plb		; Restore Data Bank

	stx	pret0	; Store result
	tya
	asl	a	; Scale 0-127 pdl1 to 0-254
	sta	pret1

	plp		; Restore mx 8/16 mode and interrupt enable state
	long	m,i

	jreturn 2:pret0
	end


;----------------------------------------
; Swaps nibble values
;
; Psychotically commented to help learn ASM  :-)
;----------------------------------------
asmNSwap	start

hnibo	equ	1	; High nibble old color value
hnibn	equ	2	; High nibble new color value

	jsubroutine (4:pointer,2:count,2:oldColor,2:newColor),2

	short	m	; Use 8 bit accumulator
	lda	newColor	; Load new color to use
	asl	a	; Shift it up a nibble
	asl	a
	asl	a
	asl	a
	sta	hnibn	; Store it for later
	lda	oldColor	; Load old color to use (16 bit argument)
	asl	a	; Shift it up a nibble
	asl	a
	asl	a
	asl	a
	sta	hnibo	; Store it for later

	ldy	#0	; Load Y (our counter) with 0

NSLow	lda	[pointer],y	; Get byte to check
	and	#$0F	; Isolate low nibble
	cmp	oldColor	; Compare to old value
	bne	NSHigh	; Not equal, skip to high nibble
	lda	[pointer],y	; Get byte to modify
	and	#$F0	; Clear low nibble
	ora	newColor	; Set low nibble to new color
	sta	[pointer],y	; Store it back

NSHigh	lda	[pointer],y	; Get byte to check
	and	#$F0	; Isolate high nibble
	cmp	hnibo	; Compare to old color
	bne	NSEnd	; Not equal, skip to next byte
	lda	[pointer],y	; Get byte to modify
	and	#$0F	; Clear high nibble
	ora	hnibn	; Set high nibble to new color
	sta	[pointer],y	; Store it back

NSEnd	iny		; Increment Y
	clc		; Clear carry
	cpy	count	; Compare Y to number of bytes to modify
	bcc	NSLow	; Keep going

	long	m	; Back to 16 bit accumulator
	jreturn
	end


;----------------------------------------
; Plots a point in a given color
;
; Psychotically commented to help learn ASM  :-)
;----------------------------------------
asmPoint	start

temp1	equ	1
temp2	equ	3
temp3	equ	5

	jsubroutine (4:surface,2:color,2:Xp,2:Yp),6
	using GlobalData

	lda	Xp	; Place X position in accumulator
	lsr	a	; Logical Shift Right (divide by 2) - bit shifted off the end goes into carry flag
	sta	temp1	; Store accumulator in temp1
	bcc	PEven	; Branch if Carry Clear - Determines Even/Odd pixel
POdd	lda	#$FFF0	; Load accumulator with pixel mask
	sta	temp2	; Store accumulator in temp2
	lda	color	; Load accumulator with color
	and	#$F	; AND accumulator with $F
	bra	PBoth	; Branch Always to PBoth

PEven	lda	#$FF0F	; Load accumulator with pixel mask
	sta	temp2	; Store accumulator in temp2
	lda	color	; Load accumulator with color
	and	#$F0	; AND accumulator with $F0

PBoth	sta	temp3	; Store accumulator in temp3
	lda	Yp	; Load accumulator with Y location
	asl	a	; Shift accumulator left (multiply by 2)
	tay		; Transfer accumulator to y register
	lda	ScTable,y	; Look up scan line address offset
	adc	temp1	; Add X position to address with carry
	tay		; Transfer accumulator to y

	lda	[surface],y	; Load existing pixels into accumulator
	and	temp2	; AND accumulator with temp2
	ora	temp3	; OR accumulator with temp3
	sta	[surface],y	; Store updated pixels back into memory

	jreturn
	end


;----------------------------------------
; "PEI Slam" shadowed SHR display to the
; actual display.  Copies all 200 lines,
; the palette, and SCBs.
;
; Basically stolen from https://www.yumpu.com/en/document/view/23761498/code-secrets-of-wolfenstein-3d-iigs-kansasfest
;----------------------------------------
asmSlam	start

	jsubroutine

	short	m	; Turn Shadowing On
	lda	>$E0C035
	and	#$F7
	sta	>$E0C035
	long	m

	php		; Push Processor Status Register
	lda	>$E0C068-1
	pha		; Save main/aux bank state
	phd		; Save DPage
	tsc		; Transfer Stack Pointer to C Accumulator
	sta	>StackPtr	; Store Accumulator in StackPtr

	sei		; Disable interrupts

	lda	#$3000	; Read/write aux 48k (SHR) via bank 0
	sta	>$E0C068-1

	ldy	#$2000	; Point to start of SHR

peiChunk	ldx	#7	; Chunks before interrupts

peiPage	tya
	tcd		; Move DP

	clc
	adc	#$FF	; Point to top of page
	tcs		; Move stack

	inc	a
	tay		; Keep + 0x100 for later

	pei	$FE	; "Slam" a single page
	pei	$FC
	pei	$FA
	pei	$F8
	pei	$F6
	pei	$F4
	pei	$F2
	pei	$F0
	pei	$EE
	pei	$EC
	pei	$EA
	pei	$E8
	pei	$E6
	nop
	pei	$E4
	pei	$E2
	pei	$E0
	pei	$DE
	pei	$DC
	pei	$DA
	pei	$D8
	pei	$D6
	pei	$D4
	pei	$D2
	pei	$D0
	pei	$CE
	pei	$CC
	nop
	pei	$CA
	pei	$C8
	pei	$C6
	pei	$C4
	pei	$C2
	pei	$C0
	pei	$BE
	pei	$BC
	pei	$BA
	pei	$B8
	pei	$B6
	pei	$B4
	pei	$B2
	nop
	pei	$B0
	pei	$AE
	pei	$AC
	pei	$AA
	pei	$A8
	pei	$A6
	pei	$A4
	pei	$A2
	pei	$A0
	pei	$9E
	pei	$9C
	pei	$9A
	pei	$98
	nop
	pei	$96
	pei	$94
	pei	$92
	pei	$90
	pei	$8E
	pei	$8C
	pei	$8A
	pei	$88
	pei	$86
	pei	$84
	pei	$82
	pei	$80
	nop
	pei	$7E
	pei	$7C
	pei	$7A
	pei	$78
	pei	$76
	pei	$74
	pei	$72
	pei	$70
	pei	$6E
	pei	$6C
	pei	$6A
	pei	$68
	pei	$66
	nop
	pei	$64
	pei	$62
	pei	$60
	pei	$5E
	pei	$5C
	pei	$5A
	pei	$58
	pei	$56
	pei	$54
	pei	$52
	pei	$50
	pei	$4E
	nop
	pei	$4C
	pei	$4A
	pei	$48
	pei	$46
	pei	$44
	pei	$42
	pei	$40
	pei	$3E
	pei	$3C
	pei	$3A
	pei	$38
	pei	$36
	pei	$34
	nop
	pei	$32
	pei	$30
	pei	$2E
	pei	$2C
	pei	$2A
	pei	$28
	pei	$26
	pei	$24
	pei	$22
	pei	$20
	pei	$1E
	pei	$1C
	pei	$1A
	nop
	pei	$18
	pei	$16
	pei	$14
	pei	$12
	pei	$10
	pei	$0E
	pei	$0C
	pei	$0A
	pei	$08
	pei	$06
	pei	$04
	pei	$02
	pei	$00

	cpy	#$A000	; Did we copy everything?
	bge	peiEnd	; Yep!

	dex
	bmi	peiInts	; Do we need to process interrupts?

	brl	peiPage

peiInts	lda	#0	; Swap main 48k into bank 0
	sta	>$E0C068-1
	lda	>StackPtr	; Load original Stack Pointer into Accumulator
	tcs		; Transfer Accumulator to Stack Pointer (Restores original stack)

	cli		; Go Interrupts!  Go!
	sei		; Stop interrupts

	lda	#$3000	; Read/write aux 48k (SHR) via bank 0
	sta	>$E0C068-1

	brl	peiChunk

peiEnd	lda	#0	; Swap main 48k into bank 0
	sta	>$E0C068-1
	lda	>StackPtr
	tcs		; Restore stack
	pld		; Restore DP
	pla
	sta	>$E0C068-1	; Restore main/aux bank state
	plp		; Restore processor register

	short	m	; Turn Shadowing Off
	lda	>$E0C035
	ora	#$08
	sta	>$E0C035
	long	m

	jreturn

StackPtr	ds	2	; Space for SP

	end

;----------------------------------------
; Start up assembly routines
;
; Builds a table of scanline offsets
;----------------------------------------
asmStart	start

stenB	equ	1	; Stencil byte (only lower nibble is used)
stenC	equ	3	; Stencil byte counter
maskW	equ	5	; Mask word

	jsubroutine (2:id,2:hertz),6
	using GlobalData

	ldy	#0	; Load 0 into y register
	lda	#0	; Load 0 into accumulator
	clc		; Clear carry flag
tScan	anop		; Build scanline table
	sta	ScTable,y	; Store accumulator in ScTable+x
	adc	#160	; Add 160 to accumulator (320 pixels / 2 pixels per byte)
	iny		; Increment y
	iny		; Increment y
	cpy	#400	; Compare y to 400 (200 scanlines)
	bcc	tScan	; Repeat Scan Table Loop until we do all 200 lines

	ldy	#0	; Load 0 into y register
	lda	#0	; Load 0 into accumulator
	clc		; Clear carry flag
tSten	anop		; Build stencil cell table
	sta	StTable,y	; Store accumulator in StencilTable+x
	adc	#320	; Add 320 to accumulator (8 lines * 40 bytes)
	iny		; Increment y
	iny		; Increment y
	cpy	#50	; Compare y to 50 (25 stencil cell offsets)
	bcc	tSten	; Repeat Stencil Table Loop until we do all 25 cells

; Convert stencil byte to mask word (4 pixels)
	clc		; Clear carry flag
	ldy	#0	; Load 0 into y register
	lda	#0	; Stencil bits starting point
	sta	stenC
tMask	ldx	#4	; How many pixels to process?
	lda	#$ffff	; Initially, the mask word is all one bits
	sta	maskW
	lda	stenC	; Put counter in bits
	xba		; Swap MSB and LSB
	asl	a	; Shift mask word over a nibble
	asl	a
	asl	a
	asl	a
	sta	stenB
nextNib	lda	stenB	; Load stencil byte into accumulator
	asl	a	; Shift first bit out of stencil byte into C
	sta	stenB	; Update stencil byte
	bcc	pixOn	; If C is clear, we draw the pixel (opposite of the mask data)
	lda	maskW	; Load mask word into accumulator
	asl	a	; Shift mask word over a pixel (4 bits)
	asl	a
	asl	a
	asl	a
;	or	#$0000	; Do not draw pixel - mask it off
	sta	maskW	; Store updated mask word
	jmp 	nextPix	; Start next pixel
pixOn	lda	maskW	; Load mask word into accumulator
	asl	a	; Shift mask word over a pixel (4 bits)
	asl	a
	asl	a
	asl	a
	ora	#$000f	; Draw pixel
	sta	maskW	; Store updated mask word
nextPix	dex		; Decrement pixel counter in X
	bne	nextNib	; Not zero?  Do next nibble/pixel
	xba		; Flip MSB and LSB
	sta	MaTable,y	; Store accumulator (maskW) in table
	lda	stenC	; Increment stencil bit pattern counter
	inc	a
	sta	stenC	; Store for the next pass
	iny		; Increment y - two bytes
	iny		; Increment y
	cpy	#256	; Compare y to 512 (128 stencil patterns)
	bcc	tMask	; Repeat Mask Table Loop until we do all 128 bit patterns

	lda	hertz	; Store refresh rate / 10 in VblCount and VblRate
	sta	>VblRate

	lda	#0	; Reset timer
	sta	>VblTime

	ph4	#VblHdr	; Start VBL interrupt task
	ldx	#$1203	;_SetHeartBeat
	jsl	$E10000

	ph2	#$0002	; Turn on heart beat IRQs
	ldx	#$2303	;_IntSource
	jsl	$E10000

	jreturn
	end


;----------------------------------------
; Shut down assembly routines
;----------------------------------------
asmStop	start
	jsubroutine

	ph4	#VblHdr	; Stop VBL interrupt task
	ldx	#$1303	;_DelHeartBeat
	jsl	$E10000

	jreturn
	end


;----------------------------------------
; Increment time counter using VBLs every 1/10 second
;----------------------------------------
VblHdr	start
	dc	i4'0'	; Space for task pointer
VblCount	dc	i2'1'	; How many VBLs between calls (1 only on initial call)
	dc	i2'$A55A'	; Task signature

	using GlobalData

	long	m,i
	lda	>VblRate	; Reset hearbeat counter
	sta	>VblCount
	lda	>VblTime	; Increment timer
	inc	a
	sta	>VblTime
	short	m,i
	rtl
	end