;
; Switch input, and debounce.  
;
;	Results are in sw_2 (current state)
;	Transistion is in sw_3 (only good for one interrupt cycle)
;
; Define variables
.equ		SWITCH = PORTB
.equ		SWDDR	= DDRB
.equ		RDSWI	= PINB
.set		PORTB_INIT = PORTD_INIT + (1<<sw_1_b) + (1<<sw_2_b) + (1<<sw_3_b) + (1<<sw_4_b) + (1<<sw_5_b) + (1<<sw_6_b)

.equ		sw_1_b	= 0		; port bits our switches are 
.equ		sw_2_b	= 1		; connected to.
.equ		sw_3_b	= 2
.equ		sw_4_b	= 3
.equ		sw_5_b	= 4
.equ		sw_6_b	= 5

.def		sw_0	= R26			; Most recent switch read
.def		sw_1	= R27			; previous switch read, stable bits
.def		sw_2	= R28			; current switch state
.def		sw_3	= R29			; Switch state change (1=changed)

sw_read:
		mov	sw_1, sw_0		; Copy previous read
		in	sw_0, RDSWI		; get current values
		eor	sw_1, sw_0		; if stable, sw_1(b) = 0
		mov	sw_3, sw_0		;
		eor	sw_3, sw_2		; of stable bits, changed bits are in sw_3
		and	sw_2, sw_1		; Force stable bits, in Sw_1, off in Switch State
		com	sw_1			; invert sw_1 so stable bits are now 1's
		and	sw_3, sw_1		; force unstable bits -> 0 in transistion register
		and	sw_1, sw_0		; copy stable bits of sw_0 to sw_1
		or	sw_2, sw_1		; move stable bits to sw_2
;
; Parse switch info
;
		bst	sw_3, sw_1_b
		brtc	sw_1_x			; STOP
		ldi	motor1, 0
		ldi	motor2, 0
sw_1_x:		
		bst	sw_3, sw_2_b		; FORWARD
		brtc	sw_2_x
		bst	sw_2, sw_2_b
		brtc	sw_2_x
		inc	motor1
		inc	motor2
		
sw_2_x:		
		bst	sw_3, sw_3_b		; backward
		brtc	sw_3_x
		bst	sw_2, sw_3_b
		brtc	sw_3_x
		dec	motor1
		dec	motor2
sw_3_x:		
		bst	sw_3, sw_4_b		; turn
		brtc	sw_4_x
		bst	sw_2, sw_4_b
		brtc	sw_4_x
		inc	motor1
		dec	motor2
sw_4_x:		
		bst	sw_3, sw_5_b		; turn other way
		brtc	sw_5_x
		bst	sw_2, sw_5_b
		brtc	sw_5_x
		dec	motor1
		inc	motor2
sw_5_x:		
		bst	sw_3, sw_6_b		; invert
		brtc	sw_6_x
		bst	sw_2, sw_6_b
		brtc	sw_6_x
		neg	motor1
		neg	motor2
sw_6_x:		
		ret

;
; INITIALIZE SWITCH LOGIC
;
sw_init:
		clr	sw_0
		clr	sw_1
		clr	sw_2
		clr	sw_3
		
		ret