File content as of revision 67:e458db8402dc:

/// @copyright
/// ========================================================================={{{
/// Copyright (c) 2013 WizziLab                                                /
/// All rights reserved                                                        /
///                                                                            /
/// IMPORTANT: This Software may not be modified, copied or distributed unless /
/// embedded on a WizziLab product. Other than for the foregoing purpose, this /
/// Software and/or its documentation may not be used, reproduced, copied,     /
/// prepared derivative works of, modified, performed, distributed, displayed  /
/// or sold for any purpose. For the sole purpose of embedding this Software   /
/// on a WizziLab product, copy, modification and distribution of this         /
/// Software is granted provided that the following conditions are respected:  /
///                                                                            /
/// *  Redistributions of source code must retain the above copyright notice,  /
///    this list of conditions and the following disclaimer                    /
///                                                                            /
/// *  Redistributions 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 WizziLab can 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 COPYRIGHT HOLDER 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.               /
/// WIZZILAB HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES,       /
/// ENHANCEMENTS OR MODIFICATIONS.                                             /
///                                                                            /
/// Should you have any questions regarding your right to use this Software,   /
/// contact WizziLab at www.wizzilab.com.                                      /
///                                                                            /
/// =========================================================================}}}
/// @endcopyright

#include "hal_types.h"
#include "kal_math.h"
#include "WizziDebug.h"

#ifndef KAL_MALLOC
    #define KAL_MALLOC MALLOC
#endif

#ifndef KAL_FREE
    #define KAL_FREE FREE
#endif

// =======================================================================
// kal_log2
// -----------------------------------------------------------------------
/// @brief  Fast log2 computation
/// @param  in          u32         operand
/// @retval             u8          log2 of the operand
// =======================================================================
_public u8 kal_log2(u32 in)
{
    if (in < 2)
    {
        // no negative return
        return 0;
    }
    else if (MAX_U32 == in)
    {
        // log2(2^32)
        return 32;
    }
    else
    {
        u32 x;
        u8 norm;
        for (x = in, norm = 31; x < 0x80000000L; norm--, x <<= 1);
        return norm;
    }
}

// =======================================================================
// kal_sqrt
// -----------------------------------------------------------------------
/// @brief  Fixed point square root
/// @param  x           u16         input fixed point value
/// @param  format      u8          fixed point format (shift)
/// @retval             u16         square root of the input in the given format
// =======================================================================
_public u16 kal_sqrt(u16 x, u8 format)
{
    #define KAL_SQRT_ITER (5)
    u32 r, s;
    u8 i;

    if (!x)
    {
        // sqrt(0)
        return 0;
    }
    else
    {
        // value to sqrt
        s = x << format;

        // start iteration with good sqrt estimator
        r = s >> ((kal_log2(x) + format)/2);

        // For better precision, increase number of iterations
        for (i = 0; (r != 0) && (i < KAL_SQRT_ITER); ++i)
        {
            r = (s/r + r)/2;
        }

        return (u16)r;
    }
}

// =======================================================================
// kal_sqrt32
// -----------------------------------------------------------------------
/// @brief  Integer square root
/// @param  x           u32         input fixed point value
/// @retval             u16         square root of the input
// =======================================================================
_public u16 kal_sqrt32(u32 x)
{
    u32 r, s;
    u8 i;

    if (!x)
    {
        // sqrt(0)
        return 0;
    }
    else
    {
#if 1
        // value to sqrt
        s = x;

        // start iteration with good sqrt estimator
        r = s >> (kal_log2(x)/2);

        // For better precision, increase number of iterations
        for (i = 0; (r != 0) && (i < KAL_SQRT_ITER); ++i)
        {
            r = (s/r + r)/2;
        }
#else
        s = 1 << 30;
        r = 0;

        while (s > x)
            s >>= 2;
     
        while (s != 0) {
            if (x >= r + s) {
                x -= r + s;
                r = (r >> 1) + s;
            }
            
            else
                r >>= 1;
            s >>= 2;
        }

#endif
        return (u16)r;
    }
}

