/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License (the "License").
 * You may not use this file except in compliance with the License.
 *
 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
 * or http://www.opensolaris.org/os/licensing.
 * See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * When distributing Covered Code, include this CDDL HEADER in each
 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
 * If applicable, add the following below this CDDL HEADER, with the
 * fields enclosed by brackets "[]" replaced with your own identifying
 * information: Portions Copyright [yyyy] [name of copyright owner]
 *
 * CDDL HEADER END
 */

/*
 * Copyright 2011 Nexenta Systems, Inc.  All rights reserved.
 */
/*
 * Copyright 2006 Sun Microsystems, Inc.  All rights reserved.
 * Use is subject to license terms.
 */

#ifdef __RESTRICT
#define restrict _Restrict
#else
#define restrict
#endif

extern const double __vlibm_TBL_atan1[];

static const double
pio4    =  7.8539816339744827900e-01,
pio2    =  1.5707963267948965580e+00,
pi      =  3.1415926535897931160e+00;

static const float
zero    =  0.0f,
one     =  1.0f,
q1      = -3.3333333333296428046e-01f,
q2      =  1.9999999186853752618e-01f,
twop24  =  16777216.0f;

void
__vatan2f(int n, float * restrict y, int stridey, float * restrict x,
        int stridex, float * restrict z, int stridez)
{
        float           x0, x1, x2, y0, y1, y2, *pz0 = 0, *pz1, *pz2;
        double          ah0, ah1, ah2;
        double          t0, t1, t2;
        double          sx0, sx1, sx2;
        double          sign0, sign1, sign2;
        int             i, k0 = 0, k1, k2, hx, sx, sy;
        int             hy0, hy1, hy2;
        float           base0 = 0.0, base1, base2;
        double          num0, num1, num2;
        double          den0, den1, den2;
        double          dx0, dx1, dx2;
        double          dy0, dy1, dy2;
        double          db0, db1, db2;

        do
        {
loop0:
                hy0 = *(int*)y;
                hx = *(int*)x;
                sign0 = one;
                sy = hy0 & 0x80000000;
                hy0 &= ~0x80000000;

                sx = hx & 0x80000000;
                hx &= ~0x80000000;

                if (hy0 > hx)
                {
                        x0 = *y;
                        y0 = *x;
                        i = hx;
                        hx = hy0;
                        hy0 = i;
                        if (sy)
                        {
                                x0 = -x0;
                                sign0 = -sign0;
                        }
                        if (sx)
                        {
                                y0 = -y0;
                                ah0 = pio2;
                        }
                        else
                        {
                                ah0 = -pio2;
                                sign0 = -sign0;
                        }
                }
                else
                {
                        y0 = *y;
                        x0 = *x;
                        if (sy)
                        {
                                y0 = -y0;
                                sign0 = -sign0;
                        }
                        if (sx)
                        {
                                x0 = -x0;
                                ah0 = -pi;
                                sign0 = -sign0;
                        }
                        else
                                ah0 = zero;
                }

                if (hx >= 0x7f800000 || hx - hy0 >= 0x0c800000)
                {
                        if (hx >= 0x7f800000)
                        {
                                if (hx ^ 0x7f800000) /* nan */
                                        ah0 =  x0 + y0;
                                else if (hy0 >= 0x7f800000)
                                        ah0 += pio4;
                        }
                        else if ((int) ah0 == 0)
                                ah0 = y0 / x0;
                        *z = (sign0 == one) ? ah0 : -ah0;
/* sign0*ah0 would change nan behavior relative to previous release */
                        x += stridex;
                        y += stridey;
                        z += stridez;
                        i = 0;
                        if (--n <= 0)
                                break;
                        goto loop0;
                }
                if (hy0 < 0x00800000) {
                        if (hy0 == 0)
                        {
                                *z = sign0 * (float) ah0;
                                x += stridex;
                                y += stridey;
                                z += stridez;
                                i = 0;
                                if (--n <= 0)
                                        break;
                                goto loop0;
                        }
                        y0 *= twop24; /* scale subnormal y */
                        x0 *= twop24; /* scale possibly subnormal x */
                        hy0 = *(int*)&y0;
                        hx = *(int*)&x0;
                }
                pz0 = z;

                k0 = (hy0 - hx + 0x3f800000) & 0xfff80000;
                if (k0 >= 0x3C800000)          /* if |x| >= (1/64)... */
                {
                        *(int*)&base0 = k0;
                        k0 = (k0 - 0x3C800000) >> 18; /* (index >> 19) << 1) */
                        k0 += 4;
                                /* skip over 0,0,pi/2,pi/2 */
                }
                else                            /* |x| < 1/64 */
                {
                        k0 = 0;
                        base0 = zero;
                }

                x += stridex;
                y += stridey;
                z += stridez;
                i = 1;
                if (--n <= 0)
                        break;


loop1:
                hy1 = *(int*)y;
                hx = *(int*)x;
                sign1 = one;
                sy = hy1 & 0x80000000;
                hy1 &= ~0x80000000;

                sx = hx & 0x80000000;
                hx &= ~0x80000000;

                if (hy1 > hx)
                {
                        x1 = *y;
                        y1 = *x;
                        i = hx;
                        hx = hy1;
                        hy1 = i;
                        if (sy)
                        {
                                x1 = -x1;
                                sign1 = -sign1;
                        }
                        if (sx)
                        {
                                y1 = -y1;
                                ah1 = pio2;
                        }
                        else
                        {
                                ah1 = -pio2;
                                sign1 = -sign1;
                        }
                }
                else
                {
                        y1 = *y;
                        x1 = *x;
                        if (sy)
                        {
                                y1 = -y1;
                                sign1 = -sign1;
                        }
                        if (sx)
                        {
                                x1 = -x1;
                                ah1 = -pi;
                                sign1 = -sign1;
                        }
                        else
                                ah1 = zero;
                }

                if (hx >= 0x7f800000 || hx - hy1 >= 0x0c800000)
                {
                        if (hx >= 0x7f800000)
                        {
                                if (hx ^ 0x7f800000) /* nan */
                                        ah1 =  x1 + y1;
                                else if (hy1 >= 0x7f800000)
                                        ah1 += pio4;
                        }
                        else if ((int) ah1 == 0)
                                ah1 = y1 / x1;
                        *z = (sign1 == one)? ah1 : -ah1;
                        x += stridex;
                        y += stridey;
                        z += stridez;
                        i = 1;
                        if (--n <= 0)
                                break;
                        goto loop1;
                }
                if (hy1 < 0x00800000) {
                        if (hy1 == 0)
                        {
                                *z = sign1 * (float) ah1;
                                x += stridex;
                                y += stridey;
                                z += stridez;
                                i = 1;
                                if (--n <= 0)
                                        break;
                                goto loop1;
                        }
                        y1 *= twop24; /* scale subnormal y */
                        x1 *= twop24; /* scale possibly subnormal x */
                        hy1 = *(int*)&y1;
                        hx = *(int*)&x1;
                }
                pz1 = z;

                k1 = (hy1 - hx + 0x3f800000) & 0xfff80000;
                if (k1 >= 0x3C800000)          /* if |x| >= (1/64)... */
                {
                        *(int*)&base1 = k1;
                        k1 = (k1 - 0x3C800000) >> 18; /* (index >> 19) << 1) */
                        k1 += 4;
                                /* skip over 0,0,pi/2,pi/2 */
                }
                else                            /* |x| < 1/64 */
                {
                        k1 = 0;
                        base1 = zero;
                }

                x += stridex;
                y += stridey;
                z += stridez;
                i = 2;
                if (--n <= 0)
                        break;

loop2:
                hy2 = *(int*)y;
                hx = *(int*)x;
                sign2 = one;
                sy = hy2 & 0x80000000;
                hy2 &= ~0x80000000;

                sx = hx & 0x80000000;
                hx &= ~0x80000000;

                if (hy2 > hx)
                {
                        x2 = *y;
                        y2 = *x;
                        i = hx;
                        hx = hy2;
                        hy2 = i;
                        if (sy)
                        {
                                x2 = -x2;
                                sign2 = -sign2;
                        }
                        if (sx)
                        {
                                y2 = -y2;
                                ah2 = pio2;
                        }
                        else
                        {
                                ah2 = -pio2;
                                sign2 = -sign2;
                        }
                }
                else
                {
                        y2 = *y;
                        x2 = *x;
                        if (sy)
                        {
                                y2 = -y2;
                                sign2 = -sign2;
                        }
                        if (sx)
                        {
                                x2 = -x2;
                                ah2 = -pi;
                                sign2 = -sign2;
                        }
                        else
                                ah2 = zero;
                }

                if (hx >= 0x7f800000 || hx - hy2 >= 0x0c800000)
                {
                        if (hx >= 0x7f800000)
                        {
                                if (hx ^ 0x7f800000) /* nan */
                                        ah2 =  x2 + y2;
                                else if (hy2 >= 0x7f800000)
                                        ah2 += pio4;
                        }
                        else if ((int) ah2 == 0)
                                ah2 = y2 / x2;
                        *z = (sign2 == one)? ah2 : -ah2;
                        x += stridex;
                        y += stridey;
                        z += stridez;
                        i = 2;
                        if (--n <= 0)
                                break;
                        goto loop2;
                }
                if (hy2 < 0x00800000) {
                        if (hy2 == 0)
                        {
                                *z = sign2 * (float) ah2;
                                x += stridex;
                                y += stridey;
                                z += stridez;
                                i = 2;
                                if (--n <= 0)
                                        break;
                                goto loop2;
                        }
                        y2 *= twop24; /* scale subnormal y */
                        x2 *= twop24; /* scale possibly subnormal x */
                        hy2 = *(int*)&y2;
                        hx = *(int*)&x2;
                }

                pz2 = z;

                k2 = (hy2 - hx + 0x3f800000) & 0xfff80000;
                if (k2 >= 0x3C800000)          /* if |x| >= (1/64)... */
                {
                        *(int*)&base2 = k2;
                        k2 = (k2 - 0x3C800000) >> 18; /* (index >> 19) << 1) */
                        k2 += 4;
                                /* skip over 0,0,pi/2,pi/2 */
                }
                else                            /* |x| < 1/64 */
                {
                        k2 = 0;
                        base2 = zero;
                }

                goto endloop;

endloop:

                ah2 += __vlibm_TBL_atan1[k2];
                ah1 += __vlibm_TBL_atan1[k1];
                ah0 += __vlibm_TBL_atan1[k0];

                db2 = base2;
                db1 = base1;
                db0 = base0;
                dy2 = y2;
                dy1 = y1;
                dy0 = y0;
                dx2 = x2;
                dx1 = x1;
                dx0 = x0;

                num2 = dy2 - dx2 * db2;
                den2 = dx2 + dy2 * db2;

                num1 = dy1 - dx1 * db1;
                den1 = dx1 + dy1 * db1;

                num0 = dy0 - dx0 * db0;
                den0 = dx0 + dy0 * db0;

                t2 = num2 / den2;
                t1 = num1 / den1;
                t0 = num0 / den0;

                sx2 = t2 * t2;
                sx1 = t1 * t1;
                sx0 = t0 * t0;

                t2 += t2 * sx2 * (q1 + sx2 * q2);
                t1 += t1 * sx1 * (q1 + sx1 * q2);
                t0 += t0 * sx0 * (q1 + sx0 * q2);

                t2 += ah2;
                t1 += ah1;
                t0 += ah0;

                *pz2 = sign2 * t2;
                *pz1 = sign1 * t1;
                *pz0 = sign0 * t0;

                x += stridex;
                y += stridey;
                z += stridez;
                i = 0;
        } while (--n > 0);

        if (i > 1)
        {
                ah1 += __vlibm_TBL_atan1[k1];
                t1 = (y1 - x1 * (double)base1) /
                        (x1 + y1 * (double)base1);
                sx1 = t1 * t1;
                t1 += t1 * sx1 * (q1 + sx1 * q2);
                t1 += ah1;
                *pz1 = sign1 * t1;
        }

        if (i > 0)
        {
                ah0 += __vlibm_TBL_atan1[k0];
                t0 = (y0 - x0 * (double)base0) /
                        (x0 + y0 * (double)base0);
                sx0 = t0 * t0;
                t0 += t0 * sx0 * (q1 + sx0 * q2);
                t0 += ah0;
                *pz0 = sign0 * t0;
        }
}