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

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, 0b11000000
  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

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

; Output the status
; par0 = Status bits
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
; par0 = Display number
; par1 = Digit
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