; The first half of the ROM will not be used, so PAD it
FILLVALUE $FF
ORG $0000
PAD $8000

CONTROL_READ EQU $4000
LCD_INS EQU $6000
LCD_DAT EQU $6001
DELAY_CNT EQU $0010
CONTROL_STR EQU $0011

BTN_UP EQU #$80
BTN_DOWN EQU #$40
BTN_LEFT EQU #$20
BTN_RIGHT EQU #$10
BTN_A EQU #$08
BTN_B EQU #$04

Init:
	; Initialize 4 times to be sure
	LDA #$38
	STA LCD_INS
	JSR WaitLcd
	JSR Delay

	LDA #$38
	STA LCD_INS
	JSR WaitLcd
	JSR Delay
	
	LDA #$38
	STA LCD_INS
	JSR WaitLcd
	JSR Delay
	
	LDA #$38
	STA LCD_INS
	JSR WaitLcd
	JSR Delay
	

	; On/Off control
	LDA #$0C
	STA LCD_INS
	JSR WaitLcd
	JSR Delay

	; Clear display
	LDA #$01
	STA LCD_INS
	JSR WaitLcd
	JSR Delay

	; Entry Mode Set
	LDA #$06
	STA LCD_INS
	JSR WaitLcd
	JSR Delay

	; Set DDRAM Address
	LDA #$80
	STA LCD_INS
	JSR WaitLcd
	JSR Delay

	; Write intro message
	JSR WriteMessage
	
	; Print other information below
	LDA #11000101b
	STA LCD_INS
	JSR WaitLcd

	LDA #'U'
	STA LCD_DAT
	JSR WaitLcd
	LDA #'D'
	STA LCD_DAT
	JSR WaitLcd
	LDA #'L'
	STA LCD_DAT
	JSR WaitLcd
	LDA #'R'
	STA LCD_DAT
	JSR WaitLcd
	LDA #'A'
	STA LCD_DAT
	JSR WaitLcd
	LDA #'B'
	STA LCD_DAT
	JSR WaitLcd

MainLoop:
	; Grab the data from the controller and save it
	LDA CONTROL_READ
	STA CONTROL_STR

	; Move to the start of display RAM
	LDA #10000101b
	STA LCD_INS
	JSR WaitLcd

	; Analyze each bit of the controller data
	; Up
	LDA BTN_UP
	AND CONTROL_STR
	BEQ UP_false
	LDA #'1'
	JMP UP_end
	UP_false:
	LDA #'0'
	UP_end:
	STA LCD_DAT
	JSR WaitLcd
	
	; Down
	LDA BTN_DOWN
	AND CONTROL_STR
	BEQ DOWN_false
	LDA #'1'
	JMP DOWN_end
	DOWN_false:
	LDA #'0'
	DOWN_end:
	STA LCD_DAT
	JSR WaitLcd
	
	; Left
	LDA BTN_LEFT
	AND CONTROL_STR
	BEQ LEFT_false
	LDA #'1'
	JMP LEFT_end
	LEFT_false:
	LDA #'0'
	LEFT_end:
	STA LCD_DAT
	JSR WaitLcd
	
	; Right
	LDA BTN_RIGHT
	AND CONTROL_STR
	BEQ RIGHT_false
	LDA #'1'
	JMP RIGHT_end
	RIGHT_false:
	LDA #'0'
	RIGHT_end:
	STA LCD_DAT
	JSR WaitLcd
	
	; A
	LDA BTN_A
	AND CONTROL_STR
	BEQ A_false
	LDA #'1'
	JMP A_end
	A_false:
	LDA #'0'
	A_end:
	STA LCD_DAT
	JSR WaitLcd
	
	; B
	LDA BTN_B
	AND CONTROL_STR
	BEQ B_false
	LDA #'1'
	JMP B_end
	B_false:
	LDA #'0'
	B_end:
	STA LCD_DAT
	JSR WaitLcd

	JSR Delay
	JSR Delay
	JSR Delay
	JSR Delay
	
	JMP MainLoop

KillLoop:
	NOP
	NOP
	JMP KillLoop

; Wait until LCD is not busy
WaitLcd:
	LDA #$80
	BIT LCD_INS
	BNE WaitLcd
	RTS

Delay:
	LDA #$00
	STA DELAY_CNT
Delay_1:
	ADC #$01
	CMP #$00
	BNE Delay_1
	INC DELAY_CNT
	LDA #$10
	CMP DELAY_CNT
	BNE Delay_1
	RTS

WriteMessage:
	LDY #$00
WriteMessage_0:
	LDA (MSG),Y
	CMP #$00
	BEQ WriteMessage_End
	STA LCD_DAT
	JSR WaitLcd
	INY
	JMP WriteMessage_0
WriteMessage_End:
	RTS

MSG
	DB "Val: "
	DB $00

; At the very end (almost), we need to declare where the PC should be initialized at.
; For our purposes, we'll start it at the beginning of the ROM area.
ORG $FFFC
DB $00, $80, $00, $00