//1 = movw wird unterstüzt, 0 = konvetionelles mov verwenden
.set useMOVW = 1
//1 = Geschwindigkeitsoptimierte Version (~600clk, ~1300byte)
//0 = Platzoptimierte Version (~1300 clk, ~150byte)
.set useSpeedOptimized = 1
//1 = sichert die verwendeten Register auf dem Stack
.set saveRegs = 0


wurzel32fast:
/* #######################################################################
Based on an Algorithm by Jim Ulery
  http://www.azillionmonkeys.com/qed/ulerysqroot.pdf

  Ported to ASM by Läubi, 2008

  Input r5:r4:r3:r2
  Output r1:r0

  ! Achtung! Die Eingabe wird hierbei zerstört!
  ! Wenn diese noch benötigt wird vorher sichern!

  C-Version:
  static unsigned julery_isqrt(unsigned long val) {
      unsigned long temp, g=0, b = 0x8000, bshft = 15;
      do {
          if ( val >= ( temp = (((g << 1) + b)<<bshft--) ) ) {
             g += b;
             val -= temp;
          }
      } while (b >>= 1);
      return g;
  }
####################################################################### */

//Macros
.macro WURZEL_SHIFTLEFT
  //@0 @1 @2 @3 << 1
  lsl @3
  rol @2
  rol @1
  rol @0
.endmacro

.macro WURZEL_SHIFTRIGHT
  //@0 @1 @2 @3 >> 1
  lsr @0
  ror @1
  ror @2
  ror @3
.endmacro

.macro WURZEL_RORRIGHT
  ror @0
  ror @1
  ror @2
  ror @3
.endmacro

//shift um 0 ... es wird also nur gerechnet
.macro WURZEL_lshift0  //--> 6/7 clocks
  //Highbytes clearen
  clr r19
  clr r18
  //temp = g
  .if useMOVW == 1
    movw r17:r16, YH:YL
  .else
    mov r17, YH
    mov r16, YL
  .endif
  //temp = temp + (b>>1)
  subi r16, BYTE1(~@0+1)
  sbci r17, BYTE2(~@0+1)
  sbci r18, BYTE3(~@0+1)
  //sbci r19, BYTE3(~@0+1)
  //es gibt hier kein weiteres carry!
  //Wir sind also fertig
.endmacro

//es wird gerechnet und um 8 geschoben
.macro WURZEL_lshift8  //--> 7 clocks
  //r16 wird 0 sein wegen schieben um 8 nach links am ende
  clr r16
  //Highbyte clearen
  clr r19
  //temp = g (allerdings schon um 8 geschoben)
  //movw können wir hier leider nicht einsetzen
  mov r18, YH
  mov r17, YL
  //temp = temp + (b>>1) (allerdings schon um 8 geschoben)
  subi r17, BYTE1(~@0+1)
  sbci r18, BYTE2(~@0+1)
  sbci r19, BYTE3(~@0+1)
  //temp ist schon automatisch geschoben!
  //Wir sind also fertig
.endmacro

//es wird gerechnet und um 16 geschoben
.macro WURZEL_lshift16  //--> 5/6 clocks
  //r17:r16 wird 0 sein wegen schieben um 16 nach links am ende
  clr r16
  clr r17
  //temp = g (allerdings schon um 16 geschoben)
  .if useMOVW == 1
    movw r19:r18, YH:YL
  .else
    mov r19, YH
    mov r18, YL
  .endif
  //temp = temp + (b>>1) (allerdings schon um 16 geschoben)
  subi r18, LOW(~@0+1)
  sbci r19, HIGH(~@0+1)
  //temp ist schon automatisch um 16 geschoben!
  //Wir sind also fertig
.endmacro


/*
Outerlopp umgeschrieben zu
  temp = g;
  temp = temp + (b >> 1);
  if (b == 1) {
    temp = temp << 1;
    temp = temp|1; //oder temp+1
    temp = temp << bshft;
  } else {
    temp = temp <<(bshft+1);
  }
  
  if ( val >= temp) {
    g += b;
    val -= temp;
  }
  Auf diese Weise können wir zuweisung addition und shift
  zusammenfassen
*/

