.segment "HEADER"

INES_MAPPER = 0
INES_MIRROR = 1
INES_SRAM   = 0

.byte 'N', 'E', 'S', $1A
.byte $02
.byte $01
.byte INES_MIRROR
.byte (INES_MAPPER & %11110000)
.byte $0, $0, $0, $0, $0, $0, $0, $0


.segment "TILES"
.incbin "background.chr"
.incbin "sprite.chr"


.segment "VECTORS"
.word nmi
.word reset
.word irq


.segment "CODE"
reset:
    sei
    lda #0
    sta $2000
    lda #0
    sta $2001
    sta $4015
    sta $4010
    lda #40
    sta $4017
    cld
    ldx #$FF
    txs

    bit $2002
    :
        bit $2002
        bpl :-

    lda #0
    ldx #0
    :
        sta $0000, X
        sta $0100, X
        sta $0200, X
        sta $0300, X
        sta $0400, X
        sta $0500, X
        sta $0600, X
        sta $0700, X
        inx
        bne :-

    lda #255
    ldx #0
    :
        sta oam, X
        inx
        inx
        inx
        inx
        bne :-

    :
        bit $2002
        bpl :-

    lda #%10101000 ; NMI enable, 8x16 sprites, ignored due to 8x16 mode
    sta $2000
    jmp main


.segment "ZEROPAGE"
nmi_lock:       .res 1
nmi_count:      .res 1
nmi_ready:      .res 1
nmt_update_len: .res 1
scroll_x:       .res 1
scroll_y:       .res 1
scroll_nmt:     .res 1
temp:           .res 1

.segment "BSS"
nmt_update:     .res 256
palette:        .res 32

.segment "OAM"
oam:            .res 256

.segment "CODE"
nmi:
    pha
    txa
    pha
    tya
    pha

    lda nmi_lock
    beq :+
        jmp @nmi_end

    :
    lda #1
    sta nmi_lock

    lda nmi_ready
    bne :+
        jmp @ppu_update_end

    :
    cmp #2
    bne :+
        lda #%00000000
        sta $2001
        ldx #0
        stx nmi_ready
        jmp @ppu_update_end

    :
    ldx #0
    stx $2003
    lda #>oam
    sta $4014

    lda #%10101000 ; NMI enable, 8x16 sprites, ignored due to 8x16 mode
    sta $2000
    lda $2002
    lda #$3F
    sta $2006
    stx $2006
    ldx #0
    :
        lda palette, X
        sta $2007
        inx
        cpx #32
        bcc :-

    ldx #0
    cpx nmt_update_len
    bcs @scroll
    @nmt_update_loop:
        lda nmt_update, X
        sta $2006
        inx
        lda nmt_update, X
        sta $2006
        inx
        lda nmt_update, X
        sta $2007
        cpx nmt_update_len
        bcc @nmt_update_loop

    lda #0
    sta nmt_update_len
@scroll:
    lda scroll_nmt
    and #%00000011
    ora #%10101000
    sta $2000
    lda scroll_x
    sta $2005
    lda scroll_y
    sta $2005

    lda #%00011110
    sta $2001

    ldx #0
    stx nmi_ready
@ppu_update_end:
    lda #0
    sta nmi_lock
@nmi_end:
    pla
    tay
    pla
    tax
    pla
    rti


.segment "CODE"
irq:
    rti


.segment "CODE"
ppu_update:
    lda #1
    sta nmi_ready
    :
        lda nmi_ready
        bne :-

    rts

ppu_skip:
    lda nmi_count
    :
        cmp nmi_count
        beq :-

    rts

ppu_off:
    lda #2
    sta nmi_ready
    :
        lda nmi_ready
        bne :-

    rts


PAD_A      = $01
PAD_B      = $02
PAD_SELECT = $04
PAD_START  = $08
PAD_U      = $10
PAD_D      = $20
PAD_L      = $40
PAD_R      = $80

.segment "ZEROPAGE"
gamepad: .res 1

