kal_math.cpp

00001 /// @copyright
00002 /// ========================================================================={{{
00003 /// Copyright (c) 2013 WizziLab                                                /
00004 /// All rights reserved                                                        /
00005 ///                                                                            /
00006 /// IMPORTANT: This Software may not be modified, copied or distributed unless /
00007 /// embedded on a WizziLab product. Other than for the foregoing purpose, this /
00008 /// Software and/or its documentation may not be used, reproduced, copied,     /
00009 /// prepared derivative works of, modified, performed, distributed, displayed  /
00010 /// or sold for any purpose. For the sole purpose of embedding this Software   /
00011 /// on a WizziLab product, copy, modification and distribution of this         /
00012 /// Software is granted provided that the following conditions are respected:  /
00013 ///                                                                            /
00014 /// *  Redistributions of source code must retain the above copyright notice,  /
00015 ///    this list of conditions and the following disclaimer                    /
00016 ///                                                                            /
00017 /// *  Redistributions in binary form must reproduce the above copyright       /
00018 ///    notice, this list of conditions and the following disclaimer in the     /
00019 ///    documentation and/or other materials provided with the distribution.    /
00020 ///                                                                            /
00021 /// *  The name of WizziLab can not be used to endorse or promote products     /
00022 ///    derived from this software without specific prior written permission.   /
00023 ///                                                                            /
00024 /// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS        /
00025 /// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED  /
00026 /// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR /
00027 /// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR          /
00028 /// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,      /
00029 /// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,        /
00030 /// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,            /
00031 /// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY     /
00032 /// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING    /
00033 /// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS         /
00034 /// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.               /
00035 /// WIZZILAB HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES,       /
00036 /// ENHANCEMENTS OR MODIFICATIONS.                                             /
00037 ///                                                                            /
00038 /// Should you have any questions regarding your right to use this Software,   /
00039 /// contact WizziLab at www.wizzilab.com.                                      /
00040 ///                                                                            /
00041 /// =========================================================================}}}
00042 /// @endcopyright
00043 
00044 #include "hal_types.h"
00045 #include "kal_math.h"
00046 #include "WizziDebug.h"
00047 
00048 #ifndef KAL_MALLOC
00049     #define KAL_MALLOC MALLOC
00050 #endif
00051 
00052 #ifndef KAL_FREE
00053     #define KAL_FREE FREE
00054 #endif
00055 
00056 // =======================================================================
00057 // kal_log2
00058 // -----------------------------------------------------------------------
00059 /// @brief  Fast log2 computation
00060 /// @param  in          u32         operand
00061 /// @retval             u8          log2 of the operand
00062 // =======================================================================
00063 _public u8 kal_log2(u32 in)
00064 {
00065     if (in < 2)
00066     {
00067         // no negative return
00068         return 0;
00069     }
00070     else if (MAX_U32 == in)
00071     {
00072         // log2(2^32)
00073         return 32;
00074     }
00075     else
00076     {
00077         u32 x;
00078         u8 norm;
00079         for (x = in, norm = 31; x < 0x80000000L; norm--, x <<= 1);
00080         return norm;
00081     }
00082 }
00083 
00084 // =======================================================================
00085 // kal_sqrt
00086 // -----------------------------------------------------------------------
00087 /// @brief  Fixed point square root
00088 /// @param  x           u16         input fixed point value
00089 /// @param  format      u8          fixed point format (shift)
00090 /// @retval             u16         square root of the input in the given format
00091 // =======================================================================
00092 _public u16 kal_sqrt(u16 x, u8 format)
00093 {
00094     #define KAL_SQRT_ITER (5)
00095     u32 r, s;
00096     u8 i;
00097 
00098     if (!x)
00099     {
00100         // sqrt(0)
00101         return 0;
00102     }
00103     else
00104     {
00105         // value to sqrt
00106         s = x << format;
00107 
00108         // start iteration with good sqrt estimator
00109         r = s >> ((kal_log2(x) + format)/2);
00110 
00111         // For better precision, increase number of iterations
00112         for (i = 0; (r != 0) && (i < KAL_SQRT_ITER); ++i)
00113         {
00114             r = (s/r + r)/2;
00115         }
00116 
00117         return (u16)r;
00118     }
00119 }
00120 
00121 // =======================================================================
00122 // kal_sqrt32
00123 // -----------------------------------------------------------------------
00124 /// @brief  Integer square root
00125 /// @param  x           u32         input fixed point value
00126 /// @retval             u16         square root of the input
00127 // =======================================================================
00128 _public u16 kal_sqrt32(u32 x)
00129 {
00130     u32 r, s;
00131     u8 i;
00132 
00133     if (!x)
00134     {
00135         // sqrt(0)
00136         return 0;
00137     }
00138     else
00139     {
00140 #if 1
00141         // value to sqrt
00142         s = x;
00143 
00144         // start iteration with good sqrt estimator
00145         r = s >> (kal_log2(x)/2);
00146 
00147         // For better precision, increase number of iterations
00148         for (i = 0; (r != 0) && (i < KAL_SQRT_ITER); ++i)
00149         {
00150             r = (s/r + r)/2;
00151         }
00152 #else
00153         s = 1 << 30;
00154         r = 0;
00155 
00156         while (s > x)
00157             s >>= 2;
00158      
00159         while (s != 0) {
00160             if (x >= r + s) {
00161                 x -= r + s;
00162                 r = (r >> 1) + s;
00163             }
00164             
00165             else
00166                 r >>= 1;
00167             s >>= 2;
00168         }
00169 
00170 #endif
00171         return (u16)r;
00172     }
00173 }
00174 
00175 // =======================================================================
00176 // kal_div
00177 // -----------------------------------------------------------------------
00178 /// @brief  Fixed point division of two s16 values
00179 /// @param  nom         s16         Numerator
00180 /// @param  denom       s16         Denominator
00181 /// @param  format      u8          fixed point format (shift)
00182 /// @retval             s16         division result in the given format
00183 // =======================================================================
00184 _public s16 kal_div(s16 num, s16 denom, u8 format)
00185 {
00186     if (!denom)
00187     {
00188         return ((num < 0) ? MIN_S16 : MAX_S16);
00189     }
00190     else
00191     {
00192         s16 sign = (num ^ denom) >> 15;
00193         u32 n = kal_abs_s32(num);
00194         u32 d = kal_abs_s32(denom);
00195         s8 rescale = 15 - format;
00196 
00197         while ((d <= n) && (rescale >= 0))
00198         {
00199             rescale--;
00200             d <<= 1;
00201         }
00202 
00203         if (rescale < 0)
00204         {
00205             return ((sign) ? MIN_S16 : MAX_S16);
00206         }
00207         else
00208         {
00209             u8 i;
00210             u16 res;
00211 
00212             for (res = 0, i = 0; i < 15; ++i)
00213             {
00214                 res <<= 1;
00215                 n <<= 1;
00216                 if (n >= d)
00217                 {
00218                     n -= d;
00219                     res++;
00220                 }
00221             }
00222             res >>= rescale;
00223             return ((sign) ? -res : res);
00224         }
00225     }
00226 }
00227 
00228 // =======================================================================
00229 // kal_div_u32
00230 // -----------------------------------------------------------------------
00231 /// @brief  Fixed point division of two u32 values
00232 /// @param  n           u32         Numerator
00233 /// @param  d           u32         Denominator
00234 /// @param  format      u8          fixed point format (shift)
00235 /// @retval             u32         division result in the given format
00236 // =======================================================================
00237 _public u32 kal_div_u32(u32 n, u32 d, u8 format)
00238 {
00239     if (!d)
00240     {
00241         return MAX_U32;
00242     }
00243     else
00244     {
00245         s8 rescale = 31 - format;
00246 
00247         if (n > MAX_S32)
00248         {
00249             rescale--;
00250             n >>= 1;
00251         }
00252 
00253         // avoid never true comparison 
00254         // in sequence n <<= 1; if (n >= d)...
00255         // when d > MAX_S32;
00256         if (d > MAX_S32)
00257         {
00258             n >>= 1;
00259             d >>= 1;
00260         }
00261 
00262         while ((d <= n) && (rescale >= 0))
00263         {
00264             rescale--;
00265             d <<= 1;
00266         }
00267 
00268         if (rescale < 0)
00269         {
00270             return MAX_U32;
00271         }
00272         else
00273         {
00274             u8 i;
00275             u32 res;
00276 
00277             for (res = 0, i = 0; i < 31; ++i)
00278             {
00279                 res <<= 1;
00280                 n <<= 1;
00281                 if (n >= d)
00282                 {
00283                     n -= d;
00284                     res++;
00285                 }
00286             }
00287             res >>= rescale;
00288             return res;
00289         }
00290     }
00291 }
00292 
00293 // =======================================================================
00294 // sin(x), x in [0°, 90°], in Q15 
00295 // =======================================================================
00296 const u16 k_sin[] = 
00297 {   
00298        0,     572,    1144,    1715,    2286,    2856,    3425,    3993,    4560,    5126,
00299     5690,    6252,    6813,    7371,    7927,    8481,    9032,    9580,   10126,   10668,
00300    11207,   11743,   12275,   12803,   13328,   13848,   14365,   14876,   15384,   15886,
00301    16384,   16877,   17364,   17847,   18324,   18795,   19261,   19720,   20174,   20622,
00302    21063,   21498,   21926,   22348,   22763,   23170,   23571,   23965,   24351,   24730,
00303    25102,   25466,   25822,   26170,   26510,   26842,   27166,   27482,   27789,   28088,
00304    28378,   28660,   28932,   29197,   29452,   29698,   29935,   30163,   30382,   30592,
00305    30792,   30983,   31164,   31336,   31499,   31651,   31795,   31928,   32052,   32166,
00306    32270,   32365,   32449,   32524,   32588,   32643,   32688,   32723,   32748,   32763,
00307    32767
00308 };
00309 
00310 // =======================================================================
00311 // kal_complex_arg
00312 // -----------------------------------------------------------------------
00313 /// @brief  Compute the argument of a complex number
00314 /// @param  in          u32         operand
00315 /// @retval             u16         angle in degrees [0, 360[
00316 // =======================================================================
00317 _public u16 kal_complex_arg(complex_t in)
00318 {
00319     u16 angle = 0;
00320     if ((in.I) || (in.Q))
00321     {
00322         u16 min = 0, max = 90, mid, sine;
00323 
00324         // compute the sine in.i / sqrt(in.i^2 + in.q^2) 
00325         u16 norm = kal_sqrt32(in.I * in.I + in.Q * in.Q);
00326         sine = (norm) ? (u16)(((u32)kal_abs_s32(in.Q << 15) + norm/2) / norm) : 0;
00327        
00328         // find angle from LUT (1° precision) using dichotomy
00329         while ((max - min) > 1)
00330         {
00331             angle = (max + min) / 2;
00332             if (sine < k_sin[angle])
00333             {
00334                 max = angle;
00335             }
00336             else
00337             {
00338                 min = angle;
00339             }
00340         }
00341 
00342         min = kal_abs_s16(k_sin[angle-1] - sine);
00343         mid = kal_abs_s16(k_sin[angle  ] - sine);
00344         max = kal_abs_s16(k_sin[angle+1] - sine);
00345 
00346         // best approx
00347         if ((min < max) && (min < mid))
00348         {
00349             angle--;
00350         }
00351         else if ((max > min) && (max > mid))
00352         { 
00353             angle++;
00354         }
00355 
00356         // quadrant
00357         if (in.I < 0)
00358         {
00359             angle = (in.Q < 0) ? angle + 180 : 180 - angle;
00360         }
00361         else
00362         {
00363             angle = (in.Q < 0) ? 360 - angle : angle;
00364         }
00365     }
00366 
00367     return angle;
00368 }
00369 
00370 // =======================================================================
00371 // kal_sin
00372 // -----------------------------------------------------------------------
00373 /// @brief  sin(a) in Q15
00374 /// @param  a           u16         angle in degrees [0, 360[ 
00375 /// @retval             s16         sin(a) in Q15
00376 // =======================================================================
00377 _public u16 kal_sin(u16 a)
00378 {
00379     if (a < 90)
00380     {
00381         return k_sin[a];
00382     }
00383     else if (a < 180)
00384     {
00385         return k_sin[180 - a];
00386     }
00387     else if (a < 270)
00388     {
00389         return -k_sin[a - 180];
00390     }
00391     else
00392     {
00393         return -k_sin[360 - a];
00394     }
00395 }
00396 
00397 // =======================================================================
00398 // kal_add
00399 // -----------------------------------------------------------------------
00400 /// @brief  Add B to A and store in A
00401 /// @param  a           s16*        pointer to the d-dimensional vector A
00402 /// @param  b           s16*        pointer to the d-dimensional vector B
00403 /// @param  d           u8          number of dimensions of the buffer
00404 /// @retval             void
00405 // =======================================================================
00406 _public void kal_add(s16* a, s16* b, u8 d)
00407 {
00408     u8 i;
00409     for (i = 0; i < d; ++i)
00410     {
00411         s16 temp = *a;
00412         *a++ = temp + *b++;
00413     }
00414 }
00415 
00416 // =======================================================================
00417 // kal_sub
00418 // -----------------------------------------------------------------------
00419 /// @brief  Subtract B from A and store in A
00420 /// @param  a           s16*        pointer to the d-dimensional vector A
00421 /// @param  b           s16*        pointer to the d-dimensional vector B
00422 /// @param  d           u8          number of dimensions of the buffer
00423 /// @retval             void
00424 // =======================================================================
00425 _public void kal_sub(s16* a, s16* b, u8 d)
00426 {
00427     u8 i;
00428     for (i = 0; i < d; ++i)
00429     {
00430         s16 temp = *a;
00431         *a++ = temp - *b++;
00432     }
00433 }
00434 
00435 // =======================================================================
00436 // kal_mean
00437 // -----------------------------------------------------------------------
00438 /// @brief  Get mean of data in a d-dimensional buffer
00439 /// @param  in          s16*        input circular buffer
00440 /// @param  out         s16*        pointer to the d-dimensional mean result
00441 /// @param  len         u8          length of the buffer
00442 /// @param  d           u8          number of dimensions of the buffer
00443 /// @retval             void
00444 // =======================================================================
00445 _public void kal_mean(s16* in, s16* out, u8 len, u8 d)
00446 {
00447     u8 i;
00448     for (i = 0; i < d; ++i)
00449     {
00450         u8 j;
00451         s16* p = in++;
00452         s32 mean = 0;
00453 
00454         for (j = 0; j < len; ++j)
00455         {
00456             mean += *p;
00457             p += d;
00458         }
00459 
00460         mean = (mean + len/2) / len;
00461         *out++ = KAL_SAT_S16(mean);
00462     }
00463 }
00464 
00465 // =======================================================================
00466 // kal_var
00467 // -----------------------------------------------------------------------
00468 /// @brief  Get the combined variance of data in a d-dimensional buffer
00469 /// @param  in          s16*        input circular buffer
00470 /// @param  mean        s16*        pointer to the d-dimensional mean
00471 /// @param  len         u8          length of the buffer
00472 /// @param  d           u8          number of dimensions of the buffer
00473 /// @retval             u32         variance
00474 // =======================================================================
00475 _public u32 kal_var(s16* in, s16* mean, u8 len, u8 d)
00476 {
00477     u8 i,j;
00478     u32 var = 0;
00479     for (i = 0; i < d; ++i)
00480     {
00481         // get var
00482         s16* p = in;
00483         for (j = 0; j < len; ++j)
00484         {
00485             s16 s = *p - *mean;
00486             var += KAL_MUL(s,s);
00487             p += d;
00488 
00489             // avoid overflows
00490             if (var > MAX_S32)
00491             {
00492                 return MAX_U32;
00493             }
00494         }
00495         in++;
00496         mean++;
00497     }
00498     var = (var + len/2) / len;
00499     return var;
00500 }
00501 
00502 // =======================================================================
00503 // kal_dev
00504 // -----------------------------------------------------------------------
00505 /// @brief  Get the deviation per axis in a d-dimensional buffer
00506 /// @param  buf         s16*        input circular buffer
00507 /// @param  inout       s16*        [in] pointer to the d-dimensional mean
00508 ///                                 [out] pointer to the d-dimensional dev
00509 /// @param  len         u8          length of the buffer
00510 /// @param  d           u8          number of dimensions of the buffer
00511 /// @retval             void        
00512 // =======================================================================
00513 _public void kal_dev(s16* buf, s16* inout, u8 len, u8 d)
00514 {
00515     u8 i, j;
00516     u32 var;
00517     s32 sqrt;
00518     s16* p, mean;
00519 
00520     for (i = 0; i < d; ++i)
00521     {
00522         var = 0;
00523         p = buf++;
00524         mean = *inout;
00525 
00526         for (j = 0; j < len; ++j)
00527         {
00528             s16 s = *p - mean;
00529             var += KAL_MUL(s,s);
00530             p += d;
00531 
00532             // avoid overflows
00533             if (var > MAX_S32)
00534             {
00535                 var = MAX_U32;
00536                 break;
00537             }
00538         }
00539         var = KAL_DIV_INT(var,len);
00540         sqrt = kal_sqrt32(var);
00541         *inout++ = KAL_SAT_S16(sqrt);
00542     }
00543 }
00544 
00545 // =======================================================================
00546 // kal_norm
00547 // -----------------------------------------------------------------------
00548 /// @brief  Get norm of an d-dimensional vector
00549 /// @param  in          s16*        d-dimensional vector
00550 /// @param  d           u8          number of dimensions of the vector
00551 /// @retval             u32         norm of the vector
00552 // =======================================================================
00553 _public u32 kal_norm(s16* in, u8 d)
00554 {
00555     u8 i;
00556     u32 norm = 0;
00557     for (i = 0; i < d; ++i)
00558     {
00559         s16 s = *in++;
00560         norm += KAL_MUL(s,s);
00561     }
00562     return norm;
00563 }
00564 
00565 // =======================================================================
00566 // kal_dist
00567 // -----------------------------------------------------------------------
00568 /// @brief  Get distance between two d-dimensional vectors
00569 /// @param  a           s16*        first d-dimensional vector
00570 /// @param  b           s16*        second d-dimensional vector
00571 /// @param  d           u8          number of dimensions of the vector
00572 /// @retval             u32         distance (norm of the difference)
00573 // =======================================================================
00574 _public u32 kal_dist(s16* a, s16* b, u8 d)
00575 {
00576     u8 i;
00577     u32 norm = 0;
00578     for (i = 0; i < d; ++i)
00579     {
00580         s32 s_long = (*a++) - (*b++);
00581         s16 s = KAL_SAT_S16(s_long);
00582         norm += KAL_MUL(s,s);
00583     }
00584     return norm;
00585 }
00586 
00587 //======================================================================
00588 //  kal_circ_buf_alloc
00589 //----------------------------------------------------------------------
00590 /// @brief Allocate and init circular buffer
00591 /// @param  len         u8                  Number of d-dimensional vectors in the buffer
00592 /// @param  dim         u8                  Number of dimensions of the buffer
00593 /// @return             kal_circ_buf_t*    Pointer to the circular buffer structure         
00594 //======================================================================
00595 _public kal_circ_buf_t* kal_circ_buf_alloc(u8 len, u8 dim)
00596 {
00597     // Allocate structure
00598     kal_circ_buf_t* circ = (kal_circ_buf_t*)KAL_MALLOC(sizeof(kal_circ_buf_t) + (dim * len  - 1) * sizeof(s16));
00599 
00600     // Init structure
00601     circ->dim = dim;
00602     circ->len = len;
00603     circ->curr = 0;
00604     return circ;
00605 }
00606 
00607 //======================================================================
00608 //  kal_circ_buf_free
00609 //----------------------------------------------------------------------
00610 /// @brief Free circular buffer
00611 /// @param  circ        kal_circ_buf_t*    Pointer to the circular buffer structure
00612 /// @return             void
00613 //======================================================================
00614 _public void kal_circ_buf_free(kal_circ_buf_t* circ)
00615 {
00616     KAL_FREE(circ);
00617 }
00618 
00619 //======================================================================
00620 //  kal_circ_buf_init
00621 //----------------------------------------------------------------------
00622 /// @brief Init the buffer with the same element
00623 /// @param  v           s16*               d-dimensional vector to write
00624 /// @param  circ        kal_circ_buf_t*    Pointer to the circular buffer structure
00625 /// @return             void
00626 //======================================================================
00627 _public void kal_circ_buf_init(s16* v, kal_circ_buf_t* circ)
00628 {
00629     u8 i;
00630     s16 *pb = &circ->buf[0];
00631     for (i = 0; i < circ->len; ++i)
00632     {
00633         u8 j;
00634         s16* pv = v;
00635         for (j = 0; j < circ->dim; ++j)
00636         {
00637             *pb++ = *pv++;
00638         }
00639     }
00640 }
00641 
00642 //======================================================================
00643 //  kal_circ_buf_add
00644 //----------------------------------------------------------------------
00645 /// @brief Add new element of type s16 in buffer, erase oldest element
00646 /// @param  v           s16*                d-dimensional vector to write
00647 /// @param  circ        kal_circ_buf_t*    Pointer to the circular buffer structure
00648 /// @return             void
00649 //======================================================================
00650 _public void kal_circ_buf_add(s16* v, kal_circ_buf_t* circ)
00651 {
00652     u8 i;
00653     s16* p = &circ->buf[0] + (circ->dim * circ->curr); 
00654     for (i = 0; i < circ->dim; ++i)
00655     {
00656         *p++ = *v++;
00657     }
00658     circ->curr++;
00659     if (circ->curr == circ->len)
00660     {
00661         circ->curr = 0;
00662     }
00663 }
00664 
00665 //======================================================================
00666 //  kal_lp_filter
00667 //----------------------------------------------------------------------
00668 /// @brief Low Pass Filter with forget factor
00669 /// @param  lp          s16*                Pointer to the LP sample
00670 /// @param  s           s16*                Pointer to the new sample
00671 /// @param  dim         u8                  Number of dimensions of the sample
00672 /// @param  ff          u8                  Forget factor
00673 /// @return             void
00674 //======================================================================
00675 _public void kal_lp_filter(s16* lpf, s16* s, u8 dim, u8 ff)
00676 {
00677     if (ff)
00678     {
00679         u8 i;
00680         for (i = 0; i < dim; ++i)
00681         {
00682             s16 a = *lpf;
00683             s16 b = *s++;
00684             a += ((b - a) / ff);
00685             *lpf++ = a;
00686         }
00687     }
00688 }
00689 
00690 //======================================================================
00691 // kal_xor 
00692 //----------------------------------------------------------------------
00693 /// @brief  xor two vectors
00694 /// @param  buf         u8*                     inout stream
00695 /// @param  iv          u8*                     in stream to XOR
00696 /// @retval             void
00697 //======================================================================
00698 _public void kal_xor(u8* buf, u8* iv, u16 len)
00699 {
00700     u16 i;
00701     for(i = 0; i < len; ++i)
00702     {
00703         buf[i] ^= iv[i];
00704     }
00705 }
00706 
00707 //======================================================================
00708 //  kal_sort
00709 //----------------------------------------------------------------------
00710 /// @brief Sort u16 buffer from smaller to bigger error
00711 /// @param  inout       u16*                inout buffer
00712 /// @param  ref         u16                 comparison reference
00713 /// @param  len         u8                  buffer length
00714 /// @return             void
00715 //======================================================================
00716 _public void kal_sort(u16* inout, u16 ref, u8 len)
00717 {
00718     u8 i,j;
00719     for (i = 0; i < len; i++)
00720     {
00721         u16 tmp, min = MAX_U16;
00722         u8 k = len;
00723         for (j = i; j < len; j++)
00724         {
00725             s32 diff = inout[j] - ref;
00726             u16 d = KAL_SAT_U16(kal_abs_s32(diff));
00727             if (d <= min)
00728             {
00729                 k = j;
00730                 min = d;
00731             }
00732         }
00733         tmp = inout[i];
00734         inout[i] = inout[k];
00735         inout[k] = tmp;
00736     }
00737 }