// =======================================================================
// kal_div
// -----------------------------------------------------------------------
/// @brief  Fixed point division of two s16 values
/// @param  nom         s16         Numerator
/// @param  denom       s16         Denominator
/// @param  format      u8          fixed point format (shift)
/// @retval             s16         division result in the given format
// =======================================================================
_public s16 kal_div(s16 num, s16 denom, u8 format)
{
    if (!denom)
    {
        return ((num < 0) ? MIN_S16 : MAX_S16);
    }
    else
    {
        s16 sign = (num ^ denom) >> 15;
        u32 n = kal_abs_s32(num);
        u32 d = kal_abs_s32(denom);
        s8 rescale = 15 - format;

        while ((d <= n) && (rescale >= 0))
        {
            rescale--;
            d <<= 1;
        }

        if (rescale < 0)
        {
            return ((sign) ? MIN_S16 : MAX_S16);
        }
        else
        {
            u8 i;
            u16 res;

            for (res = 0, i = 0; i < 15; ++i)
            {
                res <<= 1;
                n <<= 1;
                if (n >= d)
                {
                    n -= d;
                    res++;
                }
            }
            res >>= rescale;
            return ((sign) ? -res : res);
        }
    }
}

// =======================================================================
// kal_div_u32
// -----------------------------------------------------------------------
/// @brief  Fixed point division of two u32 values
/// @param  n           u32         Numerator
/// @param  d           u32         Denominator
/// @param  format      u8          fixed point format (shift)
/// @retval             u32         division result in the given format
// =======================================================================
_public u32 kal_div_u32(u32 n, u32 d, u8 format)
{
    if (!d)
    {
        return MAX_U32;
    }
    else
    {
        s8 rescale = 31 - format;

        if (n > MAX_S32)
        {
            rescale--;
            n >>= 1;
        }

        // avoid never true comparison 
        // in sequence n <<= 1; if (n >= d)...
        // when d > MAX_S32;
        if (d > MAX_S32)
        {
            n >>= 1;
            d >>= 1;
        }

        while ((d <= n) && (rescale >= 0))
        {
            rescale--;
            d <<= 1;
        }

        if (rescale < 0)
        {
            return MAX_U32;
        }
        else
        {
            u8 i;
            u32 res;

            for (res = 0, i = 0; i < 31; ++i)
            {
                res <<= 1;
                n <<= 1;
                if (n >= d)
                {
                    n -= d;
                    res++;
                }
            }
            res >>= rescale;
            return res;
        }
    }
}

// =======================================================================
// sin(x), x in [0°, 90°], in Q15 
// =======================================================================
const u16 k_sin[] = 
{   
       0,     572,    1144,    1715,    2286,    2856,    3425,    3993,    4560,    5126,
    5690,    6252,    6813,    7371,    7927,    8481,    9032,    9580,   10126,   10668,
   11207,   11743,   12275,   12803,   13328,   13848,   14365,   14876,   15384,   15886,
   16384,   16877,   17364,   17847,   18324,   18795,   19261,   19720,   20174,   20622,
   21063,   21498,   21926,   22348,   22763,   23170,   23571,   23965,   24351,   24730,
   25102,   25466,   25822,   26170,   26510,   26842,   27166,   27482,   27789,   28088,
   28378,   28660,   28932,   29197,   29452,   29698,   29935,   30163,   30382,   30592,
   30792,   30983,   31164,   31336,   31499,   31651,   31795,   31928,   32052,   32166,
   32270,   32365,   32449,   32524,   32588,   32643,   32688,   32723,   32748,   32763,
   32767
};

