;
;   Performance :
;               Program Memory  :   22 (excluding D_divS subroutine)
;               Clock Cycles    :   3000 (approximately,with 10 iterations)
;
;       The #of cycles depends on Number of Iterations Selected.
; In a lot of cases 5 or less iterations may be sufficient
;
;
;*******************************************************************
;               Newton-Raphson Method
;*************************************************************

Sqrt
	call    SqrtInit            ; compute initial sqrt = Num/2
nextIter
   #if MODE_FAST
	movfp    NumLo,ACCbLO
	movfp    NumHi,ACCbHI
   #else
	movfp    NumLo,wreg
	movwf     ACCbLO
	movfp    NumHi,wreg
	movwf     ACCbHI
   #endif
;
	call    D_divS          ; double precision division
				; double precision addition
	movfp    ACCdLO,wreg     ; ACCd + ACCa -> ACCd
	addwf     ACCaLO           ;addwf lsb
	movfp    ACCdHI,wreg
	addwfc    ACCaHI           ;addwf msb
				; now divide by 2
	bcf      _carry
	rrcf     ACCaHI
	rrcf     ACCaLO
;
	decfsz     iterCnt
	goto    nextIter
	return                     ; End Sqrt
;
SqrtInit
	movlw    _LUPCNT
	movwf     iterCnt         ; set number of iterations
  #if MODE_FAST
	movfp    NumHi,ACCaHI
	movfp    NumLo,ACCaLO
  #else
	movfp    NumHi,wreg
	movwf     ACCaHI
	movfp    NumLo,wreg         ; set initial guess root = NUM/2
	movwf     ACCaLO
  #endif
	bcf      _carry
	rrcf     ACCaHI
	rrcf     ACCaLO      ; set initial sqrt = Num/2
	return
;
	PAGE
;*******************************************************************
;                   8x8 Software Multiplier
;               ( Fast Version : Straight Line Code )
;
;   The 16 bit result is stored in 2 bytes
;
; Before calling the subroutine " mpy ", the multiplier should
; be loaded in location " mulplr ", and the multiplicand in
; " mulcnd " . The 16 bit result is stored in locations
; H_byte & L_byte.
;
;       Performance :
;                    Program Memory  :  36 words
;                    # of cycles     :  36       (excluding call & return)
;                    Scratch RAM     :   0 locations
;                    W Register      :  Used
;
;  This routine is optimized for speed efficiency ( straight line code )
;  For code efficiency, refer to "mult8x8S.asm" ( looped code )
;*******************************************************************
;     Define a macro for adding & right shifting
;
multiply    MACRO
	variable  i ;
	  i = 0
	  .while i < 8
		btfsc     mulplr,i
		addwf    H_byte
		rrcf    H_byte
		rrcf    L_byte
	    i = i+1 ;
	  .endw
	ENDM                    ; End of macro
;
;
mpy8x8_F
	clrf     H_byte
	clrf     L_byte
	movfp    mulcnd,wreg     ; move the multiplicand to W reg.
	bcf      _carry          ; Clear the carry bit in the status Reg.
;
	multiply
;
	return
;
	PAGE
;*******************************************************************
;                   8x8 Software Multiplier
;               ( Code Efficient : Looped Code )
;
;   The 16 bit result is stored in 2 bytes
;
; Before calling the subroutine " mpy ", the multiplier should
; be loaded in location " mulplr ", and the multiplicand in
; " mulcnd " . The 16 bit result is stored in locations
; H_byte & L_byte.
;
;       Performance :
;                    Program Memory  :  13 words (excluding call & return)
;                    # of cycles     :  69       (excluding call & return)
;                    Scratch RAM     :  1 byte
;                    W Register      :  Used
;
;  This routine is optimized for code efficiency ( looped code )
;  For time efficiency code refer to "mult8x8F.asm" ( straight line code )
;*******************************************************************
;
mpy8x8_S
	clrf     H_byte
	clrf     L_byte
	clrf     count
	bsf      count,3         ; set count = 8
	movfp    mulcnd,wreg
	bcf     _carry    ; Clear the carry bit in the status Reg.
loop
	btfsc      mulplr,0
	addwf     H_byte
	rrcf     H_byte
	rrcf     L_byte
	rrncf      mulplr
	decfsz     count
	goto    loop
;
	return
;
	PAGE
