.include "m16def.inc"
.def tmp0 = r16
.def par0 = r17
.def par1 = r18
.def tmp1 = r19
.def tmp2 = r20
.def ret0 = r21
.def ret1 = r22
.def par2 = r23
.def par3 = r24
.def par4 = r25
.def ret2 = r26
.def heart = r27
.def dig0 = r28
.def dig1 = r29

init:
	; Initialize stack pointer
	ldi tmp0, HIGH(RAMEND)
	out SPH, tmp0
	ldi tmp0, LOW(RAMEND)
	out SPL, tmp0
	; Set data directions and default outputs for ports
	ldi tmp0, 0b11111111
	out DDRA, tmp0
	ldi tmp0, 0b11111111
	out DDRB, tmp0
	ldi tmp0, 0b11101110
	out DDRD, tmp0
	cbi PORTC, 0
	sbi PORTC, 6
	sbi DDRC, 0
	sbi DDRC, 1
	sbi DDRC, 6
	ldi tmp0, 0b00000100
	out PORTD, tmp0
	; Display all zeros on displays
	ldi par0, 0
	ldi par1, 0
	rcall display
	ldi par0, 1
	ldi par1, 0
	rcall display
	ldi par0, 2
	ldi par1, 0
	rcall display
	ldi par0, 3
	ldi par1, 0
	rcall display

	;ldi par0, 0
	;ldi par1, 0
	;ldi par2, 0x01
	;rcall eeprom_write
	;ldi par0, 1
	;ldi par2, 0x23
	;rcall eeprom_write
	;ldi par0, 3
	;ldi par1, 1
	;rcall display
;stop:
	;rjmp stop

	rcall write_code
	
	; Load code from EEPROM
	rcall load_code

	;lds par1, 1
	;ldi par0, 0
	;rcall display
;stop:
	;rjmp stop
	
	; Grab input once and but it on the stack
	; For delta bitvector
	rcall input
	push ret1
	push ret2

	; Initialize registers and stack top
	ldi heart, 0
	ldi dig0, 0x10
	ldi dig1, 0
	ldi tmp0, 0
	ldi tmp2, 0
	push tmp0
	push tmp0
	
	rjmp main

debug_heart:
	push par0
	push par1
	push par2
	inc heart
	ldi par0, 3
	mov par1, heart
	rcall display
	mov par0, heart
	rcall status
	ldi par0, 1
	ldi par1, 0xFF
	ldi par2, 0xFF
	rcall wait
	pop par2
	pop par1
	pop par0
	ret

main:
	ldi par0, 1
	ldi par1, 0xFF
	ldi par2, 0xFF
	rcall wait
	;rcall debug_heart
	; get current button states
	rcall input
	; detect raising edge on one of the buttons
	pop tmp0
	pop tmp1
	push ret1
	push ret0
	eor tmp0, ret0
	eor tmp1, ret1
	and tmp0, ret0
	and tmp1, ret1
	; button pressed, process
	mov par0, tmp0
	mov par1, tmp1
	rcall translate
	mov par0, tmp2
	mov par1, ret0
	rcall handle
	mov tmp2, ret0
	rjmp main

wait:
	push tmp0
	push tmp1
	push tmp2
	mov tmp0, par0
	mov tmp1, par1
	mov tmp2, par2
wait_loop:
	nop
	dec tmp0
	brne wait_loop
	mov tmp0, par0
	dec tmp1
	brne wait_loop
	mov tmp0, par0
	mov tmp1, par1
	dec tmp2
	brne wait_loop
	pop tmp2
	pop tmp1
	pop tmp0
	ret

write_code:
	push par0
	push par1
	push par2
	ldi par0, 0
	ldi par1, 0
	ldi par2, 0x01
	rcall eeprom_write
	ldi par0, 1
	ldi par2, 0x23
	rcall eeprom_write
	pop par2
	pop par1
	pop par0
	ret

; Load code from EEPROM and store in memory
; Parameters:
; None
; Returns:
; ret0 = Code digits 0-1
; ret1 = Code digits 2-3
load_code:
	push par0
	push par1
	; Digits 0-1
	ldi par0, 0
	ldi par1, 0
	rcall eeprom_read
	sts 0, ret0
	; Digits 2-3
	ldi par0, 1
	rcall eeprom_read
	sts 1, ret0
	pop par1
	pop par0
	ret

; Output bits to shift register.
; Parameters:
; par0 = Bits
; par1:par2 = Call address to set 0
; par3:par4 = Call address to set 1
; Returns:
; None
shift_out:
	push par0
	push tmp0
	ldi tmp0, 8
shift_out_loop:
	sbrs par0, 7
	rjmp shift_out_low
	rjmp shift_out_high
shift_out_low:
	mov r31, par1
	mov r30, par2
	icall
	rjmp shift_out_end
