#include "rt_def.h"

#include "rt_sqrt.h"

/*
  C version of fixed-point Square Root functions
 */

long FixedSqrtLP(long n)  // Low  Precision (8.8)
{
    /*
       Not used, so unimplemented.
       If it matters, just copy the first half of the HP version,
       and multiply the answer by 256.
     */
    STUB_FUNCTION;

    return 0;
}

long FixedSqrtHP(long n)  // High Precision (8.16)
{
    /* This is more or less a direct C transliteration of the asm code.
       I've replaced right shifting with division, since right shifting
       signed values is undefined in ANSI C (though it usually works).
       ROTT does not use this routine heavily. */
    unsigned long root, mask, val;
    signed long d;

    root = 0;
    mask = 0x40000000;
    val = (unsigned long)n;
hp1:
    d = val;
    d -= mask;
    if (d < 0)
        goto hp2;
    d -= root;
    if (d < 0)
        goto hp2;

    val = d;
    root /= 2;
    root |= mask;
    mask /= 4;
    if (mask != 0)
        goto hp1;
    else
        goto hp5;
hp2:
    root /= 2;
    mask /= 4;
    if (mask != 0)
        goto hp1;

hp5:
    mask = 0x00004000;
    root <<= 16;
    val <<= 16;
hp3:
    /* use add here to properly emulate the asm - SBF */
    if ((root+mask) > val)
        goto hp4;

    val -= (root+mask);

    root /= 2;
    root |= mask;
    mask /= 4;
    if (mask != 0)
        goto hp3;
    else
        goto hp6;
hp4:
    root /= 2;
    mask /= 4;
    if (mask != 0)
        goto hp3;
hp6:

    return (long)root;
}
