File content as of revision 67:e458db8402dc:

/// @copyright
/// ========================================================================={{{
/// Copyright (c) 2013-2014 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

#ifndef __KAL_MATH_H__
#define __KAL_MATH_H__

#include "hal_types.h"

// =============================================================================
// Compare operators
// =============================================================================

#define KAL_MAX(a,b)                    (((a) > (b)) ? (a) : (b))
#define KAL_MIN(a,b)                    (((a) < (b)) ? (a) : (b))

//#define KAL_ABS(a)                      (((a) > 0) ? (a) : (-(a)))
#define KAL_ABS16_FAST(a)               ((a) ^ ((a) >> 15))
#define KAL_ABS32_FAST(a)               ((a) ^ ((a) >> 31))

#define KAL_SAT_RANGE(a,min,max)        (((a) < (min)) ? (min) : ((a) > (max)) ? (max) : (a))
#define KAL_SAT_S16(a)                  (((a) < MIN_S16) ? MIN_S16 : ((a) > MAX_S16) ? MAX_S16 : (s16)(a))
#define KAL_SAT_U16(a)                  (((a) > MAX_U16) ? MAX_U16 : (u16)(a))
#define KAL_SAT_S8(a)                   (((a) < MIN_S8)  ? MIN_S8  : ((a) > MAX_S8)  ? MAX_S8 :  (s8)(a))
#define KAL_SAT_U8(a)                   (((a) > MAX_U8)  ? MAX_U8 :  (u8)(a))

#define KAL_COMP16_FAST(a,b)            (((b) - (a)) >> 15)

// =============================================================================
// Bit manipulation operators
// =============================================================================

#define KAL_BIT_CLR(d, j)               (d)[(j)/8] &= (~(1 << ((j) & 7)))
#define KAL_BIT_SET(d, j)               (d)[(j)/8] |= (((1) << ((j) & 7)))
#define KAL_BIT_MOV(s, i, d, j)         (d)[(j)/8] |= ((((s)[(i)/8]>>((i)&7))&1)<<((j)&7))
#define KAL_BIT_IS_ONE(s, i)            ((s)[(i)/8] & (1 << ((i) & 7)))
#define KAL_BIT_GET(s, i)               ((s)[(i)/8] >> ((i) & 7) & 1)

#define KAL_2BIT_SH(i)                  (2 * ((i) & 3))
#define KAL_2BIT_GET(d, i)              (((d)[(i)/4] >> KAL_2BIT_SH(i)) & 3)  
#define KAL_2BIT_CLR(d, i)              ((d)[(i)/4] &=~ (3 << KAL_2BIT_SH(i)))
#define KAL_2BIT_SET(d, i, c)           ((d)[(i)/4] |= ((c) << KAL_2BIT_SH(i)))  
#define KAL_2BIT_MOV(s, i, d, j)        KAL_2BIT_SET(d, j, KAL_2BIT_GET(s, i))

// =============================================================================
// Simple arithmetic operators
// =============================================================================

#define KAL_MUL(x,y)                    ((x)*(y))
#define KAL_MLF(x,y,frac)               (((x)*(y)) >> (frac))
#define KAL_SHIFT_LEFT_SIGNED(a,s)      (((s) > 0) ? ((a) << (s)) : ((a) >> (-(s))))
#define KAL_SQR(x)                      KAL_MUL(x,x)
#define KAL_DIV_INT(n,d)                (((n) + ((d)/2))/(d))
#define KAL_DIV_CEILING(n,d)            (((n) + (d) - 1)/(d))
#define KAL_DIV_FLOOR(n,d)              (((n)          )/(d))

// =============================================================================
// Complex operators
// =============================================================================

#define KAL_COMPLEX_ADD(a,b,out)        { (out).I = (a).I + (b).I; (out).Q = (a).Q + (b).Q; }
#define KAL_COMPLEX_SUB(a,b,out)        { (out).I = (a).I - (b).I; (out).Q = (a).Q - (b).Q; }

#define KAL_COMPLEX_MUL(a,b,out)        { (out).I = KAL_MUL((a).I,(b).I) - KAL_MUL((a).Q,(b).Q); \
                                          (out).Q = KAL_MUL((a).I,(b).Q) + KAL_MUL((a).Q,(b).I); }

#define KAL_COMPLEX_MLF(a,b,out,frac)   { (out).I = KAL_MLF((a).I,(b).I,(frac)) - KAL_MLF((a).Q,(b).Q,(frac)); \
                                          (out).Q = KAL_MLF((a).I,(b).Q,(frac)) + KAL_MLF((a).Q,(b).I,(frac)); }

#define KAL_COMPLEX_NORM(a)             (KAL_MUL((a).I,(a).I) + KAL_MUL((a).Q,(a).Q))
#define KAL_COMPLEX_MOD(a)              (kal_sqrt32((u32)KAL_COMPLEX_NORM(a)))

#define KAL_COMPLEX_DIST(a,b,res)       { \
                                            complex_t diff; \
                                            KAL_COMPLEX_SUB((a),(b), diff); \
                                            (res) = KAL_COMPLEX_MOD(diff); \
                                        }

//======================================================================
// Circular d-dimensional buffer structure
//======================================================================
typedef struct
{
    // Number of dimensions of the buffer
    u8              dim;
    // Buffer length (number of vectors)
    u8              len;
    // Current vector position in the buffer
    u8              curr;
    // Pointer to the start of the buffer
    s16             buf[1];

} kal_circ_buf_t;

// =======================================================================
// kal_abs_s32
// -----------------------------------------------------------------------
/// @brief  Absolute function
/// @param  x           s32         input
/// @retval             u32         output
// =======================================================================
INLINE _public u32 kal_abs_s32(s32 x)
{
    return (u32)((x > 0)? x : -x);
}

INLINE _public u16 kal_abs_s16(s16 x)
{
    return (u16)((x > 0)? x : -x);
}

INLINE _public u8 kal_abs_s8(s8 x)
{
    return (u8)((x > 0)? x : -x);
}

// =======================================================================
// 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);

// =======================================================================
// 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);

// =======================================================================
// 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);

// =======================================================================
// 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);

// =======================================================================
// kal_log2
// -----------------------------------------------------------------------
/// @brief  Fast log2 computation
/// @param  in          u32         operand
/// @retval             u8          log2 of the operand
// =======================================================================
_public u8 kal_log2(u32 in);

// =======================================================================
// 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);

// =======================================================================
// 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);

// =======================================================================
// kal_cos
// -----------------------------------------------------------------------
/// @brief  cos(a) in Q15
/// @param  a           u16         angle in degrees [0, 360[ 
/// @retval             s16         cos(a) in Q15
// =======================================================================
#define kal_cos(a)      kal_sin(((450-(a)) % 360))

// =======================================================================
// 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);

// =======================================================================
// 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);

// =======================================================================
// 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 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);

// =======================================================================
// 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);

// =======================================================================
// 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);

// =======================================================================
// 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);

// =======================================================================
// 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);

//======================================================================
//  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);

//======================================================================
//  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_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);

//======================================================================
//  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);

//======================================================================
//  kal_circ_buf_stats
//----------------------------------------------------------------------
/// @brief Mean and variance of the circular buffer
/// @param  v           s16*               d-dimensional mean vector (output)
/// @param  circ        kal_circ_buf_t*    Pointer to the circular buffer structure
/// @retval             u32                variance
//======================================================================
_public u32 kal_circ_buf_stats(s16* mean, kal_circ_buf_t* circ);

//======================================================================
//  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);

//======================================================================
// 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);

//======================================================================
//  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);

#endif // __KAL_MATH_H__