shift_out_high:
	mov r31, par3
	mov r30, par4
	icall
	rjmp shift_out_end
shift_out_end:
	lsl par0
	dec tmp0
	brne shift_out_loop
	pop tmp0
	pop par0
	ret

; Input bits from shift register.
; Parameters:
; par0:par1 = Call address for one clock cycle
; par2:par3 = Call address to read serial shift in bit
; Returns:
; ret0 = Read bits
shift_in:
	push tmp0
	ldi tmp0, 8
	ldi ret0, 0
shift_in_loop:
	lsl ret0
	mov r31, par2
	mov r30, par3
	icall
	or ret0, ret2
	mov r31, par0
	mov r30, par1
	icall
	dec tmp0
	brne shift_in_loop
	pop tmp0
	ret

; Write to EEPROM
; Parameters:
; par0:par1 = Address
; par2 = Value
; Returns:
; None
eeprom_write:
	sbic EECR, EEWE
	rjmp eeprom_write
	out EEARL, par0
	out EEARH, par1
	out EEDR, par2
	sbi EECR, EEMWE
	sbi EECR, EEWE
	ret

; Read from EEPROM
; Parameters:
; par0:par1 = Address
; Returns:
; ret0 = Value
eeprom_read:
	sbic EECR, EEWE
	rjmp eeprom_read
	out EEARL, par0
	out EEARH, par1
	sbi EECR, EERE
	in ret0, EEDR
	ret

; Output the status
; Parameters:
; par0 = Status bits
; Returns:
; None
status:
	push par1
	push par2
	push par3
	push par4
	ldi par1, HIGH(status_low)
	ldi par2, LOW(status_low)
	ldi par3, HIGH(status_high)
	ldi par4, LOW(status_high)
	rcall shift_out
	pop par1
	pop par2
	pop par3
	pop par4
	ret
status_low:
	push tmp2
	in tmp2, PORTD
	cbr tmp2, 6
	rjmp status_clk
status_high:
	push tmp2
	in tmp2, PORTD
	sbr tmp2, 6
	rjmp status_clk
status_clk:
	cbr tmp2, 7
	out PORTD, tmp2
	sbr tmp2, 7
	out PORTD, tmp2
	pop tmp2
	ret

; Display the digit on a given display.
; Parameters:
; par0 = Display number
; par1 = Digit
; Returns:
; None
display:
	push par1
	push tmp0
	andi par1, 0x0F
	ldi ZL, LOW(display_map)
	ldi ZH, HIGH(display_map)
	ldi tmp0, 0
	add ZL, par0
	adc ZH, tmp0
	icall
	pop tmp0
	pop par1
	ret
display_map:
	rjmp display_0
	rjmp display_1
	rjmp display_2
	rjmp display_3
display_0:
	in tmp0, PORTA
	andi tmp0, 0xF0
	or tmp0, par1
	out PORTA, tmp0
	ret
display_1:
	in tmp0, PORTB
	andi tmp0, 0xF0
	or tmp0, par1
	out PORTB, tmp0
	ret
display_2:
	in tmp0, PORTA
	andi tmp0, 0x0F
	lsl par1
	lsl par1
	lsl par1
	lsl par1
	or tmp0, par1
	out PORTA, tmp0
	ret
display_3:
	in tmp0, PORTB
	andi tmp0, 0x0F
	lsl par1
	lsl par1
	lsl par1
	lsl par1
	or tmp0, par1
	out PORTB, tmp0
	ret

; Sets the corresponding digit number.
; Parameters:
; par0 = Digit number (0-3)
; par1 = Digit (0x00-0x0F)
set_digit:
	push tmp0
	push par1
	andi par1, 0x0F
	ldi tmp0, 0
	ldi ZL, LOW(set_digit_n)
	ldi ZH, HIGH(set_digit_n)
	add ZL, par0
	adc ZH, tmp0
	icall
	pop par1
	pop tmp0
	ret
set_digit_n:
	rjmp set_digit_0
	rjmp set_digit_1
	rjmp set_digit_2
	rjmp set_digit_3
set_digit_0:
	lsl par1
	lsl par1
	lsl par1
	lsl par1
	andi dig0, 0x0F
	or dig0, par1
	ret
set_digit_1:
	andi dig0, 0xF0
	or dig0, par1
	ret
set_digit_2:
	lsl par1
	lsl par1
	lsl par1
	lsl par1
	andi dig1, 0x0F
	or dig1, par1
	ret
set_digit_3:
	andi dig1, 0xF0
	or dig1, par1
	ret

; Gets the corresponding digit number.
; Parameters:
; par0 = Digit number (0-3)
; Returns:
; ret0 = Digit
get_digit:
	push tmp0
	ldi tmp0, 0
	ldi ZL, LOW(get_digit_n)
	ldi ZH, HIGH(get_digit_n)
	add ZL, par0
	adc ZH, tmp0
	icall
	pop tmp0
	ret
