.include "system.h65"
.include "string.h65"
.include "lcd.h65"
.include "math.h65"
.include "keypad.h65"
.include "chars.h65"
.import homeloop:absolute
.import home:absolute
.segment "SPI"
.export CODE_START

CODE_START:
.assert * = $5000, error, "SPI Code not at $5000"
    jsr lcd::clear
    lda #'>'
    jsr lcd::print_char
    jsr rb_test_init
    stz kp::_DEBUG_VAL
    ldy #0
@loop:
    lda kp::_DEBUG_VAL
    pha
    stz kp::_DEBUG_VAL
    pla
    beq @loop
    cmp #'*'
    jeq homeloop
    cmp #'#'
    beq @print
    tya
    pha
    iny
    jsr rb_test_write
    pla
    ora #$30
    jsr lcd::print_char

    bra @loop


@print:
@printloop:
    lda #'r'
    jsr lcd::print_char
    jsr @print_state

    jsr rb_test_read
    beq @empty
    ora #$30
    jsr lcd::print_char

    lda #';'
    jsr lcd::print_char

    bra @printloop
@empty:
    lda #'E'
    jsr lcd::print_char
    lda #';'
    jsr lcd::print_char
    bra @loop
@print_state:
    lda #'('
    jsr lcd::print_char
    lda RB_READ
    ora #$30
    jsr lcd::print_char
    lda RB_WRITE
    ora #$30
    jsr lcd::print_char
    lda #')'
    jsr lcd::print_char
    rts
    



TEST_MEMSIZE = $A

RBUF_MEM_START:
    .res TEST_MEMSIZE
RBUF_MEM_END:
    

.define RBUF_NAME "test"
.define _RBUF_NAME .concat("rb_", RBUF_NAME)
RB_WRITE    = RBUF_MEM_START        ; write pointer, relative to RB_WRITE
RB_READ     = RBUF_MEM_START + 1    ; read ponter, relative to RB_START
RB_START    = RBUF_MEM_START + 2
RB_LENGTH   = RBUF_MEM_END - RBUF_MEM_START - 2

.if RB_LENGTH < 1
    .fatal "buffer size too small, must be at least 1"
.endif
.if RB_LENGTH > $ff
    .fatal "buffer size too large, must be <= $ff"
.endif

; .out .sprintf("Buffer: %x -> %x (len=%x)", RBUF_MEM_START, RBUF_MEM_END, RB_LENGTH)


.proc rb_test_init
    stz RB_WRITE
    stz RB_READ
    rts
.endproc

.proc rb_test_read
    ldx RB_READ
    cpx RB_WRITE
    beq @rb_read_rts  ; if buffer empty
    lda RB_START,x
    inx ; increment RB_READ pointer, not using macro bec. of unknown Pz
    cpx #RB_LENGTH
    beq @read_wrap
    stx RB_READ 
@rb_read_rts:
    rts
@read_wrap:  ; ptr == RB_LENGTH -> ptr = 0
    stz RB_READ
    ; make sure Pz is not set
    ldx #$01
    rts
.endproc

.proc rb_test_write
    ; lda kp_VALUES, x  ; load the char in a
    ldx RB_WRITE
    sta RB_START,x
    inx  ; increment write pointer
    cpx #RB_LENGTH
    beq @write_wrap
    stx RB_WRITE
@check_buf_full:  ; increment read if buffer is full
    cpx RB_READ
    beq @read_inc
    rts
@write_wrap:  ; ptr == RB_LENGTH -> ptr = 0
    stz RB_WRITE
    ldx #0
    bra @check_buf_full
@read_inc:
    ldx RB_READ
    inx
    cpx #RB_LENGTH
    beq @read_wrap
    stx RB_READ
    rts
@read_wrap:  ; ptr == RB_LENGTH -> ptr = 0
    stz RB_READ
    rts
.endproc