// @0 = bshft
.macro WURZEL32_OUTERLOOP
    //X = b >> 1
    //wir schieben leztendlich um @0+1 = 8    
    .if @0 == 0
      //sonderfall, hier schieben wir eine 1 von b raus
      //die müssen wir später wieder reinschieben
      //dafür müssen wir b auch nicht auf temp addieren
      //Highbytes clearen
      clr r19
      clr r18
      //temp = g
      .if useMOVW == 1
        movw r17:r16, YH:YL
      .else
        mov r17, YH
        mov r16, YL
      .endif
      //temp = temp + b entfällt (b >> 1 = 0)
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      //"rausgeschobene 1" addieren
      //es gibt keinen übertrag da wir gerade um einen nach links
      //geschoben haben
      ori r16, 1
    .elif @0 == 1
      WURZEL_lshift0 (@1>>1)
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
    .elif @0 == 2
      WURZEL_lshift0 (@1>>1)
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
    .elif @0 == 3
      WURZEL_lshift0 (@1>>1)
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
    .elif @0 == 4
      WURZEL_lshift8 (@1>>1)
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
    .elif @0 == 5
      WURZEL_lshift8 (@1>>1)
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
    .elif @0 == 6
      WURZEL_lshift8 (@1>>1)
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
    .elif @0 == 7
      WURZEL_lshift8 (@1>>1)
    .elif @0 == 8
      WURZEL_lshift8 (@1>>1)
      WURZEL_SHIFTLEFT r19, r18, r17, r16
    .elif @0 == 9
      WURZEL_lshift8 (@1>>1)
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
    .elif @0 == 10
      WURZEL_lshift8 (@1>>1)
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
    .elif @0 == 11
      /*WURZEL_lshift8 (@1>>1)
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16
      WURZEL_SHIFTLEFT r19, r18, r17, r16*/
      WURZEL_lshift16 (@1>>1)
      WURZEL_RORRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
    .elif @0 == 12
      WURZEL_lshift16 (@1>>1)
      WURZEL_RORRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
    .elif @0 == 13
      WURZEL_lshift16 (@1>>1)
      WURZEL_RORRIGHT r19, r18, r17, r16
      WURZEL_SHIFTRIGHT r19, r18, r17, r16
    .elif @0 == 14
      WURZEL_lshift16 (@1>>1)
      WURZEL_RORRIGHT r19, r18, r17, r16
    .elif @0 == 15
      WURZEL_lshift16 (@1>>1)
    .else
      //das sollte nicht passieren, alle Fälle sollten oben abgehandelt sein!
      .error "ooops!!!"
    .endif
    //val >= temp ?
    cp r2, r16
    cpc r3, r17
    cpc r4, r18
    cpc r5, r19
    brlo wurzel32_2f_wd
      //g + = b
      //2-complement = invers+1
      subi YL,  LOW(~@1+1)
      sbci YH, HIGH(~@1+1)
      //val -=temp
      sub r2, r16
      sbc r3, r17
      sbc r4, r18
      sbc r5, r19
  wurzel32_2f_wd:
  
.endmacro

//function
.if useSpeedOptimized == 1
  //Register sichern
  .if saveRegs == 1
    push XL
    push XH
    push YL
    push YH
    push r16
    push r17
    push r18
    push r19
  .endif
  
  //YH:YL = g = 0
  clr YL
  clr YH
    //unrooling outer loop @0 = shiftweite, @1 = b
    WURZEL32_OUTERLOOP 15, (0x8000 >> 0)  
    WURZEL32_OUTERLOOP 14, (0x8000 >> 1)
    WURZEL32_OUTERLOOP 13, (0x8000 >> 2)  
    WURZEL32_OUTERLOOP 12, (0x8000 >> 3)  
    WURZEL32_OUTERLOOP 11, (0x8000 >> 4)
    WURZEL32_OUTERLOOP 10, (0x8000 >> 5)  
    WURZEL32_OUTERLOOP  9, (0x8000 >> 6)  
    WURZEL32_OUTERLOOP  8, (0x8000 >> 7)
    WURZEL32_OUTERLOOP  7, (0x8000 >> 8)
    WURZEL32_OUTERLOOP  6, (0x8000 >> 9)
    WURZEL32_OUTERLOOP  5, (0x8000 >> 10)
    WURZEL32_OUTERLOOP  4, (0x8000 >> 11)
    WURZEL32_OUTERLOOP  3, (0x8000 >> 12)
    WURZEL32_OUTERLOOP  2, (0x8000 >> 13)
    WURZEL32_OUTERLOOP  1, (0x8000 >> 14)
    WURZEL32_OUTERLOOP  0, (0x8000 >> 15)
  //Register wieder herstellen
  mov r0, YL
  mov r1, YH
  .if saveRegs == 1
    pop r19
    pop r18
    pop r17
    pop r16
    pop YH
    pop YL
    pop XH
    pop XL
  .endif
ret

.else
//Register sichern
  .if saveRegs == 1
  push XL
  push XH
  push r17
  push r16
  push r6
  push r7
  push r8
  push r9
  push r10
  .endif
  //X = b
  ldi XL,  LOW(0x8000)
  ldi XH, HIGH(0x8000)
  //r1:r0 = g = 0
  clr r0
  clr r1
  //temp = bshft
  ldi r17, 15
  clr r16
  wurzel32_2_wloop:
    //temp = g
    clr r9
    clr r8
    mov r7, r1
    mov r6, r0
    //g << 1
    lsl r6
    rol r7
    rol r8
    //leztes rol kann ausgelassen werden da g maximal 16 bit breit
    //und r9 = 0
    //+b
    add r6, XL
    adc r7, XH
    adc r8, r16
    adc r9, r16
    // << bshft
    mov r10, r17
    wurzel32_2_wl:
      lsl r6
      rol r7
      rol r8
      rol r9
      dec r17
    brne wurzel32_2_wl
    //bshft--
    mov r17, r10
    dec r17
    //val >= temp
    cp r2, r6
    cpc r3, r7
    cpc r4, r8
    cpc r5, r9
    brlo wurzel32_2_wd
    //g + = b
    add r0, XL
    adc r1, XH
    //val -=temp
    sub r2, r6
    sbc r3, r7
    sbc r4, r8
    sbc r5, r9
  //b >> 1 != 0?
  wurzel32_2_wd:
    lsr XH
    ror XL
    cpi XL, 1
    cpc XH, r16
    brne wurzel32_2_wloop
  //Register wieder herstellen
  .if saveRegs == 1
    pop r10
    pop r9
    pop r8
    pop r7
    pop r6
    pop r16
    pop r17
    pop XH
    pop XL
  .endif
ret  
.endif