;Input 16-Bit unsigned HDAC:LDAC

;register
; HDAC and LDAC
; temp - temp4

;lowpass
;a += f * ((in - a) + q * (a - b));
;b += f * (a - b);  





		
		clc		;clear carry
		ror	HDAC	;\
		ror	LDAC	;/ div  2 -  reduce 0->255 to 0-127 

		ldi 	temp, 0xff	;\
		eor	LDAC,temp 	; |
		eor	HDAC,temp 	; |
		ldi temp, 0x01		; |Bit invert
		add	LDAC, temp	; |and add 1 
		ldi temp ,0x00		; |
		adc HDAC,temp		;/
		
		;Input now signed

		;(in - a) - both signed
		sub 	LDAC, aL
		sbc 	HDAC, aH
		;(in-a) now in HDAC:LDAC  signed
	

		;q*(a-b)
		lds	r22, F_QH	; load Q  - unsigned
		mov	temp, aL	;\
		mov	r21, aH		;/ load a  - signed
		lds	ZH, BH		;\
		lds 	ZL, BL		;/ load b  - signed

		sub	temp, ZL;\
		sbc	r21, ZH	;/ (a-b) signed
		
		mulsu	r21,r22		;mul signed:unsigned -> (a-b) * Q
		movw	ZL,r0		;Q*(a-b) now in ZH:ZL signed



		;add both
		;both signed
		;((in-a)+q*(a-b))
		;=> HDAC:LDAC + ZH:ZL


		add	 LDAC, ZL
		adc	 HDAC, ZH

		;result is signed in HDAC:LDAC
		;*f 
		;((in-a)+q*(a-b))*f

		lds	r20, F_CH	;load F 
		mov 	r21, HDAC	;load HDAC in mulsuf laden
		mulsu	r21,r20		;mul signed unsigned HDAC*F
					;result r1:r0 signed

		;add result to a
		;a+=f*((in-a)+q*(a-b))
		add		aL, r0
		adc		aH, r1
		;a+=f*((in-a)+q*(a-b)) is in aH:aL as signed

		

		;calc b 
		;b += f * (a - b);  

		mov r21, aH		;\
		mov ZL, aL		;/ load a  signed
		lds temp, BL		;\
		lds temp2, BH		;/ load b signed

		sub ZL,temp		;\
		sbc r21, temp2 ;(a-b)	;/ (a - b) signed
		mulsu r21,r20 ;(a-b)*q	;mul signed unsigned (a-b) * F
					;result is signed in r1:r0

		
		add temp,r0		;\ add result to b  signed
		adc temp2, r1		;/ b +=(a-b)*f

		sts BL, temp		;\
		sts BH, temp2		;/ save b

		;put B to Output registers for DAC
		mov		LDAC, temp
		mov		HDAC, temp2

				
		;make output unsigned
		ldi temp, 0xff
		eor	LDAC,temp	; invert bits
		eor	HDAC,temp
		ldi temp, 0x01		; add 1
		add	LDAC, temp
		ldi temp ,0x00
		adc HDAC,temp		;  unsigned 0->127

		clc
		rol		LDAC	;/ * 2
		rol		HDAC	;\
					;result = unsigned 0->255 in DAC