/*
 ** Copyright 2003-2010, VisualOn, Inc.
 **
 ** Licensed under the Apache License, Version 2.0 (the "License");
 ** you may not use this file except in compliance with the License.
 ** You may obtain a copy of the License at
 **
 **     http://www.apache.org/licenses/LICENSE-2.0
 **
 ** Unless required by applicable law or agreed to in writing, software
 ** distributed under the License is distributed on an "AS IS" BASIS,
 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 ** See the License for the specific language governing permissions and
 ** limitations under the License.
 */

/*___________________________________________________________________________
|                                                                           |
|  This file contains mathematic operations in fixed point.                 |
|                                                                           |
|  Isqrt()              : inverse square root (16 bits precision).          |
|  Pow2()               : 2^x  (16 bits precision).                         |
|  Log2()               : log2 (16 bits precision).                         |
|  Dot_product()        : scalar product of <x[],y[]>                       |
|                                                                           |
|  These operations are not standard double precision operations.           |
|  They are used where low complexity is important and the full 32 bits     |
|  precision is not necessary. For example, the function Div_32() has a     |
|  24 bits precision which is enough for our purposes.                      |
|                                                                           |
|  In this file, the values use theses representations:                     |
|                                                                           |
|  Word32 L_32     : standard signed 32 bits format                         |
|  Word16 hi, lo   : L_32 = hi<<16 + lo<<1  (DPF - Double Precision Format) |
|  Word32 frac, Word16 exp : L_32 = frac << exp-31  (normalised format)     |
|  Word16 int, frac        : L_32 = int.frac        (fractional format)     |
|___________________________________________________________________________|
*/
#include "typedef.h"
#include "basic_op.h"
#include "math_op.h"

/*___________________________________________________________________________
|                                                                           |
|   Function Name : Isqrt                                                   |
|                                                                           |
|       Compute 1/sqrt(L_x).                                                |
|       if L_x is negative or zero, result is 1 (7fffffff).                 |
|---------------------------------------------------------------------------|
|  Algorithm:                                                               |
|                                                                           |
|   1- Normalization of L_x.                                                |
|   2- call Isqrt_n(L_x, exponant)                                          |
|   3- L_y = L_x << exponant                                                |
|___________________________________________________________________________|
*/
Word32 Isqrt(                              /* (o) Q31 : output value (range: 0<=val<1)         */
        Word32 L_x                            /* (i) Q0  : input value  (range: 0<=val<=7fffffff) */
        )
{
    Word16 exp;
    Word32 L_y;
    exp = norm_l(L_x);
    L_x = (L_x << exp);                 /* L_x is normalized */
    exp = (31 - exp);
    Isqrt_n(&L_x, &exp);
    L_y = (L_x << exp);                 /* denormalization   */
    return (L_y);
}

/*___________________________________________________________________________
|                                                                           |
|   Function Name : Isqrt_n                                                 |
|                                                                           |
|       Compute 1/sqrt(value).                                              |
|       if value is negative or zero, result is 1 (frac=7fffffff, exp=0).   |
|---------------------------------------------------------------------------|
|  Algorithm:                                                               |
|                                                                           |
|   The function 1/sqrt(value) is approximated by a table and linear        |
|   interpolation.                                                          |
|                                                                           |
|   1- If exponant is odd then shift fraction right once.                   |
|   2- exponant = -((exponant-1)>>1)                                        |
|   3- i = bit25-b30 of fraction, 16 <= i <= 63 ->because of normalization. |
|   4- a = bit10-b24                                                        |
|   5- i -=16                                                               |
|   6- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2            |
|___________________________________________________________________________|
*/
static Word16 table_isqrt[49] =
{
    32767, 31790, 30894, 30070, 29309, 28602, 27945, 27330, 26755, 26214,
    25705, 25225, 24770, 24339, 23930, 23541, 23170, 22817, 22479, 22155,
    21845, 21548, 21263, 20988, 20724, 20470, 20225, 19988, 19760, 19539,
    19326, 19119, 18919, 18725, 18536, 18354, 18176, 18004, 17837, 17674,
    17515, 17361, 17211, 17064, 16921, 16782, 16646, 16514, 16384
};