get_digit_n:
	rjmp get_digit_0
	rjmp get_digit_1
	rjmp get_digit_2
	rjmp get_digit_3
get_digit_0:
	mov ret0, dig0
	lsr ret0
	lsr ret0
	lsr ret0
	lsr ret0
	ret
get_digit_1:
	mov ret0, dig0
	andi ret0, 0x0F
	ret
get_digit_2:
	mov ret0, dig1
	lsr ret0
	lsr ret0
	lsr ret0
	lsr ret0
	ret
get_digit_3:
	mov ret0, dig1
	andi ret0, 0x0F
	ret

; Read the input buttons.
; Parameters:
; None
; Returns:
; ret0 = 0852*741
; ret1 = DCBA#963
input:
	; Write selection rows
	ldi par0, 1
input_loop:
	ldi par1, HIGH(input_write_low)
	ldi par2, LOW(input_write_low)
	ldi par3, HIGH(input_write_high)
	ldi par4, LOW(input_write_high)
	rcall shift_out
	rjmp input_read
input_write_low:
	cbi PORTD, 3
	rcall input_write_clk
	ret
input_write_high:
	sbi PORTD, 3
	rcall input_write_clk
	ret
input_read:
	push tmp0
	; clear
	cbi PORTC, 6
	push par0
	push par1
	push par2
	ldi par0, 1
	ldi par1, 1
	ldi par2, 1
	rcall wait
	sbi PORTC, 6
	ldi par0, 1
	ldi par1, 1
	ldi par2, 1
	rcall wait
	; load
	cbi PORTD, 2
	ldi par0, 1
	ldi par1, 1
	ldi par2, 1
	rcall wait
	rcall input_read_clk
	sbi PORTD, 2
	ldi par0, 1
	ldi par1, 1
	ldi par2, 1
	rcall wait
	pop par2
	pop par1
	pop par0
	; shift in
	push par0
	ldi par0, HIGH(input_read_clk)
	ldi par1, LOW(input_read_clk)
	ldi par2, HIGH(input_read_bit)
	ldi par3, LOW(input_read_bit)
	push ret0
	rcall shift_in
	mov tmp0, ret0
	pop ret0
	pop par0
	;lsr tmp0
	;lsr tmp0
	;lsr tmp0
	;lsr tmp0
	; par0 = row
	; tmp0 = col
	sbrc par0, 0
	rjmp input_read_row_0
	sbrc par0, 1
	rjmp input_read_row_1
	sbrc par0, 2
	rjmp input_read_row_2
	sbrc par0, 3
	rjmp input_read_row_3
input_write_clk:
	cbi PORTC, 1
	push par0
	push par1
	push par2
	ldi par0, 1
	ldi par1, 1
	ldi par2, 1
	rcall wait
	sbi PORTC, 1
	ldi par0, 1
	ldi par1, 1
	ldi par2, 1
	rcall wait
	pop par2
	pop par1
	pop par0
	ret
input_read_clk:
	cbi PORTD, 5
	push par0
	push par1
	push par2
	ldi par0, 1
	ldi par1, 1
	ldi par2, 1
	rcall wait
	sbi PORTD, 5
	ldi par0, 1
	ldi par1, 1
	ldi par2, 1
	rcall wait
	pop par2
	pop par1
	pop par0
	ret
input_read_bit:
	ldi ret2, 0
	sbic PIND, 4
	ldi ret2, 1
	ret
input_read_row_0:
	andi ret0, 0xF0
	or ret0, tmp0
	rjmp input_end
input_read_row_1:
	andi ret0, 0x0F
	lsl tmp0
	lsl tmp0
	lsl tmp0
	lsl tmp0
	or ret0, tmp0
	rjmp input_end
input_read_row_2:
	andi ret1, 0xF0
	or ret1, tmp0
	rjmp input_end
input_read_row_3:
	andi ret1, 0x0F
	lsl tmp0
	lsl tmp0
	lsl tmp0
	lsl tmp0
	or ret1, tmp0
	rjmp input_end
input_end:
	pop tmp0
	lsl par0
	andi par0, 0x0F
	cpi par0, 0
	brne input_loop_j
	ret
input_loop_j:
	rjmp input_loop

; Translate input bits to keycode.
; Parameters:
; par0 = Input bits 7..0
; par1 = Input bits 15..8
; Returns:
; ret0 = Translated keycode
translate:
	push par0
	push par1
	push tmp0
	ldi tmp0, 16
translate_loop:
	sbrs par0, 0
	rjmp translate_low
	rjmp translate_high
translate_low:
	lsr par0
	lsr par1
	brcc pc+2
	ori par0, 0x80
	dec tmp0
	brne translate_loop
	ldi ret0, 0xF0
	pop tmp0
	pop par1
	pop par0
	ret
