Sqrt16 - Z80

;
;   Sqrt16
;
;     Sqrt16: Integer Square Root of input + remainder
;       input:    2 bytes in _square
;       output:   1 bytes in _sqrt,     result
;                 2 bytes in M+3,       remainder
;
;   HL is loaded with _square value at the beginning, and the results are put in memory at the end
;   it can easily be changed to enter and exit the values via registers directly
;
;   medium execution time: 250us on a 4MHz Z80; almost 4000 computed roots per sec
;
;   implements:
;        R= 0
;        M= N                               ;N= input
;        D= 2^7 (80h)                       ;D= 2^(p-1),  p= #bits(result) = #bits(input)/2
;        for n= 1 to 8                      ;for n= 1 to p
;        {
;            T= ((R+R+D) >> 1) << 8         ; T= (2R+D) << (p-1)
;            if (T <= M) then M=M-T: R=R+D
;            M= M << 1
;            D= D/2
;        }

_square .db 0,0,0,0
_M4     .db 0
_sqrt   .db 0
;_T     .db 0,0
_M      = _square
;_D     .db 0

sqrt16
    ld hl,(_square) ; hl=M; T exists just temporarily in a
    ld de,00080h    ; d=R, e=D in last iteration (80h)
    ld bc, 0740h    ; b is a counter, c=D
loopsq16
    jp c, _skp016

    ld a,h      ; M-T  (T>M)
    sub c
    sub d
    jp m, _skp116   ; if "m" then M-T<0, so T>M
    jp _s1

_skp016
    ld a,h
    sub c
    sub d
_s1 ld h,a      ; M=M-T

    ld a,c      ; R=R+D; (D must be doubled)
    add a,c
    add a,d
    ld d,a

_skp116
    srl c       ; D=D/2

    xor a
    sla l       ; M=M*2
    rl h

    djnz loopsq16

lastiter16      ; D=1   ; T+1=R  =>  M-T == M-R
    jp c, _skp01

    ;or a       ; CY=0, but CY is not set here
    sbc hl,de   ; M-T (T>M)
    add hl,de   ; restore HL;   sbc+add: trick to do a 16bit compare
    jp m,_skp11 ; if "m" then M-T<0, so T>M
_skp01
    or a        ; CY=0
    sbc hl,de   ; M=M-T where T=<R><80h>=de

    inc d       ; R=R+D  =>  R=R+1
_skp11
    sla l
    rl h
    rl e

    ld (_square+2),hl   ; h is the low byte of the remainder
    ld (_square+4),de   ; e is the high byte of the remainder (1 bit);   d is the result
    ret