;
; Mult16Bit takes in two words, and a pointer to a 4 byte buffer
; and multiplies the two words together, putting the 32 bit
; result into the buffer, MS bit first
;
; Logic is as follows
;
;                                  AB CD
;                                x EF GH
;                                -------
;   Byte 3        Byte 2           Byte 1         Byte 0
;
;                                                Low(CD.GH)
;                               High(CD.GH)
;                High(CD.EF)   +Low (CD.EF)
;              + High(AB.GH)   +Low (AB.GH)
;  High(AB.EF) + Low (AB.EF)   
; +Carry       + Carry        
                                                                                  
Declare Function Mult16Bit(ABCD As Word, EFGH As Word, Result As DataPointer) Returns Boolean

Declare Byte	ABx
Declare Byte	CDx
Declare Byte	EFx
Declare Byte	GHx
Declare Word	ABEF
Declare Word	CDGH
Declare Word	CDEF
Declare Word	ABGH
Declare Byte	Carry
Declare Word	TempW
Declare Byte	Temp

ABx = HighByte(ABCD)
CDx = LowByte(ABCD)
EFx = HighByte(EFGH)
GHx = LowByte(EFGH)

CDGH = CDx * GHx
CDEF = CDx * EFx
ABEF = ABx * EFx
ABGH = ABx * GHx

Increment Result
Increment Result
Increment Result				; so we are pointing to Byte 0

Temp = LowByte(CDGH)
WriteData result,Temp
Decrement Result
Decrement Result				; now point to Byte 1

Temp = HighByte(CDGH)
Tempw = MakeWord(Temp)			; bring inthe High byte of CDGH
CDEF = CDEF And 00FFH			; get the low byte only
TempW = TempW + CDEF
ABGH = ABGH And 00FFh		
TempW = TempW + ABGH
Temp = LowByte(TEmpW)			; and strip of the lowbyte of the result
WriteData Result, Temp			; and write into Byte 1
Carry = HighByte(TempW)			; and pickup the carry
Decrement Result
Decrement Result				; now point to Byte 2

Tempw = MakeWord(Carry)			; bring in the carry
CDEF = CDx * EFx
TEmp = HighByte(CDEF)			; get the highbyte = CDEF
CDEF = MakeWord(Temp)			; get put it back into CDEF
TempW = Tempw + CDEF			; so we can add it 
ABGH = ABx * GHx
TEmp = HighByte(ABGH)			; get the highbyte = CDEF
ABGH = MakeWord(Temp)			; get put it back into CDEF
TempW = Tempw + ABGH			; so we can add it 
ABEF = ABEF And 000ffh 			; get the low byte of ABEF
TempW = TempW + ABEF
Temp = LowByte(TempW)
WriteData Result, Temp			; and write into Byte 2
Carry = HighByte(TempW)			; and pickup the carry
Decrement Result
Decrement Result				; now point to byte 3

TempW = MakeWord(Carry)			; bring in the carry
ABEF = ABx * EFx
TEmp = HighByte(ABEF)			; pull out the high byte
ABEF = MakeWord(Temp)			; and put it into the low byte
TempW = TempW + ABEF			; so we can add it to the carry

Temp = LowByte(TempW)
WriteData Result, Temp			; and write into Byte 3
Carry = HighByte(Tempw)
If Carry = 0 Then
	Return True
Else
	Return False			; if we have a carry, then the mult has overflowed.
End If

End Function

;############################################################

; divides Quotient (pointer to 4 bytes) by Divider Word
;

Declare Function Divide32(Quotient As DataPointer, divider As Word) Returns Word

Declare Word 	divisor
Declare Word 	tempW
Declare Word	QHi
Declare Word	Qlo
Declare Boolean	Not_Done
Declare Word	Thisdigit	
Declare Word	Result
Declare Word	Factor
Declare Word	ProdH
Declare Word	ProdL
Declare Byte	temp1
Declare Byte	Temp2
Declare Byte	Digit1
Declare Byte	Digit2

ProdH = ReadData(Quotient)
ProdL = ReadData(Quotient)

QHi = 0
QLo = ProdH			; start off from the LeftMost (MS) word

Factor = 01000H		; for the Position of the next result digit
Result = 0