translate_high:
	push tmp0
	push tmp1
	ldi ZL, LOW(translate_map << 1)
	ldi ZH, HIGH(translate_map << 1)
	dec tmp0
	ldi tmp1, 0
	add ZL, tmp0
	adc ZH, tmp1
	lpm ret0, Z
	pop tmp1
	pop tmp0
	pop tmp0
	pop par1
	pop par0
	ret
translate_map:
	.db 0x0D, \
		0x0C, \
		0x0B, \
		0x0A, \
		0x0F, \
		9, \
		6, \
		3, \
		0, \
		8, \
		5, \
		2, \
		0x0E, \
		7, \
		4, \
		1
; ret0 = 0852*741
; ret1 = DCBA#963

; Handles the given key input.
; Parameters:
; par0 = state
; par1 = keycode
; Returns:
; ret0 = new state
; Descriptions:
; state = 0bCSSSSSDD
; C = Blinky clock
; S = State information
; 0 = reserved
; D = Current display
.equ HANDLE_STATE_MASK = 0b01111100
.equ HANDLE_STATE_AUTH = (0 << 2)
.equ HANDLE_STATE_AUTHED = (1 << 2)
.equ HANDLE_STATE_CHANGE = (2 << 2)
handle:
	push tmp0
	push tmp1
	mov ret0, par0
	mov tmp0, par0
	andi tmp0, HANDLE_STATE_MASK
	lsr tmp0
	lsr tmp0
	ldi ZL, LOW(handle_map)
	ldi ZH, HIGH(handle_map)
	ldi tmp1, 0
	add ZL, tmp0
	adc ZH, tmp1
	ijmp
handle_map:
	rjmp handle_auth
	rjmp handle_authed
	rjmp handle_change
handle_blink:
	sbrs par0, 7
	rjmp handle_blink_low
	rjmp handle_blink_high
handle_blink_low:
	push par0
	push par1
	andi par0, 0b00000011
	ldi par1, 0x0F
	rcall display
	pop par1
	pop par0
	ori ret0, 0b10000000
	ret
handle_blink_high:
	push par0
	push par1
	push ret0
	andi par0, 0b00000011
	rcall get_digit
	mov par1, ret0
	rcall display
	pop ret0
	pop par1
	pop par0
	andi ret0, 0b01111111
	ret
handle_auth:
	rcall handle_blink
	cpi par1, 0xF0
	breq handle_auth_no_press
	cpi par1, 0x0A
	breq handle_auth_submit
	push par0
	andi par0, 0b00000011
	rcall set_digit
	rcall display
	inc par0
	sbrc par0, 2
	andi par0, 0b00000011
	andi ret0, 0b11111100
	or ret0, par0
	pop par0
	rjmp handle_end
handle_auth_no_press:
	rjmp handle_end
handle_auth_submit:
	push par0
	push ret0
	push tmp0
	push tmp1
	push tmp2
	lds tmp0, 0
	lds tmp1, 1
	; Compare digit 0
	mov tmp2, tmp0
	lsr tmp2
	lsr tmp2
	lsr tmp2
	lsr tmp2
	ldi par0, 0
	rcall get_digit
	cp tmp2, ret0
	brne handle_compare_end
	; Compare digit 1
	mov tmp2, tmp0
	andi tmp2, 0x0F
	ldi par0, 1
	rcall get_digit
	cp tmp2, ret0
	brne handle_compare_end
	; Compare digit 2
	mov tmp2, tmp1
	lsr tmp2
	lsr tmp2
	lsr tmp2
	lsr tmp2
	ldi par0, 2
	rcall get_digit
	cp tmp2, ret0
	brne handle_compare_end
	; Compare digit 3
	mov tmp2, tmp1
	andi tmp2, 0x0F
	ldi par0, 3
	rcall get_digit
	cp tmp2, ret0
	brne handle_compare_end
	; All digits match
	pop tmp2
	pop tmp1
	pop tmp0
	pop ret0
	pop par0
	andi ret0, HANDLE_STATE_MASK
	ori ret0, HANDLE_STATE_AUTHED
	rjmp handle_end
handle_compare_end:
	pop tmp2
	pop tmp1
	pop tmp0
	pop par0
	rjmp handle_end
handle_authed:
	push par0
	push par1
	ldi par0, 0
	ldi par1, 1
	rcall display
	ldi par0, 1
	ldi par1, 3
	rcall display
	ldi par0, 2
	ldi par1, 3
	rcall display
	ldi par0, 3
	ldi par1, 7
	rcall display
	pop par1
	pop par0
	rjmp handle_end
handle_change:
	rjmp handle_end
handle_end:
	pop tmp1
	pop tmp0
	ret