/*
 * timer.asm
 *
 *  Created: 26/03/2019 2:04:19 PM
 *   Author: Peter Ph
 */

 .include "m2560def.inc"
 .dseg

 .cseg

 .def temp = r16
 .def leds = r17
 .def r_floor = r18
 .def r_direction = r19
 .def pattern = r20
 .def pattern1 = r21
 .def curr_floor = r22
 .def flippy = r23
 .def dest_floor = r24
 .def temp1 = r1
 nums: .db 3, 4, 7, 9

 .equ floor_number = 2
 .equ direction = 1


 .macro clear
	ldi YL, low(@0)
	ldi YH, high(@0)
	clr temp
	st Y+, temp
	st Y, temp
.endmacro


 .dseg
 BlinkCounter:
	.byte 2
TempCounter:
	.byte 2

.cseg
	jmp RESET
	jmp DEFAULT
	jmp DEFAULT
.org OVF0addr
	jmp Timer0OVF

DEFAULT: reti

 RESET: ldi temp, high(RAMEND)
		out SPH, temp
		ldi temp, low(RAMEND)
		out SPL, temp

		ldi ZL, low(nums<<1)
		ldi ZH, high(nums<<1)

		lpm dest_floor, Z+

		ldi r_floor, floor_number
		ldi r_direction, direction
		ldi flippy, 1

		ldi pattern, 0
		clr temp

get_pattern:
		cpi temp, floor_number
		brsh got_pattern
		lsl pattern
		inc pattern
		inc temp
		rjmp get_pattern
got_pattern:
		ldi pattern1, 0
get_pattern1:
		cpi r_floor, 9
		brne check_ten
		ldi pattern1, 1
check_ten:
		cpi r_floor, 10
		brne got_pattern1
		ldi pattern1, 3
got_pattern1:

		mov curr_floor, r_floor

		ser temp
		out DDRC, temp
		out DDRG, temp

		out PORTC, pattern
		out PORTG, pattern1

		rjmp main
 Timer0OVF:
	in temp, SREG
	push temp
	push YH
	push YL
	push r25
	push r24


	lds r24, TempCounter
	lds r25, TempCounter+1
	adiw r25:r24, 1

; check if we want to wait for 5s or 2s
	cpi curr_floor, 5
	brne two_sec
	cpi r24, low(39060)
	ldi temp, high(39060)
	cpc r25, temp
	brne wait
	rjmp start

two_sec:
	cpi r24, low(15624)
	ldi temp, high(15624)
	cpc r25, temp
	brne NotSecond
	// if a second has passed lets change it up nigger
start:
	;out PORTC, pattern
	;out PORTG, pattern1

	cpi r_direction, 1
	brne going_down
going_up:
	cpi curr_floor, 10
	brne continue_up
// change dir
	ldi r_direction, 0
	rjmp start
continue_up:
	lsl pattern
	inc pattern

	cpi curr_floor, 8
	brne check_if_ten
	ldi pattern1, 1
	rjmp done_up
check_if_ten:
	cpi curr_floor, 9
	brne done_up
	ldi pattern1, 3
	rjmp done_up
change_direction:
	com r_direction
	rjmp start
going_down:
	cpi curr_floor, 0
	brne keep_going_down
	ldi r_direction, 1
	rjmp start
keep_going_down:
	cpi curr_floor, 10
	brne check_if_nine
	ldi pattern1, 1
	rjmp done_down
check_if_nine:
	cpi curr_floor, 9
	brne continue_down
	ldi pattern1, 0
	rjmp done_down
continue_down:
	lsr pattern
	rjmp done_down
done_up:
	inc curr_floor
	rjmp done
done_down:
	dec curr_floor
done:
	clear TempCounter
//----------------------------------------------------------
	rjmp EndIF
NotSecond:
	sts TempCounter, r24
	sts TempCounter+1, r25
	rjmp EndIF
wait:
	sts TempCounter, r24
	sts TempCounter+1, r25

start_blink:
	lds r24, BlinkCounter
	lds r25, BlinkCounter+1
	adiw r25:r24, 1

	cpi r24, low(3750)
	ldi temp, high(3750)
	cpc r25, temp
	brne NotBlink
	;cp curr_floor, dest_floor
	;brne done_blink
	;lpm dest_floor, Z+
	cpi flippy, 1
	brne flip_zero
	clr temp
	out PORTC, temp
	out PORTG, temp
	ldi flippy, 0
	rjmp done_blink
/*flip_zero:
	mov temp, pattern
	mov temp1, pattern1
	ldi flippy, 1
	out PORTC, temp
	out PORTG, temp1
	rjmp done_blink*/

NotBlink:
	sts BlinkCounter, r24
	sts BlinkCounter+1, r25
	rjmp EndIF
done_blink:
	clear BlinkCounter
	rjmp EndIF

EndIF:
	out PORTC, pattern
	out PORTG, pattern1

	pop r24
	pop r25
	pop YL
	pop YH
	pop temp
	out SREG, temp
	reti

main:
	;out PORTC, pattern
	clear TempCounter
	clear BlinkCounter
	ldi temp, 0b00000000
	out TCCR0A, temp
	ldi temp, 0b00000010
	out TCCR0B, temp
	ldi temp, 1<<TOIE0
	sts TIMSK0, temp
	sei
loop: rjmp loop