;*******************************************************************
;                       Numerical Differenciation
;
;       The so called "Three-Point Formula" is implemented to
; differenciate a sequence of points (uniformly sampled).
;       The eqn implemented is :
;             f'(Xn) = [ f(Xn - 2h) - 4*f(Xn - h) + 3*f(Xn)]*0.5/h
;    where Xn is the present sample and 'h' is the step size.
;
;       The above formula may be rewritten as :
;
;         f'(Xn) = [ 0.5*f(Xn -2) - 2*f(Xn - 1) + 0.5*3*f(Xn)]*1/DiffK
;  where DiffK = h = Step Size
;
;   This differenciation routine can be used very effectively
; in the computation of the differential component part in
; a PID Loop calculation in Motor Control Applications
;
;       Double precision arithmetic is used throught
;  The present sample value is assumed to be in locations
; (XnHi, XnLo). The past two values are assumed to be in locations
; (Xn_1_Hi, Xn_1_Lo) & (Xn_2_Hi, Xn_2_Lo).
;       The output value is located in DiffHi & DiffLo. No overflow
; checking mechanism is implemented. If the values are limited
; to 12 bits, then the user need not worry about overflows
;
;  It is user's responsibility to update the past values with the
; present values before calling this routine.
;  After computation, the present value Xn is not moved to Xn_1
; because the user may want these values to be intact for other
; computations ( say numerical integration)
;       Also it is user's responsibility to set past 2 values
; (Xn_1 & Xn_2) values to be zero on initialization.
;
;*******************************************************************
;
Diff
	movfp    Xn_2_Lo,wreg
	addwf     XnLo,w
	movwf     ACCbLO
	movfp    Xn_2_Hi,wreg
	addwfc    XnHi,w
	movwf     ACCbHI          ; Y = f(Xn-2) + f(Xn)
;
	movfp    XnLo,wreg
	addwf     ACCbLO
	movfp    XnHi,wreg
	addwfc    ACCbHI
	movfp    XnLo,wreg
	addwf     ACCbLO
	movfp    XnHi,wreg
	addwfc    ACCbHI    ; Y = f(Xn-2) + 3*f(Xn)
;
	bcf      _carry
	rrcf     ACCbHI
	rrcf     ACCbLO    ; Y = 0.5*[ f(Xn-2) + 3*f(Xn) ]

;
	movfp    Xn_1_Lo,wreg
	subwf     ACCbLO
	movfp    Xn_1_Hi,wreg
	subwfb    ACCbHI
	movfp    Xn_1_Lo,wreg
	subwf     ACCbLO
	movfp    Xn_1_Hi,wreg
	subwfb    ACCbHI    ; Y = 0.5*[f(Xn-2) + 3*f(Xn)] - 2*f(Xn-1)
;
	movfp    DiffKLo,wreg
	movwf     ACCaLO
	movfp    DiffKHi,wreg
	movwf     ACCaHI
;
	call    D_divS
	movfp    ACCbLO,wreg
	movwf     DiffLo
	movfp    ACCbHI,wreg
	movwf     DiffHi          ; result = Y/h
;
	return
;
	PAGE
