.include "m16def.inc"
.def tmp0 = r16
.def par0 = r17
.def par1 = r18
.def cnt = r19
.def tmp1 = r20
.def tmp2 = r21
.def ret0 = r22
.def ret1 = r23

init:
  ldi tmp0, HIGH(RAMEND)
  out SPH, tmp0
  ldi tmp0, LOW(RAMEND)
  ldi par0, 0xFF
  ldi par1, 0xFF
  rcall wait
  out SPL, tmp0
  ldi tmp0, 0b11111111
  out DDRA, tmp0
  ldi tmp0, 0b11111111
  out DDRB, tmp0
  ldi tmp0, 0b11101100
  out DDRD, tmp0
  ldi tmp0, 0b00000000
  out PORTD, tmp0

	ldi par0, 0
	ldi par1, 0
	rcall display
	ldi par0, 1
	ldi par1, 1
	rcall display
	ldi par0, 2
	ldi par1, 2
	rcall display
	ldi par0, 3
	ldi par1, 3
	rcall display

main:
	ldi par0, 0b11111111
	rcall status
	rjmp [tmp0]

wait:
  mov tmp0, par0
  dec par0
  nop
  brne wait
  mov par0, tmp0
  dec par1
  nop
  brne wait
  ret

; Output the status
; Parameters:
; par0 = Status bits
; Returns:
; None
status:
	ldi tmp0, 1
status_loop:
	mov tmp1, par0
	and tmp1, tmp0
	cp tmp0, tmp1
	breq status_set
	ldi tmp1, 0b00111111
	out PORTD, tmp1
	ldi tmp1, 0b10111111
	out PORTD, tmp1
	rjmp status_end
status_set:
	ldi tmp1, 0b01111111
	out PORTD, tmp1
	ldi tmp1, 0b11111111
	out PORTD, tmp1
	rjmp status_end
status_end:
	lsl tmp0
	cpi tmp0, 0
	brne status_loop
	ret

; Display the digit on a given display.
; Parameters:
; par0 = Display number
; par1 = Digit
; Returns:
; None
display:
	mov tmp0, par1
	andi tmp0, 0x0F
	cpi par0, 0
	breq display_0
	cpi par0, 1
	breq display_1
	cpi par0, 2
	breq display_2
	cpi par0, 3
	breq display_3
	ret
display_0:
	in tmp1, PORTA
	andi tmp1, 0xF0
	or tmp1, tmp0
	out PORTA, tmp1
	ret
display_1:
	in tmp1, PORTB
	andi tmp1, 0xF0
	or tmp1, tmp0
	out PORTB, tmp1
	ret
display_2:
	in tmp1, PORTA
	andi tmp1, 0x0F
	lsl tmp0
	lsl tmp0
	lsl tmp0
	lsl tmp0
	or tmp1, tmp0
	out PORTA, tmp1
	ret
display_3:
	in tmp1, PORTB
	andi tmp1, 0x0F
	lsl tmp0
	lsl tmp0
	lsl tmp0
	lsl tmp0
	or tmp1, tmp0
	out PORTB, tmp1
	ret

; Read the input buttons.
; Parameters:
; None
; Returns:
; ret0 = 123A456B
; ret1 = 789C*0#D
input:
	; Write selection rows
	ldi tmp0, 1
	ldi tmp1, 1
input_loop:
	mov tmp2, tmp0
	and tmp2, tmp1
	cp tmp1, tmp2
	breq input_set