.segment "CODE"
; gamepad_poll: this reads the gamepad state into the variable labelled "gamepad"
;   This only reads the first gamepad, and also if DPCM samples are played they can
;   conflict with gamepad reading, which may give incorrect results.
gamepad_poll:
	; strobe the gamepad to latch current button state
	lda #1
	sta $4016
	lda #0
	sta $4016
	; read 8 bytes from the interface at $4016
	ldx #8
	:
		pha
		lda $4016
		; combine low two bits and store in carry bit
		and #%00000011
		cmp #%00000001
		pla
		; rotate carry into gamepad variable
		ror
		dex
		bne :-
	sta gamepad
	rts


.segment "RODATA"

level:
.byte 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
.byte 0,1,0,0,1,0,0,0,0,1,0,0,2,7,7,3
.byte 0,0,0,0,0,0,0,0,0,0,0,0,6,0,0,9
.byte 0,1,0,0,1,7,7,7,7,1,7,7,6,0,0,9
.byte 0,0,1,1,0,0,0,0,0,1,0,0,4,8,8,5
.byte 0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0
.byte 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
.byte 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
.byte 0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0
.byte 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
.byte 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
.byte 0,0,0,0,0,0,0,0,0,0,0,10,10,10,10,10
.byte 0,0,1,0,0,0,0,1,0,0,0,11,11,11,11,11
.byte 0,1,2,3,4,5,6,7,8,9,0,11,11,11,11,11
.byte 0,0,0,0,0,0,0,0,0,0,0,11,11,11,11,11

metatiles:
.byte 0,0,0,0,0
.byte $14,$15,$16,$17,1 ; Full block
.byte $14,$18,$1B,$00,0 ; up / left
.byte $18,$15,$00,$19,0 ; up / right
.byte $1B,$00,$16,$1A,0 ; down / left
.byte $00,$19,$1A,$17,0 ; down / right
.byte $1B,$00,$1B,$00,0 ; left
.byte $18,$18,$00,$00,0 ; up
.byte $00,$00,$1A,$1A,0 ; down
.byte $00,$19,$00,$19,0 ; right
.byte $10,$11,$12,$13,2
.byte $12,$12,$12,$12,2

example_palette:
.byte $0F,$00,$3D,$20 ; greyscale
.byte $0F,$3D,$30,$00 ; whitescale
.byte $0F,$09,$1A,$16 ; grass
.byte $0F,$15,$26,$37 ; bg0 purple/pink
.byte $0F,$2D,$10,$3D ; reboot bottom
.byte $0F,$2D,$10,$2C ; reboot eye
.byte $0F,$01,$11,$21 ; bg2 blue
.byte $0F,$00,$10,$30 ; bg3 greyscale
; .byte $0F,$18,$28,$38 ; sp0 yellow
; .byte $0F,$14,$24,$34 ; sp1 purple
; .byte $0F,$1B,$2B,$3B ; sp2 teal
; .byte $0F,$12,$22,$32 ; sp3 marine

.segment "ZEROPAGE"
buttons:    .res 1

player_pos_x:   .res 1
player_pos_y:   .res 1
player_subpos_x:  .res 1 ; XXXXYYYY | first 4 bits X subposition, last 4 bits Y
player_subpos_y:  .res 1 ; XXXXYYYY | first 4 bits X subposition, last 4 bits Y
player_vel_x:   .res 1 ; +PPPSSSS | first bit sign, next 3 pixels, last subpixels
player_vel_y:   .res 1 ; in subpixels

player_status:  .res 1 ; 76543210 | 0: facing (0 right, 1 left)
                       ;          | 7: talking
horizontal_speed: .res 1

cursor_x: .res 1
cursor_y: .res 1
temp_x:   .res 1
temp_y:   .res 1
temp_mul: .res 1
var_m:    .res 1
var_n:    .res 1
var_o:    .res 1
var_p:    .res 1

jump_pressed_last_frame: .res 1

frame_counter: .res 1
last_frame_jumped: .res 1
last_frame_moving: .res 1

