.include "ATxmega128A1def.inc"
.def tempreg=r16
.def counterreg=r17
.def outputreg=r22

.equ DIR_PORTA_MASK=0xA8
.equ DIR_PORTB_MASK=0x02
.equ KEY_INTERRUPT_MASK=0x04


.org 0x0
	rjmp RESET

.org PORTB_INT0_vect
	rjmp ISR 

.org INT_VECTORS_SIZE

RESET:
	; init section 
	; init counter variable 
	ser outputreg

	; initialize 32Mhz clock 
	call SET_CLOCK_32MHZ
	call INIT_INTR
 
	; init PORTs
	; init PORTA (LEDs)
	ldi tempreg,DIR_PORTA_MASK
	sts PORTA_DIR,tempreg

	; init PORTB
	ldi tempreg,DIR_PORTB_MASK
	sts PORTB_DIR,tempreg

	; set pull up resitors for PORTB
	; because there're the keys we need
	; additionally we have to set the right 
	; input/sense configuration 
	; we want to sense every falling edge
	ldi tempreg,PORT_OPC_PULLUP_gc | PORT_ISC_FALLING_gc
	sts PORTB_PIN0CTRL,tempreg
	sts PORTB_PIN2CTRL,tempreg

	; init LEDs
	; we do not want them be lit from the beginning 
	ser tempreg 
	sts PORTA_OUT,tempreg
	sts PORTB_OUT,tempreg

	; global interrupt flag
	sei

MAIN:
	call READKEYS
	rjmp MAIN

READKEYS:
	; check if keys are pressed
	lds tempreg,PORTB_IN
	
	; is SW1 pressed?
	sbrs tempreg,2
	call SW1_PROC
	
	sbrs tempreg,0
	call SW2_PROC
	ret

INIT_EVSYS:
	; to be implemented...
	ret

INIT_INTR:
	; activate port interrupt
	ldi tempreg,PORT_INT0LVL_LO_gc
	sts PORTB_INTCTRL,tempreg

	; select pin2
	ldi tempreg,KEY_INTERRUPT_MASK
	sts PORTB_INT0MASK,tempreg

	; set PMIC
	ldi tempreg,PMIC_LOLVLEN_bm
	sts PMIC_CTRL,tempreg

	ret

SW1_PROC:
	; wait ...
	call DELAY_100MS

	; set LEDs
	; modifying register "output" to set LEDs
	sbrc counterreg,0
	cbr outputreg,0x08

	sbrc counterreg,1
	cbr outputreg,0x20

	sbrc counterreg,2
	cbr outputreg,0x80

	sbrc counterreg,3
	cbr outputreg,0x02

	sts PORTA_OUT,outputreg
	sts PORTB_OUT,outputreg

	call DELAY_1S

	call CLEAR

	ret

SW2_PROC:
	; wait ...
	call DELAY_100MS

	; init -> Z-Pointer
	ldi ZL, low(TCC0_CNT<<1)
	ldi ZH, high(TCC0_CNT<<1)

	lpm tempreg,Z
	
	sbrc tempreg,0
	cbr outputreg,0x08

	sbrc tempreg,1
	cbr outputreg,0x20

	sbrc tempreg,2
	cbr outputreg,0x80

	sbrc tempreg,3
	cbr outputreg,0x02

	sts PORTA_OUT,outputreg
	sts PORTB_OUT,outputreg

	call DELAY_1S

	call CLEAR

	ret

CLEAR:
	; clr LEDs after 1s and 
	; jump back to MAIN
	; new operation is possible

	; reset counter
	clr counterreg
	ldi tempreg,0x00
	sts TCC0_CNT,tempreg
	sts TCC0_CNT+1,tempreg

	ser outputreg
	sts PORTA_OUT,outputreg
	sts PORTB_OUT,outputreg
	ret

SET_CLOCK_32MHZ:
	; configure XOSC 
	; configure with 2 -9 MHz and 1KCLK 
	ldi tempreg,OSC_FRQRANGE_2TO9_gc | OSC_XOSCSEL_XTAL_1KCLK_gc
	sts OSC_XOSCCTRL,tempreg
	
	; configure PLL
	; PLL with factor 4 (constant PLLFAC2 writes factor 4) 
	; set source fpr PLL -> external oscillator
	ldi tempreg,OSC_PLLSRC_XOSC_gc | OSC_PLLFAC2_bm
	sts OSC_PLLCTRL,tempreg
	
	; enable PLL and XTAL
	ldi tempreg,OSC_PLLEN_bm | OSC_XOSCEN_bm | OSC_RC2MEN_bp
	sts OSC_CTRL,tempreg
STATUSLOOP:
	; PLL must be ready 
	; checking PLLEN bit
	lds tempreg,OSC_STATUS ; loading state
	sbrs tempreg,OSC_PLLRDY_bp 
	
	rjmp STATUSLOOP
	
	; disable change protection 
	ldi tempreg,CCP_IOREG_gc
	sts CPU_CCP,tempreg ; release protection for 4 cycles
	
	ldi tempreg,CLK_SCLKSEL_PLL_gc
	sts CLK_CTRL,tempreg

	ret

DELAY_100MS:
	ldi r18,0x30
DELAY_100MS_1:
	ldi r19,0xFF
DELAY_100MS_2:
	ldi r20,0xFF
DELAY_100MS_3:
	dec r20
	brne DELAY_100MS_3

	dec r19
	brne DELAY_100MS_2

	dec r18
	brne DELAY_100MS_1

	ret

DELAY_1S:
	ldi r21,0x0A
DELAY_1S_1:
	call DELAY_100MS
	dec r21 
	brne DELAY_1S_1

	ret

ISR:
	push tempreg
	lds tempreg,CPU_SREG
	push tempreg

	; counterreg will be only incremented 
	; when counterreg is below 16
	ldi tempreg,0x0F
	cpse tempreg,counterreg
	inc counterreg

	pop tempreg
	sts CPU_SREG,tempreg
	pop tempreg

	reti