heapq.asm

heapq.asm

			; Binheap priority queue
			; Based on 6502 code by MagerValp
			; https://github.com/MagerValp/AsmHeap


			; 255 items max, 3 bytes wasted
			struct HEAPQ
			align 256
tree			ds 255, 0
size			db 0
indices			ds 256, 0
values			ds 256*2, 0
			ends


			module heapq

			; Push a 16-bit value with a given priority
			; in: hl -> heap, a = priority, de = value
			; out: none
push			push bc, de, hl
			call __push
			pop hl, de, bc
			ret

			; Pop the value with the highest priority
			; in: hl -> heap
			; out: a = priority, de = value
pop			push bc, hl
			call __pop
			pop hl, bc
			ret


__push			dec l: inc (hl): ld l, (hl)

			; store node
			dec l: ld (hl), a

			ld c, h

			; store index
			inc h: ld (hl), l

			; store value
			inc h: ld (hl), e
			inc h: ld (hl), d

			ld h, c

			ld a, l: or a: ret z
			ld d, h: ld e, l

.loop			dec e: srl e

			; swap nodes
			ld c, (hl): ld a, (de)
			cp c: ret c
			ld (hl), a: ld a, c: ld (de), a

			; swap indices
			inc h: inc d
			ld c, (hl): ld a, (de)
			ld (hl), a: ld a, c: ld (de), a
			dec h: dec d

			ld l, e

			ld a, l: or a: jp nz, .loop
			ret


__pop			ld a, (hl) ; get node
			inc h: ld e, (hl)
			dec h ; get index

			ld d, h
			dec l: dec (hl): jr z, .get_value

			push af, de

			ld b, (hl): inc l
			ld e, b

			; store node
			ld a, (de): ld (hl), a

			; store index
			inc h: inc d
			ld a, (de): ld (hl), a
			dec h: dec d

			jr .loop

.swap			ld e, c

			; swap nodes
			ld c, (hl): ld a, (de)
			ld (hl), a: ld a, c: ld (de), a

			; swap indices
			inc h: inc d
			ld c, (hl): ld a, (de)
			ld (hl), a: ld a, c: ld (de), a
			dec h: dec d

.loop			ld c, l

			ld a, l: add a: inc a: ld e, a
			cp b: jr nc, .e_gte_sz
			ld a, (de): cp (hl): jp nc, $+4: ld l, e

			inc e: ld a, e
			cp b: jr nc, .e_gte_sz
			ld a, (de): cp (hl): jp nc, $+4: ld l, e

.e_gte_sz		ld a, l: cp c: jp nz, .swap

			pop de, af

.get_value		ex de, hl
			inc h: inc h
			ld e, (hl): inc h: ld d, (hl)
			ret

			endmodule