.include "tn85def.inc"
.def tmp0 = r16
.def tmp1 = r17
.def tmp2 = r18
.def tmp3 = r19

; Interrupt vectors
.org 0x0000
interrupts:
	rjmp init
	.org ADCCaddr
	rjmp adc_isr

.org INT_VECTORS_SIZE
init:
	; Initialize stack pointer
	ldi tmp0, HIGH(RAMEND)
	out SPH, tmp0
	ldi tmp0, LOW(RAMEND)
	out SPL, tmp0
	; Set pin directions
	ldi tmp0, 0b00010010
	out DDRB, tmp0
	; Set default outputs
	;ldi tmp0, 0b00000010
	;out PORTB, tmp0
	; Initialize timer 0B
	ldi tmp0, 0b00100011
	out TCCR0A, tmp0
	ldi tmp0, 0b00000001
	out TCCR0B, tmp0
	; Initialize timer 1B
	ldi tmp0, 0b00000001
	out TCCR1, tmp0
	ldi tmp0, 0b01100000
	out GTCCR, tmp0
	; Enable interrupts
	ldi tmp0, 0b10000000
	out SREG, tmp0
	; Initialize ADC
	ldi tmp0, 0b01000001
	out ADMUX, tmp0
	ldi tmp0, 0b10101000
	out ADCSRA, tmp0
	; Start ADC conversion
	sbi ADCSRA, ADSC

main:
	rjmp main

adc_isr:
	push tmp0
	push tmp1
	; Read out ADC
	in tmp0, ADCL
	in tmp1, ADCH
	; Substract offset
	subi tmp0, LOW(638)
	sbci tmp1, HIGH(638)
	; Normalize to 8 bit for timer
	lsr tmp1
	ror tmp0
	lsr tmp1
	ror tmp0
	; Set as timer compare
	out OCR0B, tmp0
	com tmp0
	out OCR1B, tmp0
	pop tmp1
	pop tmp0
	reti