/* 
Sqrt (Ver. 1.0)
Author: Nicholas Lombardo
Date:	December 20, 2014
Inputs: N, 16-bit number with arbitrary radix point, between R and R+1 bit
Output: S, 16-bit sqrt(N) radix point shifted left to be between (R+R/2) and (R+R/2)+1
EX.	for R=0, N is integer.  SH will have an integer, SL will be values right of radix

1. guess a square root starting at 0x8000
2. increase/decrease next bit according to compare
3. repeat up to 16 times

ex. guess up: 	0x8000 --> 0xC000
ex. guess down:	0x8000 --> 0x4000
*/
.INCLUDE <m328pdef.inc>

.DSEG

N:	.byte	2
S:	.byte	2

.CSEG

;-----------------------------
;---         Setup:	       ---
;-----------------------------

clr		r15			// used for adc

ldi 	r17,0x7F	// N = 32651.0		(for R=0)	OR
ldi 	r16,0x8B	// N =   127.5430	(for R=7)	
sts		N+1,r17
sts		N,r16

;-----------------------------
;---     Main Program:	   ---
;-----------------------------

Setup:
	lds		r14,N+1			// N saved to r14:r13
	lds		r13,N

	ldi		r17,0x80		// r17:r16 used for guesses
	clr		r16
	movw	r19:r18,r17:r16	// r19:r18 will perform the binary sweep

loop:
	rcall	square			// guess: r10,r9,r8,r7 = (r17:r16)^2
	rcall	shiftreg		// r19:r18>>1
	brcs	numfound		// if c=1, then r18 shifted out and sweep is done
	rcall	compare			// compare guess (r10:r9:r8:r7) to (r14:r13:0:0)
	rjmp	loop

numfound:
	sts		S+1,r17			// S = 180.6992 (B4.B3) = 180 +  179/256	(R=7)
	sts		S,r16			// S = 11.2937	(B.4B3) = 11  + 1203/4096	(R=11)
	rjmp	numfound


;-----------------------------
;---     Subroutines:	   ---
;-----------------------------
//////////////////////////////
// Square: square the guess
// Input:	r17:r16
// Output:	r10:r9:r8:r7 = (r17:r16)^2
square:
	mul		r16,r16 // L^2		(r8:r7)
	mov		r8,r1
	mov		r7,r0
	mul		r17,r17 // H^2		(r10:r9)
	mov		r10,r1
	mov		r9,r0	
	mul		r16,r17 // 2*H*L	(r9:r8)
	lsl		r1
	adc		r10,r15
	lsl		r0
	adc		r1,r15 
	add		r8,r0
	adc		r1,r15
	adc		r9,r1
	adc		r10,r15
	ret

//////////////////////////////
// Shiftreg:
// Input:	r19:r18
// Output:	r19>>1, r18>>1
Shiftreg:
	lsr		r19
	brcc	noshiftout
	clc
	ldi		r18,0x80
	cpse	r18,r18
noshiftout:
	lsr		r18
	ret


//////////////////////////////
// Compare: 
// If guess^2 < N: inc guess
// If guess^2 > N: dec guess
// Input:	r14:r13
// Output:  r17:r16
compare:
	cp		r10,r14		// compare high byte
	brcs	guessup
	breq	r10r14eq
	rjmp	guessdown
	
// r10==r14
r10r14eq:
	cp		r9,r13		// compare low byte
	brcs	guessup
	breq	r9r13eq
	rjmp	guessdown

// r10:r9==r14:r13
r9r13eq:				
	tst		r8			// two 0 bytes added for accuracy
	breq	r8r12eq
	rjmp	guessdown

r8r12eq:
	tst		r7
	breq	numfound
	rjmp	guessdown

// increase r17:r16
	guessup:
		or		r16,r18
		or		r17,r19
		rjmp	endcompare

// decrease r17:r16
	guessdown:			
		sub		r16,r18
		brcc	nocarry	// dec r17 if sub borrows from high byte
		dec		r17
	nocarry:
		sub		r17,r19
		rjmp	endcompare

endcompare:
	ret