void Isqrt_n(
        Word32 * frac,                        /* (i/o) Q31: normalized value (1.0 < frac <= 0.5) */
        Word16 * exp                          /* (i/o)    : exponent (value = frac x 2^exponent) */
        )
{
    Word16 i, a, tmp;

    if (*frac <= (Word32) 0)
    {
        *exp = 0;
        *frac = 0x7fffffffL;
        return;
    }

    if((*exp & 1) == 1)                       /*If exponant odd -> shift right */
        *frac = (*frac) >> 1;

    *exp = negate((*exp - 1) >> 1);

    *frac = (*frac >> 9);
    i = extract_h(*frac);                  /* Extract b25-b31 */
    *frac = (*frac >> 1);
    a = (Word16)(*frac);                  /* Extract b10-b24 */
    a = (Word16) (a & (Word16) 0x7fff);
    i -= 16;
    *frac = L_deposit_h(table_isqrt[i]);   /* table[i] << 16         */
    tmp = vo_sub(table_isqrt[i], table_isqrt[i + 1]);      /* table[i] - table[i+1]) */
    *frac = vo_L_msu(*frac, tmp, a);          /* frac -=  tmp*a*2       */

    return;
}

/*___________________________________________________________________________
|                                                                           |
|   Function Name : Pow2()                                                  |
|                                                                           |
|     L_x = pow(2.0, exponant.fraction)         (exponant = interger part)  |
|         = pow(2.0, 0.fraction) << exponant                                |
|---------------------------------------------------------------------------|
|  Algorithm:                                                               |
|                                                                           |
|   The function Pow2(L_x) is approximated by a table and linear            |
|   interpolation.                                                          |
|                                                                           |
|   1- i = bit10-b15 of fraction,   0 <= i <= 31                            |
|   2- a = bit0-b9   of fraction                                            |
|   3- L_x = table[i]<<16 - (table[i] - table[i+1]) * a * 2                 |
|   4- L_x = L_x >> (30-exponant)     (with rounding)                       |
|___________________________________________________________________________|
*/
static Word16 table_pow2[33] =
{
    16384, 16743, 17109, 17484, 17867, 18258, 18658, 19066, 19484, 19911,
    20347, 20792, 21247, 21713, 22188, 22674, 23170, 23678, 24196, 24726,
    25268, 25821, 26386, 26964, 27554, 28158, 28774, 29405, 30048, 30706,
    31379, 32066, 32767
};

Word32 Pow2(                               /* (o) Q0  : result       (range: 0<=val<=0x7fffffff) */
        Word16 exponant,                      /* (i) Q0  : Integer part.      (range: 0<=val<=30)   */
        Word16 fraction                       /* (i) Q15 : Fractionnal part.  (range: 0.0<=val<1.0) */
       )
{
    Word16 exp, i, a, tmp;
    Word32 L_x;

    L_x = vo_L_mult(fraction, 32);            /* L_x = fraction<<6           */
    i = extract_h(L_x);                    /* Extract b10-b16 of fraction */
    L_x =L_x >> 1;
    a = (Word16)(L_x);                    /* Extract b0-b9   of fraction */
    a = (Word16) (a & (Word16) 0x7fff);

    L_x = L_deposit_h(table_pow2[i]);      /* table[i] << 16        */
    tmp = vo_sub(table_pow2[i], table_pow2[i + 1]);        /* table[i] - table[i+1] */
    L_x -= (tmp * a)<<1;              /* L_x -= tmp*a*2        */

    exp = vo_sub(30, exponant);
    L_x = vo_L_shr_r(L_x, exp);

    return (L_x);
}

/*___________________________________________________________________________
|                                                                           |
|   Function Name : Dot_product12()                                         |
|                                                                           |
|       Compute scalar product of <x[],y[]> using accumulator.              |
|                                                                           |
|       The result is normalized (in Q31) with exponent (0..30).            |
|---------------------------------------------------------------------------|
|  Algorithm:                                                               |
|                                                                           |
|       dot_product = sum(x[i]*y[i])     i=0..N-1                           |
|___________________________________________________________________________|
*/

Word32 Dot_product12(                      /* (o) Q31: normalized result (1 < val <= -1) */
        Word16 x[],                           /* (i) 12bits: x vector                       */
        Word16 y[],                           /* (i) 12bits: y vector                       */
        Word16 lg,                            /* (i)    : vector length                     */
        Word16 * exp                          /* (o)    : exponent of result (0..+30)       */
        )
{
    Word16 sft;
    Word32 i, L_sum;
    L_sum = 0;
    for (i = 0; i < lg; i++)
    {
        Word32 tmp = (Word32) x[i] * (Word32) y[i];
        if (tmp == (Word32) 0x40000000L) {
            tmp = MAX_32;
        }
        L_sum = L_add(L_sum, tmp);
    }
    L_sum = L_shl2(L_sum, 1);
    L_sum = L_add(L_sum, 1);
    /* Normalize acc in Q31 */
    sft = norm_l(L_sum);
    L_sum = L_sum << sft;
    *exp = 30 - sft;            /* exponent = 0..30 */
    return (L_sum);

}