/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "_cxcore.h"

#ifdef HAVE_CONFIG_H
#include <cvconfig.h>
#endif

#define ICV_MATH_BLOCK_SIZE  256

#define _CV_SQRT_MAGIC     0xbe6f0000

#define _CV_SQRT_MAGIC_DBL CV_BIG_UINT(0xbfcd460000000000)

#define _CV_ATAN_CF0  (-15.8131890796f)
#define _CV_ATAN_CF1  (61.0941945596f)
#define _CV_ATAN_CF2  0.f /*(-0.140500406322f)*/

static const float icvAtanTab[8] = { 0.f + _CV_ATAN_CF2, 90.f - _CV_ATAN_CF2,
    180.f - _CV_ATAN_CF2, 90.f + _CV_ATAN_CF2,
    360.f - _CV_ATAN_CF2, 270.f + _CV_ATAN_CF2,
    180.f + _CV_ATAN_CF2, 270.f - _CV_ATAN_CF2
};

static const int icvAtanSign[8] =
    { 0, 0x80000000, 0x80000000, 0, 0x80000000, 0, 0, 0x80000000 };

CV_IMPL float
cvFastArctan( float y, float x )
{
    Cv32suf _x, _y;
    int ix, iy, ygx, idx;
    double z;

    _x.f = x; _y.f = y;
    ix = _x.i; iy = _y.i;
    idx = (ix < 0) * 2 + (iy < 0) * 4;

    ix &= 0x7fffffff;
    iy &= 0x7fffffff;

    ygx = (iy <= ix) - 1;
    idx -= ygx;

    idx &= ((ix == 0) - 1) | ((iy == 0) - 1);

    /* swap ix and iy if ix < iy */
    ix ^= iy & ygx;
    iy ^= ix & ygx;
    ix ^= iy & ygx;

    _y.i = iy ^ icvAtanSign[idx];

    /* ix = ix != 0 ? ix : 1.f */
    _x.i = ((ix ^ CV_1F) & ((ix == 0) - 1)) ^ CV_1F;
    
    z = _y.f / _x.f;
    return (float)((_CV_ATAN_CF0*fabs(z) + _CV_ATAN_CF1)*z + icvAtanTab[idx]);
}


IPCVAPI_IMPL( CvStatus, icvFastArctan_32f,
    (const float *__y, const float *__x, float *angle, int len ), (__y, __x, angle, len) )
{
    int i = 0;
    const int *y = (const int*)__y, *x = (const int*)__x;

    if( !(y && x && angle && len >= 0) )
        return CV_BADFACTOR_ERR;

    /* unrolled by 4 loop */
    for( ; i <= len - 4; i += 4 )
    {
        int j, idx[4];
        float xf[4], yf[4];
        double d = 1.;

        /* calc numerators and denominators */
        for( j = 0; j < 4; j++ )
        {
            int ix = x[i + j], iy = y[i + j];
            int ygx, k = (ix < 0) * 2 + (iy < 0) * 4;
            Cv32suf _x, _y;

            ix &= 0x7fffffff;
            iy &= 0x7fffffff;

            ygx = (iy <= ix) - 1;
            k -= ygx;

            k &= ((ix == 0) - 1) | ((iy == 0) - 1);

            /* swap ix and iy if ix < iy */
            ix ^= iy & ygx;
            iy ^= ix & ygx;
            ix ^= iy & ygx;
            
            _y.i = iy ^ icvAtanSign[k];

            /* ix = ix != 0 ? ix : 1.f */
            _x.i = ((ix ^ CV_1F) & ((ix == 0) - 1)) ^ CV_1F;
            idx[j] = k;
            yf[j] = _y.f;
            d *= (xf[j] = _x.f);
        }

        d = 1. / d;

        {
            double b = xf[2] * xf[3], a = xf[0] * xf[1];

            float z0 = (float) (yf[0] * xf[1] * b * d);
            float z1 = (float) (yf[1] * xf[0] * b * d);
            float z2 = (float) (yf[2] * xf[3] * a * d);
            float z3 = (float) (yf[3] * xf[2] * a * d);

            z0 = (float)((_CV_ATAN_CF0*fabs(z0) + _CV_ATAN_CF1)*z0 + icvAtanTab[idx[0]]);
            z1 = (float)((_CV_ATAN_CF0*fabs(z1) + _CV_ATAN_CF1)*z1 + icvAtanTab[idx[1]]);
            z2 = (float)((_CV_ATAN_CF0*fabs(z2) + _CV_ATAN_CF1)*z2 + icvAtanTab[idx[2]]);
            z3 = (float)((_CV_ATAN_CF0*fabs(z3) + _CV_ATAN_CF1)*z3 + icvAtanTab[idx[3]]);

            angle[i] = z0;
            angle[i+1] = z1;
            angle[i+2] = z2;
            angle[i+3] = z3;
        }
    }

    /* process the rest */
    for( ; i < len; i++ )
        angle[i] = cvFastArctan( __y[i], __x[i] );

    return CV_OK;
}


