.include "string.h65"
Export str, strf, printf_buffer

.bss
;;********************************************************************************
;; @brief Used to store output string of Printf macro
;; @todo Use dynamically allocated buffer when a memory allocator is implemented
;;********************************************************************************
printf_buffer: .res $41

.zeropage
fmt_idx: .res 1
out_idx: .res 1
pad_zero:.res 1
digits:  .res 1
; addr:    .res 2
.code
;;********************************************************************************
;; @function Format a string
;; @details
;;  If there is no value to be read, the `Pz` will be set
;;  Formats:
;;  - `x`: unsigned hex integer (1 byte) -> 2 chars
;;  - `X`: unsigned hex integer (2 byte) -> 4 chars TODO
;;  - `u`: unsigned decimal integer (1 byte) TODO
;;  - `U`: unsigned decimal integer (2 bytes) TODO
;;  - `s`: null-terminated string  (2 bytes ptr)
;;  - `c`: char
;; @todo Implement decimal, test X
;; @param ARG0-1: Format string address
;; @param ARG2-3: Output string address
;; @param ARG4+:  Additional parameters
;; @returns 
;; @modifies: A, X, Y, ARG4, ARG5
;;********************************************************************************
.proc strf
    stz out_idx
    stz fmt_idx
    ldy #0  ; position in string
    ldx #0  ; index of format args
@loop:
    ldy fmt_idx
    lda (ARG0),y
    jeq @null
    cmp #'%'
    beq @percent
@normal_char:  ; store A in output string
        ldy out_idx
        sta (ARG2),y
        inc fmt_idx
        inc out_idx
        beq @out_overflow
        bra @loop
@percent:  ; check for format in next position
        iny
        sty fmt_idx
        lda (ARG0),y
        beq @null
        ; padding
        cmp #'0'
        beq @percent_zero
        stz pad_zero
        ; formats
        cmp #'x'
        beq @format_hex1
        cmp #'X'
        beq @format_hex2
        cmp #'c'
        beq @format_char

        bra @normal_char
@percent_zero:
        sta pad_zero
        bra @percent
; @percent_num:
;         sta pad_zero
;         bra @percent

@format_hex1:   ; 1 byte hex -> 2 chars
            lda ARG4,x
            phx
                jsr str::uint8_to_hex_str
                ldy out_idx
                sta (ARG2),y  ; most sig digit
                txa
            plx
            iny
            beq @out_overflow
            sta (ARG2),y  ; least sig digit
            iny
            beq @out_overflow
            sty out_idx
            inx  ; 1 byte of args handeled
            bra @format_return

@format_hex2:   ; 2 byte hex -> 4 chars
            lda #2
            ldy out_idx
            jsr str::uint_to_hex_str
            bmi @out_overflow  ; might also be x overflow
            sty out_idx
            bra @format_return

@format_char:   ; 1 char
            lda ARG4,x
            ldy out_idx
            sta (ARG2),y
            inc out_idx
            beq @out_overflow
            inx
            bra @format_return

@format_string:   ; string
            ldy #0
            cpx #0
            beq @format_string_loop
            ; store the pointer to the string at arg4-5
@format_string_move_ptr:
            lda ARG4,x
            sta ARG4
            lda ARG5,x
            sta ARG5
@format_string_loop:
            lda (ARG4),y
            beq @format_string_end  ; if null
            phy
            ldy out_idx
            sta (ARG2),y
            inc out_idx
            beq @out_overflow
            ply
            iny
@format_string_end:
            inx
            inx
            bra @format_return

@format_return:  ; increment fmt_idx to account for the formating char
        inc fmt_idx  ; at this point, out_idx will always overflow first
        jmp @loop
@out_overflow:  ; store 0 in last position
    ldy #$ff
    sty out_idx
@store_null:
    lda #0
@null:  ; store and return
    ldy out_idx
    sta (ARG2),y
@rts:
    rts
.endproc