; Parameter in r0: unsigned value
; Return value in r0: principal square root of r0

00000404      push    r4
00000408      mov     r12,r0
0000040C      mov     r1,1h

BRANCH_A:   ; Sanitise input
00000410      cmp     r0,r1         ;\
00000414      movhi   r0,r0,lsr 1h  ;} Next result = minimum 2^n >= sqrt(radicand), for integer n
00000418      movhi   r1,r1,lsl 1h  ;/
0000041C      bhi     BRANCH_A

BRANCH_B:   ; Main loop
00000420      mov     r0,r12        ; r0 = radicand
00000424      mov     r4,r1         ; Current result = next result
00000428      mov     r3,0h         ; r3 = 0
0000042C      mov     r2,r1         ; r2 = next result

BRANCH_C:   ; Operation 1
00000430      cmp     r2,r0,lsr 1h  ;\
00000434      movls   r2,r2,lsl 1h  ;} r2 = maximum 2^n * next result <= radicand, for integer n
00000438      bcc     BRANCH_C      ;/

BRANCH_D:   ; Operation 2
0000043C      cmp     r0,r2         ;            If radicand > r2:
00000440      adc     r3,r3,r3      ; Double r3      ++r3
00000444      subcs   r0,r0,r2      ;                r0 -= r2
00000448      teq     r2,r1         ;\
0000044C      movne   r2,r2,lsr 1h  ;} For powers of two from r2 to r1
00000450      bne     BRANCH_D      ;/
; Flow control
00000454      add     r1,r1,r3      ;\
00000458      movs    r1,r1,lsr 1h  ;} r1 = avg(r1,r3)
0000045C      cmp     r1,r4         ;\
00000460      bcc     BRANCH_B      ;} Loop until current result < r3
00000464      mov     r0,r4         ; Return current result
00000468      pop     r4
0000046C      bx      r14


; halfword Sqrt(word radicand)
; {
;     word r0,r1,r2,r3,r4,r12;
;     signed word cmp;
;
;     r0 = radicand;
;     r12 = r0;
;     r1 = 1;
;
; BRANCH_A:
;     if (r0 > r1)
;     {
;         r0 >>= 1;
;         r1 <<= 1;
;         goto BRACH_A;
;     }
;
; BRANCH_B:
;     r0 = r12;
;     r4 = r1;
;     r3 = 0;
;     r2 = r1;
;
; BRANCH_C:
;     cmp = r2 - r0>>1;
;     if (cmp <= 0)
;         r2 <<= 1;
;     if (cmp < 0)
;         goto BRANCH_C;
;
; BRANCH_D:
;     r3 += r3;
;     if (r0 >= r2)
;     {
;         ++r3;
;         r0 -= r2;
;     }
;     if (r2 != r1)
;     {
;         r2 >>= 1;
;         goto BRANCH_D;
;     }
;     r1 += r3;
;     r1 >>= 1;
;     if (r1 < r4)
;         goto BRANCH_B;
;     r0 = r4;
;     return r0;
; }

; halfword Sqrt(word radicand)
; {
;     word r0;    //
;     word r1;    // next_result
;     word r2;    //
;     word r3;    //
;     word r4;    // current_result
;     word r12;   // radicand
; 
;     r0 = radicand;
;     next_result = 1;
; 
;     // next_result = minimum 2^n >= sqrt(r0)
;     while (r0 > next_result)
;     {
;         r0 /= 2;
;         next_result *= 2;
;     }
; 
;     do
;     {
;         r0 = radicand;
;         r2 = current_result = next_result;
;         r3 = 1;
; 
;         // r2 = maximum 2^n * next_result <= radicand
;         while (r2 <= r0/2)
;             r2 *= 2;
; 
;         r0 -= r2;   // r0 = difference from r2 to radicand
;         for (; r2 != next_result; r2 /= 2)
;         {
;             r3 *= 2;
;             if (r0 >= r2)
;             {
;                 ++r3;
;                 r0 -= r2;
;             }
;         }
; 
;         next_result = mean(next_result, r3);    // 
;     } while (next_result < current_result);
; 
;     return current_result;
; }