/* ************************************************************************** *\
   Fast cube root by Ken Turkowski
   (http://www.worldserver.com/turk/computergraphics/papers.html)
\* ************************************************************************** */
CV_IMPL  float  cvCbrt( float value )
{
    float fr;
    Cv32suf v, m;
    int ix, s;
    int ex, shx;

    v.f = value;
    ix = v.i & 0x7fffffff;
    s = v.i & 0x80000000;
    ex = (ix >> 23) - 127;
    shx = ex % 3;
    shx -= shx >= 0 ? 3 : 0;
    ex = (ex - shx) / 3; /* exponent of cube root */
    v.i = (ix & ((1<<23)-1)) | ((shx + 127)<<23);
    fr = v.f;

    /* 0.125 <= fr < 1.0 */
    /* Use quartic rational polynomial with error < 2^(-24) */
    fr = (float)(((((45.2548339756803022511987494 * fr +
    192.2798368355061050458134625) * fr +
    119.1654824285581628956914143) * fr +
    13.43250139086239872172837314) * fr +
    0.1636161226585754240958355063)/
    ((((14.80884093219134573786480845 * fr +
    151.9714051044435648658557668) * fr +
    168.5254414101568283957668343) * fr +
    33.9905941350215598754191872) * fr +
    1.0));

    /* fr *= 2^ex * sign */
    m.f = value;
    v.f = fr;
    v.i = (v.i + (ex << 23) + s) & (m.i*2 != 0 ? -1 : 0);
    return v.f;
}

//static const double _0_5 = 0.5, _1_5 = 1.5;


IPCVAPI_IMPL( CvStatus, icvInvSqrt_32f, (const float *src, float *dst, int len), (src, dst, len) )
{
    int i = 0;

    if( !(src && dst && len >= 0) )
        return CV_BADFACTOR_ERR;

    for( ; i < len; i++ )
        dst[i] = (float)(1.f/sqrt(src[i]));

    return CV_OK;
}


IPCVAPI_IMPL( CvStatus, icvSqrt_32f, (const float *src, float *dst, int len), (src, dst, len) )
{
    int i = 0;

    if( !(src && dst && len >= 0) )
        return CV_BADFACTOR_ERR;

    for( ; i < len; i++ )
        dst[i] = (float)sqrt(src[i]);

    return CV_OK;
}


IPCVAPI_IMPL( CvStatus, icvSqrt_64f, (const double *src, double *dst, int len), (src, dst, len) )
{
    int i = 0;

    if( !(src && dst && len >= 0) )
        return CV_BADFACTOR_ERR;

    for( ; i < len; i++ )
        dst[i] = sqrt(src[i]);

    return CV_OK;
}


IPCVAPI_IMPL( CvStatus, icvInvSqrt_64f, (const double *src, double *dst, int len), (src, dst, len) )
{
    int i = 0;

    if( !(src && dst && len >= 0) )
        return CV_BADFACTOR_ERR;

    for( ; i < len; i++ )
        dst[i] = 1./sqrt(src[i]);

    return CV_OK;
}

#define ICV_DEF_SQR_MAGNITUDE_FUNC(flavor, arrtype, magtype)\
static CvStatus CV_STDCALL                                  \
icvSqrMagnitude_##flavor(const arrtype* x, const arrtype* y,\
                         magtype* mag, int len)             \
{                                                           \
    int i;                                                  \
                                                            \
    for( i = 0; i <= len - 4; i += 4 )                      \
    {                                                       \
        magtype x0 = (magtype)x[i], y0 = (magtype)y[i];     \
        magtype x1 = (magtype)x[i+1], y1 = (magtype)y[i+1]; \
                                                            \
        x0 = x0*x0 + y0*y0;                                 \
        x1 = x1*x1 + y1*y1;                                 \
        mag[i] = x0;                                        \
        mag[i+1] = x1;                                      \
        x0 = (magtype)x[i+2], y0 = (magtype)y[i+2];         \
        x1 = (magtype)x[i+3], y1 = (magtype)y[i+3];         \
        x0 = x0*x0 + y0*y0;                                 \
        x1 = x1*x1 + y1*y1;                                 \
        mag[i+2] = x0;                                      \
        mag[i+3] = x1;                                      \
    }                                                       \
                                                            \
    for( ; i < len; i++ )                                   \
    {                                                       \
        magtype x0 = (magtype)x[i], y0 = (magtype)y[i];     \
        mag[i] = x0*x0 + y0*y0;                             \
    }                                                       \
                                                            \
    return CV_OK;                                           \
}


ICV_DEF_SQR_MAGNITUDE_FUNC( 32f, float, float )
ICV_DEF_SQR_MAGNITUDE_FUNC( 64f, double, double )