define queueH $10
define queueB $11
define queueT $12
define qLength $02
define prevBlock $05
define currentBlock $06

; Use the monitor to see visually what the queue is doing.
; Step through the program to see how the queue changes.
main:
  ; Initialize the memory for our Queue structure.
  jsr init

  ; Insert 1 2 3 4 into our queue.
  lda #$01
  jsr insert
  lda #$02
  jsr insert
  lda #$03
  jsr insert
  lda #04
  jsr insert

  ; pop 1 from the queue
  jsr pop

  ; pop 2 from the queue
  jsr pop

  ; pop 3 from the queue
  jsr pop

  ; pop 4 from the queue
  jsr pop

  ; insert 5 6 7 into the queue
  lda #$05
  jsr insert
  lda #$06
  jsr insert
  lda #$07
  jsr insert


  brk



init:
  lda #$0
  sta queueH
  sta queueB
  sta queueT
  sta qLength
  rts

insert:
  ; Check if we're storing nothing
  cmp #$0
  ; Exit the function if the A register is #$0
  beq endEarly

  ; Insert whatever is stored on A
  ; register to the end of the queue.
  ldx qLength

  ; qLength offsets the insert position to the
  ; end of the queue.
  sta queueH, x

  ; Increase the length of the queue
  ; after insertion.
  inc qLength

  ; Zero out the A register.
  lda #$0
  rts

; Removes the first element of the queue
; and shifts the remaining values forward.
pop:
  ; Check if the queue is empty.
  ldx qLength
  cpx #$0

  ; If the queue is empty rts and do nothing.
  beq endEarly

  ; Zero out the X register that will be used
  ; in the shiftForward subroutine.
  ldx #$0
  jsr shiftForward

  ; After the values have been shifted forward
  ; in the queue decrease the length of the queue.
  dec qLength

  rts

; Shift the values forward in the queue.
; *Note: This subroutine is very similar to the
;        shift values subroutine in our snake game.
shiftForward:
  cpx qLength
  beq endEarly
  lda queueB, x
  sta queueH, x
  inx
  jsr shiftForward

; Ends a subroutine early if needed.
endEarly:
  rts