                    ; Square root of an unsigned 32-bit integer.
                    ; Programmed by Soji Yamakawa 2017/11/10
                    ; Based on the method on the Stackoverflow:
                    ;    https://stackoverflow.com/questions/1623375
                    ; Due to overflow at (*1) the input is limited to 32767*32767.

                    EXPORT  SQRT32_INPUT_HI
                    EXPORT  SQRT32_INPUT_LO
                    EXPORT  SQRT32_OUTPUT
                    EXPORT  SQRT32

SQRT32_INPUT_HI     FDB     0   ; Input Higher 16-bit,  Output Squre Root.
SQRT32_INPUT_LO     FDB     0   ; Input Lower 16-bit,  Output undefined.
SQRT32_OUTPUT       EQU     SQRT32_INPUT_HI

SQRT32_GUESS        FDB     0
SQRT32_GUESSSQ_HI   FDB     0
SQRT32_GUESSSQ_LO   FDB     0

SQRT32              PSHS    A,B,X,Y,U

                    ; Calculate an initial guess.
                    LDX     #1
                    STX     SQRT32_GUESS,PCR
                    STX     SQRT32_GUESSSQ_LO,PCR
                    LDX     #0
                    STX     SQRT32_GUESSSQ_HI,PCR

SQRT32_UPSHIFT      LDU     SQRT32_GUESSSQ_HI,PCR
                    CMPU    SQRT32_INPUT_HI,PCR
                    BHI     SQRT32_NEWTON_RAPHSON_IN
                    BLO     SQRT32_UPSHIFT_NEXT
                    LDU     SQRT32_GUESSSQ_LO,PCR
                    CMPU    SQRT32_INPUT_LO,PCR
                    BHI     SQRT32_NEWTON_RAPHSON_IN
SQRT32_UPSHIFT_NEXT
                    LSL     SQRT32_GUESS+1,PCR
                    ROL     SQRT32_GUESS,PCR
                    BMI     SQRT32_UPSHIFT_OVERFLOW
                    LSL     SQRT32_GUESSSQ_LO+1,PCR
                    ROL     SQRT32_GUESSSQ_LO  ,PCR
                    ROL     SQRT32_GUESSSQ_HI+1,PCR
                    ROL     SQRT32_GUESSSQ_HI  ,PCR
                    BRA     SQRT32_UPSHIFT

SQRT32_UPSHIFT_OVERFLOW
                    LDX     #$7FFF
                    STX     SQRT32_GUESS,PCR

SQRT32_NEWTON_RAPHSON_IN
                    LDD     SQRT32_GUESS,PCR
SQRT32_NEWTON_RAPHSON
                    STD     SQRT32_GUESS,PCR

                    LDX     SQRT32_INPUT_HI,PCR
                    STX     DIV32_16_NUMER_HI,PCR
                    LDX     SQRT32_INPUT_LO,PCR
                    STX     DIV32_16_NUMER_LO,PCR
                    LDX     SQRT32_GUESS,PCR
                    STX     DIV32_16_DENOM,PCR

                    LBSR    UDIV32_16

                    ; (*1)
                    ADDD    DIV32_16_DENOM,PCR  ; Quotient
                    LSRA
                    RORB
                    ; D is new guess.
                    CMPD    SQRT32_GUESS,PCR
                    BLO     SQRT32_NEWTON_RAPHSON

SQRT32_NEWTON_RAPHSON_DONE
                    LDD     SQRT32_GUESS,PCR
                    STD     SQRT32_INPUT_HI,PCR
                    PULS    A,B,X,Y,U,PC