.segment "CODE"
main:
	ldx #0
	:
		lda example_palette, X
		sta palette, X
		inx
		cpx #32
		bcc :-

	jsr setup_background
	jsr ppu_update

    lda #$40
    sta cursor_x
    sta cursor_y

    lda #0
    sta frame_counter
    sta last_frame_jumped

    jsr init_objects

@loop:
    lda $2002
    lda frame_counter
    clc
    adc #1
    sta frame_counter


    jsr controller
    jsr movement
    lda player_pos_x

@draw:
    jsr draw_player
	jsr ppu_update

	jmp @loop

draw_player:
    lda player_pos_y
    sec
    sbc #1
    sta oam + (4 * 2) + 0
    sta oam + (4 * 3) + 0
    sec
    sbc #16
    sta oam + (4 * 0) + 0
    sta oam + (4 * 1) + 0

    lda player_status
    and #%00000001
    cmp #0
    bne :+
        lda player_pos_x
        sta oam + (4 * 1) + 3
        sta oam + (4 * 3) + 3
        sec
        sbc #8
        sta oam + (4 * 0) + 3
        sta oam + (4 * 2) + 3
        jmp :++
    :
        lda player_pos_x
        sta oam + (4 * 0) + 3
        sta oam + (4 * 2) + 3
        sec
        sbc #8
        sta oam + (4 * 1) + 3
        sta oam + (4 * 3) + 3
    :

    lda player_status
    and #%10000000
    cmp #0
    bne :+
        lda #$45
        jmp :++
    :
        lda #$49
    :
    sta oam + (4 * 0) + 1
    clc
    adc #2
    sta oam + (4 * 1) + 1
    lda #$65
    sta oam + (4 * 2) + 1
    lda #$67
    sta oam + (4 * 3) + 1

    
    lda player_status
    and #%00000001
    cmp #0
    beq :+
        lda #%01000000
        jmp :++
    :
        lda #%00000000
    :

    ora #%00000001
    sta oam + (4 * 0) + 2
    ora #%00000001
    sta oam + (4 * 1) + 2
    and #%11111100
    sta oam + (4 * 2) + 2
    and #%11111100
    sta oam + (4 * 3) + 2

    rts

init_objects:
    lda #0
    sta var_n
    ldx #0
    :
        lda var_n
        clc
        adc #8
        sta var_n

        sta player_pos_y, X ; Set Y position
        sta player_pos_x, X ; Set X position

        txa
        lsr
        lsr
        sta oam+1, X

        inx
        inx
        inx
        inx
        cpx #(8*4)
        bne :-

    lda #0
    sta player_vel_x
    sta player_vel_y
    sta player_status

    lda #%00101000
    sta horizontal_speed

    rts

draw:
@end:
	rts

