.include "system.h65"
.include "string.h65"
.include "lcd.h65"
.include "math.h65"
.include "keypad.h65"
.include "keyboard.h65"
.include "chars.h65"
.import homeloop:absolute
.import home:absolute

.segment "SPI"
.export CODE_START

.import memcopy



CODE_START:
.assert * = $5000, error, "SPI Code not at $5000"
    jsr lcd::clear
    lda #'$'
    jsr lcd::print_char
    ; jmp homeloop
    DEBUG_LED_OFF 0
    DEBUG_LED_OFF 1
    DEBUG_LED_OFF 2

    lda #<kb_irq1
    sta ARG0
    lda #>kb_irq1
    sta ARG1
    lda #<$3000
    sta ARG2
    lda #>$3000
    sta ARG3
    ldy #10
    jsr memcopy

    lda #<kb_irq2
    sta ARG0
    lda #>kb_irq2
    sta ARG1
    lda #<$3100
    sta ARG2
    lda #>$3100
    sta ARG3
    ldy #10
    jsr memcopy


    ; PrintNC $3000

    jsr kb::init

    stz kb::status

    lda #'%'
    jsr lcd::print_char

    stz kp::_DEBUG_VAL
    ldy #0

@loop:
    wai
    lda kb::scancode
    beq @noscancode
    jsr process_scancode
    stz kb::scancode
    lda char
    beq @onlykeycode
    Strf fmt_codechar,out_str,keycode,kb::status,char
    PrintNC out_str
    stz char
    stz keycode
    bra @noscancode
@onlykeycode:
    lda keycode
    beq @noscancode
    Strf fmt_code,out_str,keycode,keycode_flags
    PrintNC out_str
    stz keycode
@noscancode:
    bbr5 kb::status, @shift_off
@shift_on:
    DEBUG_LED_ON 0
    bra @check_caps
@shift_off:
    DEBUG_LED_OFF 0
@check_caps:
    bbr4 kb::status, @caps_off
@caps_on:
    DEBUG_LED_ON 2
    bra @read_keypad
@caps_off:
    DEBUG_LED_OFF 2
@read_keypad:
    lda kp::_DEBUG_VAL
    jeq @loop
    stz kp::_DEBUG_VAL
    cmp #'*'
    jeq homeloop
    cmp #'1'
    beq @l1
    cmp #'2'
    beq @l2
    cmp #'3'
    beq @l3
    cmp #'A'
    beq @lA
    cmp #'B'
    beq @lB
    cmp #'C'
    beq @lC
    jsr lcd::print_char
    jmp @loop
@l1:
    ; jsr irq_on_shift_reg
    jsr $3000
    lda #'*'
    jsr lcd::print_char
    jmp @loop
@l2:
    ; jsr irq_on_timer
    jsr $3100
    lda #'#'
    jsr lcd::print_char
    jmp @loop
@l3:
    lda $3000,y
    jsr lcd::print_char
    iny
    jmp @loop
@lA:
    lda kb::KB_IO + IO::SR
    jsr lcd::print_char
    jmp @loop
@lB:
    ; Printf "kc%x;", out_str, kb::scancode
    jsr lcd::clear
    ldx out_idx
    lda #0
    sta out_str,x
    Print out_str
    stz out_idx

    jmp @loop
@lC:
    jsr lcd::clear
    jmp @loop


; key_read: .res 2
; scancode:  .res 1
kb_irq1:
    ; lda #'!'
    ; jsr lcd::print_char
    jsr kb::irq_shift_reg_handler
    ; lda #':'
    ; jsr lcd::print_char
    rts
.byte '='
kb_irq2:
    ; lda #'?'
    ; jsr lcd::print_char
    ; jsr kb::on_timer_irq
    jsr kb::irq_timer_handler
    ; lda #';'
    ; jsr lcd::print_char
    rts
.byte '@'