; r0=1
{
00000408      mov     r12,r0        ; r12=1
0000040C      mov     r1,1h         ; r1=1
BRANCH_A:
00000410      cmp     r0,r1         ; r0==r1
00000414      movhi   r0,r0,lsr 1h  ; false
00000418      movhi   r1,r1,lsl 1h  ; false
0000041C      bhi     BRANCH_A      ; false
BRANCH_B:
00000420      mov     r0,r12        ; r0=1
00000424      mov     r4,r1         ; r4=1
00000428      mov     r3,0h         ; r3=0
0000042C      mov     r2,r1         ; r2=1
BRANCH_C:
00000430      cmp     r2,r0,lsr 1h  ; r2>r0/2
00000434      movls   r2,r2,lsl 1h  ; false
00000438      bcc     BRANCH_C      ; false
BRANCH_D:
0000043C      cmp     r0,r2         ; r0==r2
00000440      adc     r3,r3,r3      ; r3=1
00000444      subcs   r0,r0,r2      ; r0=0
00000448      teq     r2,r1         ; r2==r0
0000044C      movne   r2,r2,lsr 1h  ; false
00000450      bne     BRANCH_D      ; false
00000454      add     r1,r1,r3      ; r1=2
00000458      movs    r1,r1,lsr 1h  ; r1=1
0000045C      cmp     r1,r4         ; r1==r4
00000460      bcc     BRANCH_B      ; false
; r4=1
}

; r0=4
{
00000408      mov     r12,r0        ; r12=4
0000040C      mov     r1,1h         ; r1=1
BRANCH_A:
00000410      cmp     r0,r1         ; r0>r1 r0==r1
00000414      movhi   r0,r0,lsr 1h  ; r0=2  false
00000418      movhi   r1,r1,lsl 1h  ; r1=2  false
0000041C      bhi     BRANCH_A      ; true  false
BRANCH_B:
00000420      mov     r0,r12        ; r0=4
00000424      mov     r4,r1         ; r4=2
00000428      mov     r3,0h         ; r3=0
0000042C      mov     r2,r1         ; r2=2
BRANCH_C:
00000430      cmp     r2,r0,lsr 1h  ; r2==r0/2
00000434      movls   r2,r2,lsl 1h  ; r2=4
00000438      bcc     BRANCH_C      ; false
BRANCH_D:
0000043C      cmp     r0,r2         ; r0==r2 r0<r2
00000440      adc     r3,r3,r3      ; r3=1   r3=2
00000444      subcs   r0,r0,r2      ; r0=0   false
00000448      teq     r2,r1         ; r2!=r1 r2==r1
0000044C      movne   r2,r2,lsr 1h  ; r2=2   false
00000450      bne     BRANCH_D      ; true   false
00000454      add     r1,r1,r3      ; r1=4
00000458      movs    r1,r1,lsr 1h  ; r1=2
0000045C      cmp     r1,r4         ; r1==r4
00000460      bcc     BRANCH_B      ; false
; r4=2
}

; r0=9
{
00000408      mov     r12,r0        ; r12=9
0000040C      mov     r1,1h         ; r1=1
BRANCH_A:
00000410      cmp     r0,r1         ; r0>r1 r0>r1 r0<r1
00000414      movhi   r0,r0,lsr 1h  ; r0=4  r0=2  false
00000418      movhi   r1,r1,lsl 1h  ; r1=2  r1=4  false
0000041C      bhi     BRANCH_A      ; true  true  false
BRANCH_B:
00000420      mov     r0,r12        ; r0=9           r0=9
00000424      mov     r4,r1         ; r4=4           r4=3
00000428      mov     r3,0h         ; r3=0           r3=0
0000042C      mov     r2,r1         ; r2=4           r2=3
BRANCH_C:                                            
00000430      cmp     r2,r0,lsr 1h  ; r2==r0/2       r2<r0/2 r2>r0/2
00000434      movls   r2,r2,lsl 1h  ; r2=8           r2=6    false
00000438      bcc     BRANCH_C      ; false          true    false
BRANCH_D:                                            
0000043C      cmp     r0,r2         ; r0>r2  r0<r2   r0>r2  r0==r2
00000440      adc     r3,r3,r3      ; r3=1   r3=2    r3=1   r3=3
00000444      subcs   r0,r0,r2      ; r0=1   false   r0=3   r0=0
00000448      teq     r2,r1         ; r2!=r1 r2==r1  r2!=r1 r2==r1
0000044C      movne   r2,r2,lsr 1h  ; r2=4   false   r2=3   false
00000450      bne     BRANCH_D      ; true   false   true   false
00000454      add     r1,r1,r3      ; r1=6           r1=6
00000458      movs    r1,r1,lsr 1h  ; r1=3           r1=3
0000045C      cmp     r1,r4         ; r1<r4          r1==r4
00000460      bcc     BRANCH_B      ; true           false
; r4=3
}