;*******************************************************************
;
;                       Numerical Integration
;
;
;       Simpson's Three-Eighths Rule is implemented
;
;       Y(n) = [ f(X0) + 3*f(X1) + 3*f(X2) + f(X3)]*3*h/8
;
;  where 'h' is the step size and the integral is over the
;  range X0 to X3
;       The above equation can be rewritten as
;
;       Y(n) = [ f(X0) + 3*f(X1) + 3*f(X2) + f(X3)]*IntgK
;
;   where IntgK = 3*h/8 (in locations (IntgKHi, IntgKHi)
;
;   This Integration routine can be used very effectively
; in the computation of the integral component part in
; a PID Loop calculation in Motor Control Applications
;
;       Double precision arithmetic is used throught
;  The three input values over which the integral is to be computed
; are assumed to be in locations (X0Lo,X0Hi), (X1Lo,X1Hi) , (X2Lo,X2Hi)
; and (X3Lo,X3Hi)
;       The output value is located in IntgHi & IntgLo. No overflow
; checking mechanism is implemented. If the values are limited
; to 12 bits, then the user need not worry about overflows
;
;  It is user's responsibility to update the past values with the
; present values before calling this routine.
;  After computation, the present value Xn is not moved to Xn_1
; because the user may want these values to be intact for other
; computations ( say numerical integration)
;       Also it is user's responsibility to set past 2 values
; (Xn_1 & Xn_2) values to be zero on initialization.
;
;
;*******************************************************************
;
Integrate
	movfp    X0Lo,wreg
	addwf     X3Lo,w
	movwf     ACCbLO
	movfp    X0Hi,wreg
	addwfc    X3Hi,w
	movwf     ACCbHI          ; Intg = f(X0) + f(X3)
;
	movfp    X1Lo,wreg
	addwf     ACCbLO
	movfp    X1Hi,wreg
	addwfc    ACCbHI   ; Intg = f(X0) + f(X3) +X1
	movfp    X1Lo,wreg
	addwf     ACCbLO
	movfp    X1Hi,wreg
	addwfc    ACCbHI   ; Intg = f(X0) + f(X3) +2*X1
	movfp    X1Lo,wreg
	addwf     ACCbLO
	movfp    X1Hi,wreg
	addwfc    ACCbHI   ; Intg = f(X0) + f(X3) +3*X1
;
	movfp    X2Lo,wreg
	addwf     ACCbLO
	movfp    X2Hi,wreg
	addwfc    ACCbHI   ; Intg = f(X0) + f(X3) +3*X1 + X2
	movfp    X2Lo,wreg
	addwf     ACCbLO
	movfp    X2Hi,wreg
	addwfc    ACCbHI   ; Intg = f(X0) + f(X3) +3*X1 + 2*X2
	movfp    X2Lo,wreg
	addwf     ACCbLO
	movfp    X2Hi,wreg
	addwfc    ACCbHI   ; Intg = f(X0) + f(X3) +3*X1 + 3*X2
;
	movfp    IntgKLo,wreg
	movwf     ACCaLO
	movfp    IntgKHi,wreg
	movwf     ACCaHI          ;  ACCa = IntgK (prepare for multiplication)
;
	call    D_mpyS          ; make sure to set for either SIGNED or UNSIGNED
	movfp    ACCdLO,wreg
	movwf     IntgLo          ; 32 bit result in ACCd & ACCc
	movfp    ACCdHI,wreg
	movwf     IntgHi          ; upper 16 bits = result
;
	return
;
	PAGE
;*******************************************************************
;
;                       Random Number Generator
;
; This routine generates a 16 Bit Pseudo Sequence Random Generator
; It is based on Linear shift register feedback. The sequence
; is generated by (Q15 xorwf Q14 xorwf Q12 xorwf Q3 )
;
;    The 16 bit random number is in location RandHi(high byte)
;  & RandLo (low byte)
;
;       Before calling this routine, make sure the initial values
; of RandHi & RandLo are NOT ZERO
;       A good chiose of initial random number is 0x3045
;*******************************************************************
;
Random16
	rlcf     RandHi,w
	xorwf     RandHi,w
	rlcf     wreg            ; carry bit = xorwf(Q15,14)
;
	swapf    RandHi
	swapf    RandLo,w
	rlncf      wreg
	xorwf     RandHi,w        ; LSB = xorwf(Q12,Q3)
	swapf    RandHi
	andlw   0x01
	rlcf     RandLo
	xorwf     RandLo
	rlcf     RandHi
	return
;
	PAGE
;*******************************************************************
;                       Gaussian Noise Generator
;
; This routine generates a 16 Bit Gaussian distributed random
; points. This routine calls the routine "Random16", which
; generates a psuedo random noise sequence. Gaussian noise
; is computed using the CENTRAL LIMIT THEOREM.
;       The Central Limit Theorem states that the average weighted
; sum of uncorelated samples tends to have a Gaussian distribution
; For practical purposes, the sum could be over a sample size
; of 32 Random numbers. Better results could result if a larger
; sample size is desired. For faster results, a sum over 16 samples
; would also be adequate ( say, for applications like Speech synthesis,
; channel simulations, etc).
;
;       The 16 bit Gaussian distributed point is in locations
; GaussHi & GaussLo
;
;       Before calling this routine, the initial seed of Random
; number should be NON ZERO ( refer to notes on "Random16" routine
;
;*******************************************************************
;
Gauss
	clrf     count
	bsf      count,5         ; set Sample size = 32
	clrf     GaussLo
	clrf     GaussHi
	clrf     GaussTmp
;
NextGauss
	call    Random16        ; get a random value
	movfp    RandLo,wreg
	addwf     GaussLo
	movfp    RandHi,wreg
	addwfc    GaussHi
	clrf     wreg
	addwfc    GaussTmp
	decfsz     count
	goto    NextGauss       ; sum 16 random numbers
;
	movlw    5
GaussDiv16
	rrcf     GaussTmp
	rrcf     GaussHi
	rrcf     GaussLo           ; weghted average
	decfsz     wreg              ; divide by 32
	goto    GaussDiv16
;
	return
;

	END                       ; End Of arith.asm