movement:
    ; Talking
    ldx #0
    jsr btn_b
    cmp #0
    beq :+
        lda player_status
        ora #%10000000
        sta player_status
        jmp :++
    :
        lda player_status
        and #%01111111
        sta player_status
    :

    ; Horizontal velocity
    ldx #0
    jsr btn_right
    cmp #0
    beq :+
        ldx #1
        lda horizontal_speed
        sta player_vel_x

        lda player_status
        and #%11111110
        sta player_status

        lda frame_counter
        sta last_frame_moving
    :

    jsr btn_left
    cmp #0
    beq :+
        ldx #1
        lda horizontal_speed
        eor #%11111111
        clc
        adc #1
        sta player_vel_x

        lda player_status
        ora #%00000001
        sta player_status

        lda frame_counter
        sta last_frame_moving
    :

    cpx #1 ; skip decay if right or left pressed
    beq @end
        lda frame_counter
        clc
        sbc last_frame_moving
        and #3
        cmp #0
        bne @end

        lda player_vel_x
        and #%01111111
        cmp #0
        beq @end ; If player_vel_x = 0, skip decay

        lda player_vel_x
        and #%10000000
        ; If player_vel_x positive, decrement
        bne :++
            lda player_vel_x
            sec
            sbc #16
            sta player_vel_x
            cmp #0
            bpl :+
                ; negative, clamp to 0
                lda #0
                sta player_vel_x
            :
            jmp @end
        :
        ; Else if player_vel_x < 0, increment
        lda player_vel_x
        clc
        adc #16
        sta player_vel_x
        cmp #0
        beq :+
        bmi :+
            ; positive, clamp to 0
            lda #0
            sta player_vel_x
        :
    @end:

    ; Jump
    jsr btn_a
    cmp #0
    beq @fail_jump ; If jump not pressed, forget it
    lda jump_pressed_last_frame
    cmp #1
    beq @fail_jump ; If jump last frame, forget it

    ; Jump newly pressed this frame
    lda #249
    sta player_vel_y

    lda frame_counter
    sta last_frame_jumped

    @fail_jump:
    
    jsr btn_a
    sta jump_pressed_last_frame


    ; Gravity
    lda frame_counter
    sec
    sbc last_frame_jumped
    and #%00000001
    cmp #0
    bne :+
        lda player_vel_y
        clc
        adc #1    ; Add 2 to velocity
        sta player_vel_y
    :

    ; Apply Y velocity
    lda player_pos_y
    clc
    adc player_vel_y
    sta player_pos_y

    ; Apply X velocity
    ; Position
    lda player_vel_x
    and #%10000000
    cmp #0
    bne :+
        ; 0: right
        lda player_vel_x
        and #%01111111
        clc
        ror
        clc
        ror
        clc
        ror
        clc
        ror
        clc
        adc player_pos_x
        sta player_pos_x ; add pixels

        lda player_vel_x
        ; A contains signed subpixels

        clc
        adc player_subpos_x ; add subposition and velocity subpixels
        and #%00001111
        cmp player_subpos_x ; if A (result) is less than current subpos, overflow occurred
        sta player_subpos_x
        beq @end_add_subpos
        bcs @end_add_subpos ; so skip if subpos is smaller than result
            ldx player_pos_x
            inx
            stx player_pos_x
        @end_add_subpos:

        jmp :++
    :
        ; 1: left
        lda player_vel_x
        sec
        ror
        sec
        ror
        sec
        ror
        sec
        ror
        ora #%10000000
        clc
        adc #1
        clc
        adc player_pos_x
        sta player_pos_x ; add pixels


        lda player_vel_x
        ; A contains signed subpixels

        clc
        adc player_subpos_x ; add subposition and velocity subpixels
        and #%00001111
        cmp player_subpos_x ; if A (result) is greater than current subpos, underflow occurred
        sta player_subpos_x
        beq @end_sub_subpos
        bcc @end_sub_subpos ; so skip if subpos is bigger than result
            ldx player_pos_x
            dex
            stx player_pos_x
        @end_sub_subpos:
    :


    ; load x pos, filter blocks, check up and down
    
    lda player_vel_y
    cmp #0
    bmi :+ ; branch if moving up
        jsr downward_collision_check
        jsr left_collision_check
        jsr right_collision_check
        jsr upward_collision_check
        jmp :++
    :
        jsr upward_collision_check
        jsr left_collision_check
        jsr right_collision_check
        jsr downward_collision_check
    :

    rts

    ; Keep grounded
    cmp #200
    bcc :+
        lda #0
        sta player_vel_y
        lda #200
        sta player_pos_y
    :
    ; Bonk
    lda #8
    cmp player_pos_y
    bcc :+
        lda #0
        sta player_vel_y
        lda #8
        sta player_pos_y
    :

    rts

