// vim: filetype=kickass

tree_clear:
    lda #0
    tay

    !loop:
        sta tree_values_lo, y
        sta tree_values_hi, y
        sta tree_left, y
        sta tree_right, y
        iny
        bne !loop-

    rts

.const next_index = $02
.const input = $03
tree_insert:
    ldx #0

    !loop:
        lda tree_values_lo, x
        bne !not_zero+
        lda tree_values_hi, x
        bne !not_zero+

            // Insert the node
            lda input
            sta tree_values_lo, x
            lda input + 1
            sta tree_values_hi, x

            inc next_index
            rts

    !not_zero:
        sec
        lda tree_values_lo, x
        sbc input
        lda tree_values_hi, x
        sbc input + 1
        bcc !greater+

        !less:
            lda tree_left, x
            bne !+
                lda next_index
                sta tree_left, x
            !:
            lda tree_left, x
            tax

            jmp !loop-

        !greater:
            lda tree_right, x
            bne !+
                lda next_index
                sta tree_right, x
            !:
            lda tree_right, x
            tax

            jmp !loop-

    brk


tree_contains:
    ldx #0

    !loop:
        sec
        lda tree_values_lo, x
        sbc input
        beq !lo_eq+
        lda tree_values_hi, x
        sbc input + 1
        bcc !greater+

        !less:
            lda tree_left, x
            beq !not_found+
            tax
            jmp !loop-

        !greater:
            lda tree_right, x
            beq !not_found+
            tax
            jmp !loop-

        !not_found:
            // Match not found, set 1 so bne branches
            lda #1
            rts

        !lo_eq:
            lda tree_values_hi, x
            sbc input + 1
            bne !less-

            // Match found, set 0 so beq branches
            lda #0
            rts

    brk

test_tree:
    move_16_imm($03, !str_test+)
    jsr write_string

    move_16_imm($03, $10)
    jsr tree_insert
    move_16_imm($03, $08)
    jsr tree_insert
    move_16_imm($03, $04)
    jsr tree_insert
    move_16_imm($03, $12)
    jsr tree_insert
    move_16_imm($03, $1337)
    jsr tree_insert

    move_16_imm($03, $08)
    jsr tree_contains
    bne !fail+

    move_16_imm($03, $13)
    jsr tree_contains
    beq !fail+

    move_16_imm($03, $12)
    jsr tree_contains
    bne !fail+

    move_16_imm($03, $1337)
    jsr tree_contains
    bne !fail+

    move_16_imm($03, $0420)
    jsr tree_contains
    beq !fail+

    move_16_imm($03, !str_success+)
    jsr write_string

    rts

!fail:
    u16_u16_move($10, $03)

    move_16_imm($03, !str_fail+)
    jsr write_string

    lda $11
    jsr print_hex_0x
    lda $10
    jsr print_hex
    jsr print_newline

    rts

!str_test:
    .text "[  ] test-tree"
    .byte '\r', 0

!str_fail:
    .text "[!!]"
    .byte '\n'
    .text "Failed: "
    .byte 0

!str_success:
    .text "[OK]"
    .byte '\n', 0