!to "tastaohnekernal.prg",cbm

KeyboardMatrixRow           = $fc
KeyboardScanMatrixPointer   = $fe

*=$1000
sei
loop1:
jsr PrintFIFO
jsr KeyboardScan
jmp loop1

KeyboardScan:                       ;;;;;;; Prepare Scan

;;;;;;;;;;;;;;;;; DEBUG
lda #$7f
waitloop:
cmp $d012
bne waitloop

inc $d020
;;;;;;;;;;;;;;;;; DEBUG

lda #$00                            ; Check if we have a keypress after all
sta $dc00
lda $dc01
cmp #$ff
beq KeyboardScan_NoKeyPress

lda Keyboard_KeyboardUp             
bne KeyboardScanFinished            ; If the last status was no key press
                                    ; go and grab a key


lda #<KeyboardScanMatrix            ; Prepare Matrix Pointer
sta KeyboardScanMatrixPointer
lda #>KeyboardScanMatrix
sta KeyboardScanMatrixPointer+1

lda #%11111110                      ; Prepare row select
ldx #$00
stx KeyboardMatrixRow

KeyboardScanLoop:                   ;Check every row
sta $dc00               
pha                                 ;Put current row pos on stack
lda $dc01                           ;Grab that row

ldy #$ff                            ;Get the collumn
KeyboardScan_MatrixLoop1:
iny
clc
ror
bcs KeyboardScan_MatrixLoop1
cpy #$08                            ;When no key is pressed then check next row
beq KeyboardScan_NoKeyPressed
lda (KeyboardScanMatrixPointer),y
jsr Keyboard_AddToFIFO

KeyboardScan_NoKeyPressed:
pla                         ;Retrive row pos
sec                         ;ROL it
rol
pha
lda KeyboardScanMatrixPointer
clc
adc #$08
sta KeyboardScanMatrixPointer
pla
inx                         ;And check if we are finished
cpx #$08
bne KeyboardScanLoop
KeyboardScanFinished:
dec $d020
rts

KeyboardScan_NoKeyPress:
dec $d020
lda #$00
sta Keyboard_KeyboardUp
rts


Keyboard_AddToFIFO:
ldy #$01                                ;Keep that we had a keypress
sty Keyboard_KeyboardUp 
cmp #$ff
beq KeyboardScan_AddToFIFO_ShiftKey
ldy KeyboardScanFIFOWritePointer
cpy KeyboardScanFIFOReadPointer
;beq KeyboardScan_AddToFIFO_NoAdd
sta KeyboardScanFIFO,y
iny
cpy #$10
beq Keyboard_AddToFIFO_ResetPointer
sty KeyboardScanFIFOWritePointer
rts
Keyboard_AddToFIFO_ResetPointer
ldy #$00
sty KeyboardScanFIFOWritePointer
KeyboardScan_AddToFIFO_NoAdd:
KeyboardScan_AddToFIFO_ShiftKey:
rts

;;;;;;;;;;;;;;;;;;;;;; DEBUG, print current FIFO
PrintFIFO:
ldx #$00
PrintFIFO_Loop:
lda KeyboardScanFIFO,x
cpx KeyboardScanFIFOWritePointer
bne PrintFIFO_NoInvert
ora #$80
PrintFIFO_NoInvert:
sta $0400,x
inx
cpx #$10
bne PrintFIFO_Loop
lda KeyboardScanFIFOWritePointer
sta $0428
lda KeyboardScanFIFOReadPointer
sta $0429
rts




*=$3000
!convtab scr
KeyboardScanMatrix:
!byte $fb,$fc,$fd,$f7,$f1,$f3,$f5,$fe
!byte "3","w","a","4","z","s","e",$ff
!byte "5","r","d","6","c","f","t","x"
!byte "7","y","g","8","b","h","u","v"
!byte "9","i","j","0","m","k","o","n"
!byte "+","p","l","-",".",":","@",","
!byte $1c,"*",";",$53,$ff,"=",$1e,"/"
!byte "1",$1f,$58,"2"," ",$5a,"q",$57

KeyboardScanFIFOWritePointer:
!byte $00
KeyboardScanFIFOReadPointer:
!byte $0f
KeyboardScanFIFO:
!fill 16
!byte $01
Keyboard_KeyboardUp
!byte $00