// =======================================================================
// kal_complex_arg
// -----------------------------------------------------------------------
/// @brief  Compute the argument of a complex number
/// @param  in          u32         operand
/// @retval             u16         angle in degrees [0, 360[
// =======================================================================
_public u16 kal_complex_arg(complex_t in)
{
    u16 angle = 0;
    if ((in.I) || (in.Q))
    {
        u16 min = 0, max = 90, mid, sine;

        // compute the sine in.i / sqrt(in.i^2 + in.q^2) 
        u16 norm = kal_sqrt32(in.I * in.I + in.Q * in.Q);
        sine = (norm) ? (u16)(((u32)kal_abs_s32(in.Q << 15) + norm/2) / norm) : 0;
       
        // find angle from LUT (1° precision) using dichotomy
        while ((max - min) > 1)
        {
            angle = (max + min) / 2;
            if (sine < k_sin[angle])
            {
                max = angle;
            }
            else
            {
                min = angle;
            }
        }

        min = kal_abs_s16(k_sin[angle-1] - sine);
        mid = kal_abs_s16(k_sin[angle  ] - sine);
        max = kal_abs_s16(k_sin[angle+1] - sine);

        // best approx
        if ((min < max) && (min < mid))
        {
            angle--;
        }
        else if ((max > min) && (max > mid))
        { 
            angle++;
        }

        // quadrant
        if (in.I < 0)
        {
            angle = (in.Q < 0) ? angle + 180 : 180 - angle;
        }
        else
        {
            angle = (in.Q < 0) ? 360 - angle : angle;
        }
    }

    return angle;
}

// =======================================================================
// kal_sin
// -----------------------------------------------------------------------
/// @brief  sin(a) in Q15
/// @param  a           u16         angle in degrees [0, 360[ 
/// @retval             s16         sin(a) in Q15
// =======================================================================
_public u16 kal_sin(u16 a)
{
    if (a < 90)
    {
        return k_sin[a];
    }
    else if (a < 180)
    {
        return k_sin[180 - a];
    }
    else if (a < 270)
    {
        return -k_sin[a - 180];
    }
    else
    {
        return -k_sin[360 - a];
    }
}

// =======================================================================
// kal_add
// -----------------------------------------------------------------------
/// @brief  Add B to A and store in A
/// @param  a           s16*        pointer to the d-dimensional vector A
/// @param  b           s16*        pointer to the d-dimensional vector B
/// @param  d           u8          number of dimensions of the buffer
/// @retval             void
// =======================================================================
_public void kal_add(s16* a, s16* b, u8 d)
{
    u8 i;
    for (i = 0; i < d; ++i)
    {
        s16 temp = *a;
        *a++ = temp + *b++;
    }
}

// =======================================================================
// kal_sub
// -----------------------------------------------------------------------
/// @brief  Subtract B from A and store in A
/// @param  a           s16*        pointer to the d-dimensional vector A
/// @param  b           s16*        pointer to the d-dimensional vector B
/// @param  d           u8          number of dimensions of the buffer
/// @retval             void
// =======================================================================
_public void kal_sub(s16* a, s16* b, u8 d)
{
    u8 i;
    for (i = 0; i < d; ++i)
    {
        s16 temp = *a;
        *a++ = temp - *b++;
    }
}

// =======================================================================
// kal_mean
// -----------------------------------------------------------------------
/// @brief  Get mean of data in a d-dimensional buffer
/// @param  in          s16*        input circular buffer
/// @param  out         s16*        pointer to the d-dimensional mean result
/// @param  len         u8          length of the buffer
/// @param  d           u8          number of dimensions of the buffer
/// @retval             void
// =======================================================================
_public void kal_mean(s16* in, s16* out, u8 len, u8 d)
{
    u8 i;
    for (i = 0; i < d; ++i)
    {
        u8 j;
        s16* p = in++;
        s32 mean = 0;

        for (j = 0; j < len; ++j)
        {
            mean += *p;
            p += d;
        }

        mean = (mean + len/2) / len;
        *out++ = KAL_SAT_S16(mean);
    }
}