Do

	If Divider < QLo Then		; we can get get a whole number division
		Thisdigit = Qlo / Divider
		Result = Result + Thisdigit
		QLo = Qlo Mod Divider	
	End If

	If QHi <> 0	Then					; if we have something to borrow from
		Decrement QHi				; Borrow 1(0)H
		; QLo = 65535 - Divider + QLo + 1	; and take off 1 Lot of Divider
		TempW = 65535 - Divider
		QLo = TempW + QLo
		Increment QLo				; this is to get the 1000H's Compliment correct; 
		Increment Result				; and we have taken off one more instance of Divider
	Else
		If Factor <> 0 Then			; if we still have digits left

			; we have nothing to borrow, so we move down to the next digits

			QHi = QLo / 01000H
			QLo = QLo Mod 01000H			; and leave the 3 lower digits
			Temp1 = HighByte(QLo)			; get the upper one of the 3 digits
			TempW = Temp1 * 010h			; and roll it left (TempW = 0,0,Digit3,0)
			Push Tempw					; push it onto the Stack		
			Temp2 = LowByte(QLo)			; get the lower 2 digits (Temp2 = Digit2,Digit1)
			Digit1 = Temp2 Mod 16			; get Digit1 (Digit1 = 0,Digit1)
			Digit2 = Temp2 / 16			; get Digit2 (Digit2 = 0,Digit2)
			Temp1 = Pop()				; pop off the Digit3,0 of TempW into Temp1
			Temp2 = Pop()				; pop off the 0,0 of TempW (which we dont need)
			Temp1 = Temp1 + Digit2			; and get Temp1 = Digit3,Digit2
			Push Temp1					; save it onto the stack
			TempW = Digit1 * 16			; TempW = 0,0,Digit1,0
			Temp1 = LowByte(Tempw)			; Temp1 = Digit1,0
			Thisdigit = ProdL / Factor		; get the next digit from the LS Word
			Temp2 = LowByte(Thisdigit)
			Temp1 = Temp1 + Temp2			; Temp1 = Digit1,ThisDigit
			Push Temp1					
			QLo = Pop()					; QLo = Digit3, Digit2, Digit1, ThisDigit		

			ProdL = ProdL Mod Factor	
			If Factor > 1 Then
				Factor = Factor / 010H		; then get ready for the next time
			Else
				Factor = 0
			End If
			; take Result = 0,Digit3,digit2,digit1 into Digit3,Digit2,Digit1,0
			Temp1 = LowByte(Result)		; temp1 = Digit2,Digit1
			Temp2 = HighByte(Result)	; temp2 = 0,Digit3
		
			Result = Temp2 * 010H		; Result = 0,0,Digit3,0
			TempW = Temp1 * 010H		; TempW = 0,Digit2,Digit1,0
			Temp2 = HighByte(Tempw)		; Temp2 = 0,Digit2
			Temp1 = LowByte(Result)		; Temp1 = Digit3,0
			TEmp1 = Temp1 + Temp2		; Temp1 = Digit3,Digit2
			Push Temp1
			Temp1 = LowByte(Tempw)		; Temp1 = Digit1,0
			Push	Temp1		
			Result = Pop()			; and pop off Result=digit3,digit2,digit1,digit0
		End If
	End If

	If Qlo > Divider Then
		Not_Done = True		; If we still have room to keep dividing
	Else
		If Factor <> 0 Then	; if we still have digits to work through
			Not_Done = True
		Else
			Not_Done = False
		End If
	End If
Until Not_Done = False			; then we can keep going

Return Result

End Function ; Divide32

;#############################################################
; Add32
; Adds two 32 byte values together
; Var1 = Var1 + Var2

Declare Function Add32(Var1 As DataPointer, Var2 As DataPointer)
Declare Byte carry
Declare Byte loopcount
Declare Byte	Temp1
Declare Byte	Temp2
Declare Word	Temp1W
Declare Word	Temp2W

Var1 = var1 + 3	; point to LS byte
Var2 = Var2 + 3	; point to LS byte
Carry = 0
Loopcount = 0
Do
	Temp1 = ReadData(Var1)
	Temp2 = ReadData(Var2)

	Temp1W = MakeWord(Temp1)
	Temp2W = MakeWord(Temp2)

	Temp1W = Temp1W + Temp2W
	Temp2W = MakeWord(Carry)	; and get any carry 
	Temp1W = Temp1W + Temp2W	; add it in
	Carry = HighByte(Temp1W)	; get carry for next time
	Temp1 = LowByte(Temp1W)

	Decrement Var1		; point back to LS Byte
	WriteData	Var1,Temp1	; and write in the LS byte of the Result
	Var1 = Var1 - 2
	Var2 = Var2 - 2		; point to the next Most Significant Byte
	Increment loopcount