setup_background:
	lda $2002 ; reset latch
	lda #$20
	sta $2006
	lda #$00
	sta $2006
	; empty nametable
	lda #0
	ldy #30 ; 30 rows
	:
		ldx #32 ; 32 columns
		:
			sta $2007
			dex
			bne :-
		dey
		bne :--

    lda $2002
    lda #$20
    sta $2006
    lda #$00
    sta $2006
	ldy #0
	@row_start:
        ldx #0
        :
            txa
            pha

            ; index is (Y * 16) + X
            stx temp_x
            lda #16
            jsr mul_y
            clc
            adc temp_x
            tax

            lda level, X
            ldx #5
            jsr mul_x
            tax
            
            lda metatiles+0, X
            sta $2007
            lda metatiles+1, X
            sta $2007

            pla
            tax
            inx
            cpx #16
            bne :-

        ; again
        ldx #0
        :
            txa
            pha

            ; index is (Y * 16) + X
            stx temp_x
            lda #16
            jsr mul_y
            clc
            adc temp_x
            tax

            lda level, X
            ldx #5
            jsr mul_x
            tax

            lda metatiles+2, X
            sta $2007
            lda metatiles+3, X
            sta $2007

            pla
            tax
            inx
            cpx #16
            bne :-
		iny
        cpy #15
		bne @row_start


	; clear attributes
    ;lda #0
	;ldx #64 ; 64 bytes
	;:
    ;    lda #%11100100
;
	;	sta $2007
	;	dex
	;	bne :-

    lda #0
    sta $50

    ldy #0
    :
        tya
        pha

        ldx #0
        :
            tya
            pha
            txa
            pha

            tya
            asl
            asl
            asl
            asl
            asl
            sta temp_y

            txa
            asl

            clc
            adc temp_y

            tax

            lda level, X
            tay
            lda #5
            jsr mul_y
            tay
            lda metatiles+4, Y
            sta temp_x

            lda level+1, X
            tay
            lda #5
            jsr mul_y
            tay
            lda metatiles+4, Y
            asl
            asl
            adc temp_x
            sta temp_x

            lda level+16, X
            tay
            lda #5
            jsr mul_y
            tay
            lda metatiles+4, Y
            asl
            asl
            asl
            asl
            adc temp_x
            sta temp_x

            lda level+17, X
            tay
            lda #5
            jsr mul_y
            tay
            lda metatiles+4, Y
            asl
            asl
            asl
            asl
            asl
            asl
            adc temp_x
            sta $2007

            ldx $50
            inx
            stx $50

            sta $60, X

            pla
            tax
            pla
            tay
            inx
            cpx #8
            bne :-

        pla
        tay
        iny
        cpy #8
        bne :--

    rts

mul_x:
    cmp #0
    beq @zero
    cpx #0
    beq @zero

    sta temp_mul

    txa
    pha
    lda #0
    :
        clc
        adc temp_mul
        dex
        bne :-
    sta temp_mul
    pla
    tax
    lda temp_mul
    rts

    @zero:
    lda #0
    rts

mul_y:
    cmp #0
    beq @zero
    cpy #0
    beq @zero

    sta temp_mul

    tya
    pha
    lda #0
    :
        clc
        adc temp_mul
        dey
        bne :-
    sta temp_mul
    pla
    tay
    lda temp_mul
    rts

    @zero:
    lda #0
    rts

controller:
    lda #1
    sta $4016
    sta buttons
    lda #0
    sta $4016
    :
        lda $4016
        lsr
        rol buttons
        bcc :-

    rts

btn_right:
    lda buttons
    and #%00000001
    rts

btn_left:
    lda buttons
    and #%00000010
    lsr
    rts

btn_down:
    lda buttons
    and #%00000100
    lsr
    lsr
    rts

btn_up:
    lda buttons
    and #%00001000
    lsr
    lsr
    lsr
    rts

btn_start:
    lda buttons
    and #%00010000
    lsr
    lsr
    lsr
    lsr
    rts

btn_select:
    lda buttons
    and #%00100000
    lsr
    lsr
    lsr
    lsr
    lsr
    rts

btn_b:
    lda buttons
    and #%01000000
    lsr
    lsr
    lsr
    lsr
    lsr
    lsr
    rts

btn_a:
    lda buttons
    and #%10000000
    lsr
    lsr
    lsr
    lsr
    lsr
    lsr
    lsr
    rts