// =======================================================================
// kal_var
// -----------------------------------------------------------------------
/// @brief  Get the combined variance of data in a d-dimensional buffer
/// @param  in          s16*        input circular buffer
/// @param  mean        s16*        pointer to the d-dimensional mean
/// @param  len         u8          length of the buffer
/// @param  d           u8          number of dimensions of the buffer
/// @retval             u32         variance
// =======================================================================
_public u32 kal_var(s16* in, s16* mean, u8 len, u8 d)
{
    u8 i,j;
    u32 var = 0;
    for (i = 0; i < d; ++i)
    {
        // get var
        s16* p = in;
        for (j = 0; j < len; ++j)
        {
            s16 s = *p - *mean;
            var += KAL_MUL(s,s);
            p += d;

            // avoid overflows
            if (var > MAX_S32)
            {
                return MAX_U32;
            }
        }
        in++;
        mean++;
    }
    var = (var + len/2) / len;
    return var;
}

// =======================================================================
// kal_dev
// -----------------------------------------------------------------------
/// @brief  Get the deviation per axis in a d-dimensional buffer
/// @param  buf         s16*        input circular buffer
/// @param  inout       s16*        [in] pointer to the d-dimensional mean
///                                 [out] pointer to the d-dimensional dev
/// @param  len         u8          length of the buffer
/// @param  d           u8          number of dimensions of the buffer
/// @retval             void        
// =======================================================================
_public void kal_dev(s16* buf, s16* inout, u8 len, u8 d)
{
    u8 i, j;
    u32 var;
    s32 sqrt;
    s16* p, mean;

    for (i = 0; i < d; ++i)
    {
        var = 0;
        p = buf++;
        mean = *inout;

        for (j = 0; j < len; ++j)
        {
            s16 s = *p - mean;
            var += KAL_MUL(s,s);
            p += d;

            // avoid overflows
            if (var > MAX_S32)
            {
                var = MAX_U32;
                break;
            }
        }
        var = KAL_DIV_INT(var,len);
        sqrt = kal_sqrt32(var);
        *inout++ = KAL_SAT_S16(sqrt);
    }
}

// =======================================================================
// kal_norm
// -----------------------------------------------------------------------
/// @brief  Get norm of an d-dimensional vector
/// @param  in          s16*        d-dimensional vector
/// @param  d           u8          number of dimensions of the vector
/// @retval             u32         norm of the vector
// =======================================================================
_public u32 kal_norm(s16* in, u8 d)
{
    u8 i;
    u32 norm = 0;
    for (i = 0; i < d; ++i)
    {
        s16 s = *in++;
        norm += KAL_MUL(s,s);
    }
    return norm;
}

// =======================================================================
// kal_dist
// -----------------------------------------------------------------------
/// @brief  Get distance between two d-dimensional vectors
/// @param  a           s16*        first d-dimensional vector
/// @param  b           s16*        second d-dimensional vector
/// @param  d           u8          number of dimensions of the vector
/// @retval             u32         distance (norm of the difference)
// =======================================================================
_public u32 kal_dist(s16* a, s16* b, u8 d)
{
    u8 i;
    u32 norm = 0;
    for (i = 0; i < d; ++i)
    {
        s32 s_long = (*a++) - (*b++);
        s16 s = KAL_SAT_S16(s_long);
        norm += KAL_MUL(s,s);
    }
    return norm;
}

//======================================================================
//  kal_circ_buf_alloc
//----------------------------------------------------------------------
/// @brief Allocate and init circular buffer
/// @param  len         u8                  Number of d-dimensional vectors in the buffer
/// @param  dim         u8                  Number of dimensions of the buffer
/// @return             kal_circ_buf_t*    Pointer to the circular buffer structure         
//======================================================================
_public kal_circ_buf_t* kal_circ_buf_alloc(u8 len, u8 dim)
{
    // Allocate structure
    kal_circ_buf_t* circ = (kal_circ_buf_t*)KAL_MALLOC(sizeof(kal_circ_buf_t) + (dim * len  - 1) * sizeof(s16));

    // Init structure
    circ->dim = dim;
    circ->len = len;
    circ->curr = 0;
    return circ;
}