Until loopcount = 4

End Function; Add32

;#############################################################
; Sub32
; Subtracts 32 byte value
; Var1 = Var1 - Var2
; returns False if Var2 was bigger than Var1 (but has overwritten Var1 in the process

Declare Function Sub32(Var1 As DataPointer, Var2 As DataPointer)Returns Boolean
Declare Byte 	Borrow
Declare Byte 	loopcount
Declare Byte	Temp1
Declare Byte	Temp2
Declare Word	Temp1W
Declare Word	Temp2W

Var1 = var1 + 3	; point to LS byte
Var2 = Var2 + 3	; point to LS byte
Borrow = 0
Loopcount = 0
Do
	Temp1 = ReadData(Var1)
	Temp2 = ReadData(Var2)

	If Borrow <> 0 Then
		If Temp1 <> 0 Then
			temp1 = temp1 - Borrow	; handle any borrow
			Borrow = 0
		Else
			Temp1 = 255
			Borrow = 1		; remember to borrow from the next byte
		End If
	End If		

	Temp1W = MakeWord(Temp1)
	Temp2W = MakeWord(Temp2)

	If Temp1W < Temp2W Then
		Temp1W = Temp1W + 0100H	; borrow from next byte
		Borrow = Borrow + 1
	End If
		
	Temp1W = Temp1W - Temp2W
	Temp1 = LowByte(Temp1W)

	Decrement Var1		; point back to LS Byte
	WriteData	Var1,Temp1	; and write in the LS byte of the Result
	Var1 = Var1 - 2
	Var2 = Var2 - 2		; point to the next Most Significant Byte
	Increment loopcount
Until loopcount = 4

If Borrow = 0 Then
	Return True
Else
	Return False
End If

End Function; Sub32


;#############################################################
; square root functions
;
; SqrootW - Returns the square root of a word, in a word 
;
; This method dating back to the Babylonians (1700 B.C.) is known as the Babylonian Algorithm.
; If A > 0 is a guess of the square root of the positive number Q, 
; then a better approximation to SQRT(Q) is given by B = (A + Q/A)/2.

Declare Function	sqrootW(target As Word) Returns Word

Declare Word	guess			; the [improving] estimate of the sqrot
Declare Word	lastguess		; what we guessed last time around
Declare Word	closeenough = 1	; how close do we need to get
Declare Word	change		; when checking guess against lastguess
Declare Word	tempw

guess = target / 2			; guess half way
lastguess = 0
change = guess - lastguess

While change > closeenough		; check how close this guess is to last one

	tempw = target / guess		; newguess = (Target/Guess + Guess) / 2
	guess = guess + tempw
	guess = guess / 2
	If guess > lastguess Then	
		change = guess - lastguess
	Else
		change = lastguess - guess
	End If
	lastguess = guess			; remember for next time around

End While

Return guess
End Function ; Sqroot

;############################################

; Sqroot32 - Returns the square root of a 32Bit Value, in a word 
;
; This method dating back to the Babylonians (1700 B.C.) is known as the Babylonian Algorithm.
; If A > 0 is a guess of the square root of the positive number Q, 
; then a better approximation to SQRT(Q) is given by B = (A + Q/A)/2.

Declare Function	sqroot32(target As DataPointer) Returns Word


Declare Word	guess			; the [improving] estimate of the sqrot
Declare Word	lastguess		; what we guessed last time around
Declare Word	closeenough = 20	; how close do we need to get
Declare Word	change		; when checking guess against lastguess
Declare Word	tempw

Push Target					; save the pointer to our target
guess = Divide32(target, 2)		; start off with guess = target / 2	
Target = Pop()				; restore our target pointer = pop()
lastguess = 0
change = guess - lastguess


While change > closeenough		; check how close this guess is to last one

	Push	target			; save a copy of our target pointer
	tempw = divide32(target,guess)	; newguess = (Target/Guess + Guess) / 2
	target = Pop()			;  restore our target pointer

	guess = guess + tempw
	guess = guess / 2
	If guess > lastguess Then	
		change = guess - lastguess
	Else
		change = lastguess - guess
	End If
	lastguess = guess			; remember for next time around

End While


Return guess
End Function ; Sqroot

;############################################