Sqrt32 - Z80

;
;   Sqrt32
;
;     Sqrt32: Integer Square Root of input + remainder
;       input:    4 bytes in _square
;       output:   2 bytes in _sqrt,     result
;                 3 bytes in M+2,       remainder
;
;   implements:
;        R= 0
;        M= N                               ;N= input
;        D=2^15 (8000h)                     ;D= 2^(p-1),  p= #bits(result) = #bits(input)/2
;        for n= 1 to 16                     ;for n= 1 to p
;        {
;            T= ((R+R+D) >> 1) << 16        ; 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,0
;_R     = _sqrt
;_T     .db 0,0,0,0
_M      = _square
;_D     .db 0,0

sqrt32
    xor a
    ld (_M4),a
    ld b,a
    ld c,a
    ;ld (_sqrt),bc
    ld de,04000h
    ld ixh, 0fh     ; 15 iterations + lastiter = 16
loopsq
    ;ld hl,(_sqrt)
    ld h,b          ; T=[((2*R+D)>>1) << 16]; in fact upperbytes(T) = R + (D >> 1)
    ld l,c
    add hl,de
    ;ld (_T+2),hl
    ;ld (_D),de
    push de
    or a
    jp nz, _sk      ; if bit 0,a =1 => m>t

    ;or a           ; CY=0
    ;ld de,(_T+2)
    ex de,hl
    ld hl,(_M+2)
    sbc hl,de;      if m>t  => nc nz nn;  if t=m => z nc nn; if m<t => nz c n; need to jump when t>m
    jp c, _skp1    ; jump if t>m
    jp _skp0

_sk ex de,hl
    ld hl,(_M+2)
    sbc hl,de

_skp0
    ld (_M+2),hl    ; M = M-T

    pop hl          ; R = R+D
    push hl
    add hl,hl
    ;ld de,(_sqrt)
    ;add hl,de
    add hl,bc
    ;ld (_sqrt),hl
    ld b,h
    ld c,l

_skp1
    xor a
    ld hl,(_M)      ; M = M*2
    add hl,hl
    ld (_M),hl
    ld hl,(_M+2)
    adc hl,hl
    ld (_M+2),hl
    rla

    ;ld de,(_D)     ; D = D/2
    pop de
    srl d
    rr e

    dec ixh
    jp nz, loopsq

lastiter
    or a
    jp nz, _sk0

    ;or a         ; CY=0     in this iteration _T+2=_sqrt
    ;ld de,(_sqrt)
    ;ld hl,(_M+2)
    sbc hl,bc;      if m>t  => nc nz nn;  if t=m => z nc nn;  if m<t => nz c n; need to jump when t>m
    jp c, _sk1    ; jump if t>m
    jp nz, _sk0   ; jump if t<m  (because  t != m   and   t !> m)
    ld a,(_M+1)
    sub 080h
    jp c, _sk1
    jp _s

_sk0
    ld a,(_M+1)
    sub 080h
    ;ld (_M+1),a
    ;or a
    ld hl,(_M+2)
    sbc hl,bc
_s  ld (_M+2),hl
    inc bc          ; D=1,   R = R+D => R = R+1
_sk1
    ;ld a,(_M+1)
    sla a           ; M = M*2  (M = M << 1)
    ;ld (_M+1),a
    ld hl,(_M+2)
    adc hl,hl
    ld (_M+2),hl    ; remainder to _M+2
    jp nc, _sR
    ld a,1
    ld (_M+4),a
_sR ld (_sqrt),bc   ; result (BC) to _sqrt; could be saved in _M+0, to have M+0/M+1 = result, M+2/M+3 = remainder

    ret