downward_collision_check:
    lda #0
    sta var_n
    @begin_check:
    lda player_pos_x
    clc
    adc scroll_x
    ldx var_n
    cpx #0
    bne :+
        sec
        sbc #5
        jmp :++
    :
        clc
        adc #4
    :
    ; divide by 16
    clc
    ror
    clc
    ror
    clc
    ror
    clc
    ror
    sta temp_x

    lda player_pos_y
    ; divide by 16
    clc
    adc #8
    clc
    ror
    clc
    ror
    clc
    ror
    clc
    ror
    tay

    lda #16
    jsr mul_y
    adc temp_x
    tax
    lda level, X
    cmp #0
    beq :+
        tya
        asl
        asl
        asl
        asl
        sec
        sbc #8
        cmp player_pos_y
        bcs :+
        sta player_pos_y
        lda #0
        sta player_vel_y
    :

    ldx var_n
    inx
    stx var_n
    cpx #1
    beq @begin_check

    rts

upward_collision_check:
    lda #0
    sta var_n
    @begin_check:
    lda player_pos_x
    clc
    adc scroll_x
    ldx var_n
    cpx #0
    bne :+
        sec
        sbc #5
        jmp :++
    :
        clc
        adc #4
    :
    ; divide by 16
    clc
    ror
    clc
    ror
    clc
    ror
    clc
    ror
    sta temp_x

    lda player_pos_y
    ; divide by 16
    sec
    sbc #8
    clc
    ror
    clc
    ror
    clc
    ror
    clc
    ror
    tay

    lda #16
    jsr mul_y
    adc temp_x
    tax
    lda level, X
    cmp #0
    beq :+
        iny
        tya
        asl
        asl
        asl
        asl
        clc
        adc #8
        cmp player_pos_y
        bcc :+
        sta player_pos_y
        lda #0
        sta player_vel_y
    :

    ldx var_n
    inx
    stx var_n
    cpx #1
    beq @begin_check

    rts

right_collision_check:
    lda #0
    sta var_n
    @begin_check:
    lda player_pos_x
    clc
    adc scroll_x
    ; divide by 16
    clc
    adc #8
    clc
    ror
    clc
    ror
    clc
    ror
    clc
    ror
    sta temp_x

    lda player_pos_y
    ldx var_n
    cpx #0
    bne :+
        sec
        sbc #7
        jmp :++
    :
        clc
        adc #6
    :
    ; divide by 16
    clc
    ror
    clc
    ror
    clc
    ror
    clc
    ror
    tay

    lda #16
    jsr mul_y
    adc temp_x
    tax
    lda level, X
    cmp #0
    beq :+
        lda temp_x
        asl
        asl
        asl
        asl
        sec
        sbc #8
        cmp player_pos_x
        bcs :+
        sta player_pos_x
        lda #0
        sta player_vel_x
    :

    ldx var_n
    inx
    stx var_n
    cpx #1
    beq @begin_check

    rts

left_collision_check:
    lda #0
    sta var_n
    @begin_check:
    lda player_pos_x
    clc
    adc scroll_x
    ; divide by 16
    sec
    sbc #8
    clc
    ror
    clc
    ror
    clc
    ror
    clc
    ror
    sta temp_x

    lda player_pos_y
    ldx var_n
    cpx #0
    bne :+
        sec
        sbc #7
        jmp :++
    :
        clc
        adc #6
    :
    ; divide by 16
    clc
    ror
    clc
    ror
    clc
    ror
    clc
    ror
    tay

    lda #16
    jsr mul_y
    adc temp_x
    tax
    lda level, X
    cmp #0
    beq :+
        lda temp_x
        clc
        adc #1
        asl
        asl
        asl
        asl
        clc
        adc #8
        cmp player_pos_x
        bcc :+
        sta player_pos_x
        lda #0
        sta player_vel_x
    :

    ldx var_n
    inx
    stx var_n
    cpx #1
    beq @begin_check

    rts