.proc process_scancode
    ; DEBUG_LED_OFF 1
    ; check how this scancode needs to be interpreted
    ldx kb::scancode
    bit kb::status
    jmi release_key   ; bit 7 set
    bvs @twobytecode  ; bit 6 set
    ; DEBUG_LED_ON 1

    ; check mods
    cpx #kb::K_RELEASE
    beq @code_release
    cpx #kb::K::LEFTSHIFT
    beq @code_shift
    cpx #kb::K::RIGHTSHIFT
    beq @code_shift
    cpx #kb::K_TWOBYTE
    beq @code_twobytecode
    cpx #kb::K::CAPSLOCK
    beq @code_capslock

    ; a real keycode and not a released keycode, twobyte or release scancode
    stz keycode_flags
    stx keycode
    cmp #$62  ; $61 is the largest ascii scancode
    bcs @special
    bbs5 kb::status,@modshift   ; if shift active
@nomod:
    lda kb::CHARS_NOMOD,x 
    beq @special
@char:  ; order which is fastet most of the time (lowercase characters)
    sta char
    rts
@modshift:
    lda kb::CHARS_MODSHIFT,x 
    bne @char
@special:
    rts
    
@code_release:      ; set release
    smb7 kb::status 
    rts
@code_twobytecode:  ; set twobytecode
    smb6 kb::status
    rts
@code_shift:        ; process shift
    bbs4 kb::status, @unset_shift  ; unset shift when capslock is on
@set_shift:
    smb5 kb::status
    rts
@unset_shift:
    rmb5 kb::status
    rts
@code_capslock:     ; toggle capslock & shift
    bbs1 kb::status, @rts   ; ignore if held
    lda kb::status
    ora #(kb::STATUS::CAPSLOCK_HELD)
    ; toggle and capslock shift
    eor #(kb::STATUS::CAPSLOCK | kb::STATUS::SHIFT)
    sta kb::status
    rts
@code_rightalt:     ; set rightalt
    smb3 kb::status
    rts
@code_numlock:      ; set numlock
    smb2 kb::status
    rts

@twobytecode:
    rmb6 kb::status  ; unset twobytecode
    ; check mods
    cpx #kb::K2::RIGHTALT
    beq @code_rightalt

    lda #kb::STATUS::TWOBYTE
    sta keycode_flags
    stx keycode
@rts:
    rts
.endproc

;; @param S: The status register must be set through `bit kb::status`
;; @param X: The scancode
.proc release_key
    bvs @twobytecode
    rmb7 kb::status 

    ; check mods
    cpx #kb::K_TWOBYTE
    beq @code_twobytecode
    cpx #kb::K::LEFTSHIFT
    beq @code_shift
    cpx #kb::K::RIGHTSHIFT
    beq @code_shift
    cpx #kb::K::CAPSLOCK
    beq @code_capslock
    ; no need to process capslock - it gets toggled when pressed
    rts
@code_twobytecode:  ; unset special
    rmb6 kb::status
    rts
@code_shift:        ; process shift
    bbs4 kb::status, @code_shift_caps  ; set shift when capslock is on
    rmb5 kb::status ; unset if capslock is off
    rts
@code_shift_caps:
    smb5 kb::status
    rts
@code_capslock:     ; unset CAPSLOCK_HELD
    rmb1 kb::status
    rts
@code_rightalt:     ; unset rightalt
    rmb3 kb::status
    rts
@code_numlock:      ; unset numlock
    rmb2 kb::status
    rts

@twobytecode:
    rmb6 kb::status  ; unset twobytecode
    ; check mods
    cpx #kb::K2::RIGHTALT
    beq @code_rightalt
    rts
.endproc

out_idx: .res 1
out_str: .res 40
char:           .res 1
keycode:        .res 1
keycode_flags:  .res 1
fmt_codechar: .asciiz "%x %x %c "
fmt_code: .asciiz "%x %x   "