;***********************************************
; Complex number magnitude calculation
; using CORDIC algorithm described at
; www.dspguru.com\info\faqs\cordic.htm
;
; Input:
;  ReH:ReL, ImH:ImL - complex number (16 bit signed)
;
; Output:
;  ReH:ReL - magnitude (16 bit unsigned)
;  ImH:ImL - garbage
;
; Temporaries:
;  RekH:RekL - Re multipled by k (k=2^-L, L=0,1,2,...15)
;  Counter - loop counter
;  Temp
;
; Instructions: 147
; Execution time(worst case including return):
;  18+18+15*(8+2+20+4+7.5*9)+60 ~= 1600 instruction cycles

; Notes:
;  1) Precision is 0.028%, depends on how exact
;  the division by CORDIC gain is implemented:
;	(0.60725293510314)
;	a) 1/2+1/8-1/64-1/512 -> 0.028%
;	b) 1/2+1/8-1/64-1/512-1/4096 -> 0.012384%
;	c) 1/2+1/8-1/64-1/512-1/4096+1/16384 -> 0.002333%
;  2) Range of input data should be restricted so
;  that M=sqrt(Re*Re+Im*Im) is less than 65536*0.60725293510314~=39760
;  to prevent overflow in magnitude during calculation
;  3) To reduce execution time, the number of loops can be
;  reduced to 8. The angle after rotation the initial
;  vector 8 times is less then 0.22381 deg, which is good
;  enough precision. Besides, the gain at 8 rotations is smaller
;  and closer to the approximated gain, which is used in this code.
;  Reduced execution time will be ~800 cycles!
;
; 6 Aug 2000 by Nikolai Golovchenko
;***********************************************
Magnitude16
;Find absolute value of the vector components
	btfss ReH, 7		;Re = abs(Re)
	 goto Magnitude16a
	comf ReL, f
	comf ReH, f
	incf ReL, f
	skpnz
	 incf ReH, f
Magnitude16a
	btfss ImH, 7		;Im = abs(Im)
	 goto Magnitude16b
	comf ImL, f
	comf ImH, f
	incf ImL, f
	skpnz
	 incf ImH, f
Magnitude16b
;Test imaginary part for zero and if yes, quit
	movf ImL, w
	iorwf ImH, w
	skpnz
	 return			;quit if zero imaginary part
;Perform first iteration
	movf ImL, w		;Imk = Im
	movwf ImkL
	movf ImH, w
	movwf ImkH
	
	movf ReL, w		;Im' = Im - Re
	subwf ImL, f
	movf ReH, w
	skpc
	 incfsz ReH, w
	  subwf ImH, f

	movf ImkL, w		;Re' = Re + Im = Re + Imk
	addwf ReL, f
	movf ImkH, w
	skpnc
	 incfsz ImkH, w
	  addwf ReH, f
;Begin loop
	movlw	1
	movwf Counter
Magnitude16loop
;load scaled values
	movf ImL, w		;Imk = Im
	movwf ImkL
	movf ImH, w
	movwf ImkH
	movf ReL, w		;Rek = Re
	movwf RekL
	movf ReH, w
	movwf RekH
;scale them (1 to 15 right shifts)
	movf Counter, w		;load counter value to Temp
	movwf Temp
Magnitude16loop2
	clrc			;unsigned right shift for Rek
	rrf RekH, f
	rrf RekL, f
	rlf ImkH, w		;signed right shift for Imk
	rrf ImkH, f
	rrf ImkL, f
	decfsz Temp, f
	 goto Magnitude16loop2
;update current values
	movf ImkL, w	
	btfsc ImH, 7		;if Im < 0 add a phase, if Im >= 0 substract a phase
	 goto Magnitude16AddPhase
;substract a phase
	addwf ReL, f		;Re' = Re + Imk
	movf ImkH, w
	skpnc
	 incfsz ImkH, w
	  addwf ReH, f

	movf RekL, w		;Im' = Im - Rek
	subwf ImL, f
	movf RekH, w
	skpc
	 incfsz RekH, w
	  subwf ImH, f

	goto Magnitude16loopend
Magnitude16AddPhase
;add a phase
	skpnc			;correct Imk, because shifts of negative
	 incfsz ImkL, w 	;values like (-1 >> 1 = -1) can
	decf ImkH, f		;accumulate error. With this correction,
	incf ImkH, f		;shifts of negative values will work like
				;shifts of positive values (i.e. round to zero)
				
	subwf ReL, f		;Re' = Re - Imk
	movf ImkH, w
	skpc
	 incfsz ImkH, w
	  subwf ReH, f

	movf RekL, w		;Im' = Im + Rek
	addwf ImL, f
	movf RekH, w
	skpnc
	 incfsz RekH, w
	  addwf ImH, f
Magnitude16loopend
	incf Counter, f
	btfss Counter, 4	;repeat untill counter reaches 16
;or uncomment this for better performance
;	btfss Counter, 3	;repeat untill counter reaches 8
	 goto Magnitude16loop

;Optional:
;Divide result by 1.64676025786545 (CORDIC gain)
;or multiply by 0.60725293510314 = 1/2+1/8-1/64-1/512 - 0.028%
	movf	ReH, w
	movwf	RekH
	movf	ReL, w
	movwf	RekL
	clrc
	rrf	ReH, f
	rrf	ReL, f
	clrc
	rrf	ReH, f
	rrf	ReL, f
	clrc
	rrf	ReH, f
	rrf	ReL, f
	comf	ReL, f
	comf	ReH, f
	incf	ReL, f
	skpnz
	incf	ReH, f
	clrf	Temp
	btfsc	ReH, 7
	comf	Temp, f
	subwf	ReL, f
	movf	RekH, w
	skpc
	incfsz	RekH, w
	subwf	ReH, f
	skpc
	 decf Temp, f
	rrf	Temp, f
	rrf	ReH, f
	rrf	ReL, f
	rrf	Temp, f
	rrf	ReH, f
	rrf	ReL, f
	rlf	ReH, w
	rrf	ReH, f
	rrf	ReL, f
	movf	RekL, w
	addwf	ReL, f
	movf	RekH, w
	skpnc
	incfsz	RekH, w
	addwf	ReH, f
	clrc
	rrf	ReH, f
	rrf	ReL, f
	clrc
	rrf	ReH, f
	rrf	ReL, f
	movf	RekL, w
	addwf	ReL, f
	movf	RekH, w
	skpnc
	incfsz	RekH, w
	addwf	ReH, f
	rrf	ReH, f
	rrf	ReL, f

;Done!
	return
;***********************************************