//======================================================================
//  kal_circ_buf_free
//----------------------------------------------------------------------
/// @brief Free circular buffer
/// @param  circ        kal_circ_buf_t*    Pointer to the circular buffer structure
/// @return             void
//======================================================================
_public void kal_circ_buf_free(kal_circ_buf_t* circ)
{
    KAL_FREE(circ);
}

//======================================================================
//  kal_circ_buf_init
//----------------------------------------------------------------------
/// @brief Init the buffer with the same element
/// @param  v           s16*               d-dimensional vector to write
/// @param  circ        kal_circ_buf_t*    Pointer to the circular buffer structure
/// @return             void
//======================================================================
_public void kal_circ_buf_init(s16* v, kal_circ_buf_t* circ)
{
    u8 i;
    s16 *pb = &circ->buf[0];
    for (i = 0; i < circ->len; ++i)
    {
        u8 j;
        s16* pv = v;
        for (j = 0; j < circ->dim; ++j)
        {
            *pb++ = *pv++;
        }
    }
}

//======================================================================
//  kal_circ_buf_add
//----------------------------------------------------------------------
/// @brief Add new element of type s16 in buffer, erase oldest element
/// @param  v           s16*                d-dimensional vector to write
/// @param  circ        kal_circ_buf_t*    Pointer to the circular buffer structure
/// @return             void
//======================================================================
_public void kal_circ_buf_add(s16* v, kal_circ_buf_t* circ)
{
    u8 i;
    s16* p = &circ->buf[0] + (circ->dim * circ->curr); 
    for (i = 0; i < circ->dim; ++i)
    {
        *p++ = *v++;
    }
    circ->curr++;
    if (circ->curr == circ->len)
    {
        circ->curr = 0;
    }
}

//======================================================================
//  kal_lp_filter
//----------------------------------------------------------------------
/// @brief Low Pass Filter with forget factor
/// @param  lp          s16*                Pointer to the LP sample
/// @param  s           s16*                Pointer to the new sample
/// @param  dim         u8                  Number of dimensions of the sample
/// @param  ff          u8                  Forget factor
/// @return             void
//======================================================================
_public void kal_lp_filter(s16* lpf, s16* s, u8 dim, u8 ff)
{
    if (ff)
    {
        u8 i;
        for (i = 0; i < dim; ++i)
        {
            s16 a = *lpf;
            s16 b = *s++;
            a += ((b - a) / ff);
            *lpf++ = a;
        }
    }
}

//======================================================================
// kal_xor 
//----------------------------------------------------------------------
/// @brief  xor two vectors
/// @param  buf         u8*                     inout stream
/// @param  iv          u8*                     in stream to XOR
/// @retval             void
//======================================================================
_public void kal_xor(u8* buf, u8* iv, u16 len)
{
    u16 i;
    for(i = 0; i < len; ++i)
    {
        buf[i] ^= iv[i];
    }
}

//======================================================================
//  kal_sort
//----------------------------------------------------------------------
/// @brief Sort u16 buffer from smaller to bigger error
/// @param  inout       u16*                inout buffer
/// @param  ref         u16                 comparison reference
/// @param  len         u8                  buffer length
/// @return             void
//======================================================================
_public void kal_sort(u16* inout, u16 ref, u8 len)
{
    u8 i,j;
    for (i = 0; i < len; i++)
    {
        u16 tmp, min = MAX_U16;
        u8 k = len;
        for (j = i; j < len; j++)
        {
            s32 diff = inout[j] - ref;
            u16 d = KAL_SAT_U16(kal_abs_s32(diff));
            if (d <= min)
            {
                k = j;
                min = d;
            }
        }
        tmp = inout[i];
        inout[i] = inout[k];
        inout[k] = tmp;
    }
}