ATY_LIB/ALGO_AlgorithmBase_ATY.c

/**
* @file ALGO_AlgorithmBase_ATY.c
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2026 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief functions of base algorithm
*
* @version
*       - 1_01_220601 > ATY
*           -# Preliminary version, first Release
*       - 1_02_251124 > ATY
*           -# add and test
* @todo long support
********************************************************************************
*/

#ifndef __ALGO_AlgorithmBase_ATY_C
#define __ALGO_AlgorithmBase_ATY_C

#include "ALGO_AlgorithmBase_ATY.h"
#define ALGO_AlgorithmBase_ATY_TAG "\r\n[ALGO_AlgorithmBase_ATY] "

/******************************* For user *************************************/

/******************************************************************************/

/* Bit Inversion **************************************************************/
/* Easy way to implement Invert ***********************************************/
/**
 * @brief   Invert buf with uint8 size(LSB <-> MSB)
 * @param   genBuf Generate buf
 * @param   srcBuf Input source buf
 */
void ALGO_InvertUint8_Group(uint8_t* genBuf, uint8_t* srcBuf){
    uint8_t i = 0;
    uint8_t temp_uint8 = 0;

    for(i = 0; i < 8; i++){
        if((*srcBuf) & (1 << i))
            temp_uint8 |= 1 << (7 - i);
    }
    *genBuf = temp_uint8;
}

/**
 * @brief   Invert buf with uint16 size(LSB <-> MSB)
 * @param   genBuf Generate buf
 * @param   srcBuf Input source buf
 */
void ALGO_InvertUint16_Group(uint16_t* genBuf, uint16_t* srcBuf){
    uint8_t i = 0;
    uint16_t temp_uint16 = 0;

    for(i = 0; i < 16; i++){
        if((*srcBuf) & (1 << i))
            temp_uint16 |= 1 << (15 - i);
    }
    *genBuf = temp_uint16;
}

/**
 * @brief   Invert buf with uint32 size(LSB <-> MSB)
 * @param   genBuf Generate buf
 * @param   srcBuf Input source buf
 */
void ALGO_InvertUint32_Group(uint32_t* genBuf, uint32_t* srcBuf){
    uint8_t i = 0;
    uint32_t temp_uint32 = 0;

    for(i = 0; i < 32; i++){
        if((*srcBuf) & (1 << i))
            temp_uint32 |= 1 << (31 - i);
    }
    *genBuf = temp_uint32;
}

/**
 * @brief   Invert buf with n size(LSB <-> MSB)
 * @param   genBuf Generate buf
 * @param   srcBuf Input source buf
 * @param   len Data length
 */
void ALGO_InvertBitsN_Group(uint32_t* genBuf, uint32_t* srcBuf, uint8_t len){
    uint8_t i = 0;
    uint32_t temp_uint32 = 0;

    for(i = 0; i < len; i++){
        if((*srcBuf) & (1 << i))
            temp_uint32 |= 1 << (len - 1 - i);
    }
    *genBuf = temp_uint32;
}
/* End of Easy way to implement Invert ****************************************/


/* Math Utilities *************************************************************/
/**
 * @brief   Pow() function in easy way
 * @param   x value to deal
 * @param   n index, >= 0
 * @note    17 significant digits
 */
float ALGO_MATH_POW_EASY(float x, uint8_t n){
    float result = 1;
    while(n--)
        result *= x;
    return result;
}

/**
 * @brief   Pow() function in quick way
 * @param   x value to deal
 * @param   n index, >= 0
 */
int ALGO_MATH_POW_QUICK(int x, int n){
    float result = 1;
    while(n){
        if(n & 1) result = result * x;
        x = x * x;
        n >>= 1;
    }
    return result;
}

/**
 * @brief   Pow() function
 * @param   x value to deal
 * @param   n index
 */
float ALGO_MATH_POW(float x, int n){
    if(n == 0)
        return 1.0;
    else if(n < 0)
        return 1.0 / ALGO_MATH_POW(x, -n);
    else if(n > 0){
        float temp_d = ALGO_MATH_POW(x, n / 2);
        if((n % 2) == 1)    // odd n
            return x * temp_d * temp_d;
        else                // even n
            return temp_d * temp_d;
    }
    return 0;
}

/**
 * @brief   Sqrt() function
 * @param   x value to deal
 */
int ALGO_MATH_SQRT(int x){
    int left = 1;
    int right = x;
    int result = 0;

    while(left <= right){
        int mid = left + (right - left) / 2;
        if(mid <= x / mid){
            result = mid;
            left = mid + 1;
        }
        else{
            right = mid - 1;
        }
    }
    return result;
}

/**
 * @brief   ln() function
 * @param   x value to deal
 * @note    17 significant digits
 */
float ALGO_MATH_LogLn(float x){
    // take the first 15+1 terms to estimate
    const int N = 15;
    int k, nk;
    float a, aa, b;
    a = (x - 1) / (x + 1);
    aa = a * a;
    nk = 2 * N + 1;
    b = 1.0 / nk;
    for(k = N; k > 0; k--){
        nk = nk - 2;
        b = 1.0 / nk + aa * b;
    }
    return 2.0 * a * b;
}


float ALGO_NumberSuitScop(float valueIn, float scopMin, float scopMax, float step){
    uint16_t errCount = 0;
    while((valueIn < scopMin || valueIn > scopMax) && (uint32_t)step != 0){
        errCount++; if(errCount > 60000) return -1;
        if(valueIn < scopMin)
            valueIn += step;
        else if(valueIn > scopMax)
            valueIn -= step;
    }
    return valueIn;
}


uint16_t ALGO_BinarySearch(const double* arr, uint16_t size, double target){
    uint16_t l = 0, r = size;
    while(l < r){
        uint16_t m = l + ((r - l) >> 1);
        if(arr[m] < target) l = (uint16_t)(m + 1);
        else r = m;
    }
    return (l < size) ? l : size;
}

float ALGO_Sqrt_NewtonNumber(float x){
    float xhalf = 0.5 * x;
    int i = *(int*)&x;

    if(!x) return 0;

    i = 0x5f375a86 - (i >> 1);      // beautiful number
    x = *(float*)&i;
    x = x * (1.5 - xhalf * x * x); // Newton iteration
    x = x * (1.5 - xhalf * x * x); // Newton iteration
    x = x * (1.5 - xhalf * x * x); // Newton iteration
    return (1 / x);
}


/* Metrics ********************************************************************/
float ALGO_GetRms(uint16_t currentValue, uint16_t currentNum, float sumValue, uint16_t wholeNum){
    float value = sumValue;
    value += (currentValue * currentValue);
    if(currentNum >= wholeNum){
        value /= currentNum;
        value = ALGO_Sqrt_NewtonNumber(value);
        return value;
    }
    return 0;
}



/* RC Filters *****************************************************************/
double ALGO_RC_LpFilter(ALGO_RC_FilterPara_t* rcPara, double val){
    rcPara->lVal = ((double)val * rcPara->k + rcPara->lVal * (1 - rcPara->k));
    return rcPara->lVal;
}

double ALGO_RC_HpFilter(ALGO_RC_FilterPara_t* rcPara, double val){
    rcPara->lVal = ((double)val * rcPara->k + rcPara->lVal * (1 - rcPara->k));
    return -(val - rcPara->lVal);
}







/* Debug Tests ****************************************************************/
#ifdef __DEBUG_ALGO_AlgorithmBase_ATY
void ALGO_Swap_Test(void){
    uint32_t a = 0, b = 1;
    printf_ATY("\r\nALGO_Swap_Test - Before: %d - %d\r\n", a, b);
    ALGO_SWAP(uint8_t, 0, a, b);
    printf_ATY("\r\nALGO_Swap_Test - After: %d - %d\r\n", a, b);
}

void ALGO_Sort_Test(void){
    const uint32_t temp_uint32[12] = {
        0xffffff14, 0xffffff19, 0xffffff17, 0xffffff12,
        0xffffff18, 0xffffff00, 0xffffff11, 0xffffffff,
        0xffffff13, 0xffffff15, 0xffffff16, 0xffffff20};
    printf_ATY("\r\nALGO_Sort_Test - Before: \r\n");
    for(uint8_t i = 0; i < 12; i++)
        printf_ATY("%x ", temp_uint32[i]);
    printf_ATY("\r\n");
    ALGO_Sort(uint32_t, 0, temp_uint32);
    printf_ATY("\r\nALGO_Sort_Test - After: \r\n");
    for(uint8_t i = 0; i < 12; i++)
        printf_ATY("%x ", temp_uint32[i]);
    printf_ATY("\r\n");
}

void ALGO_AverageInDelExtremum_Test(void){
    const uint32_t temp_uint32g[12] = {14, 19, 17, 12, 18, 0, 11, 1055, 13, 15, 16, 20};
    uint32_t temp_uint32;
    printf_ATY("\r\nALGO_AverageInDelExtremum_Test - Origin: \r\n");
    for(uint8_t i = 0; i < 12; i++)
        printf_ATY("%d ", temp_uint32g[i]);
    printf_ATY("\r\n");
    ALGO_AverageInDelExtremum(uint32_t, 0, temp_uint32g, temp_uint32);
    printf_ATY("\r\nALGO_AverageInDelExtremum_Test - Result: %d\r\n", temp_uint32);
}

void ALGO_INVERT_Test(void){
    uint32_t TT0 = 0;
    const uint8_t TT1 = 0xF0;         // 0x0F
    const uint8_t TT2 = 0x12;         // 0x48
    const uint16_t TT3 = 0xF0E0;      // 0x070F
    const uint16_t TT4 = 0x1234;      // 0x2C48
    const uint32_t TT5 = 0xF0E0D0C0;  // 0x030B070F
    const uint32_t TT6 = 0x12345678;  // 0x1E6A2C48
    ALGO_INVERT_BITS(TT1);
    TT0 = TT1;
    ALGO_INVERT_BITS(TT2);
    TT0 = TT2;
    ALGO_INVERT_BITS(TT3);
    TT0 = TT3;
    ALGO_INVERT_BITS(TT4);
    TT0 = TT4;
    ALGO_INVERT_BITS(TT5);
    TT0 = TT5;
    ALGO_INVERT_BITS(TT6);
    TT0 = TT6;
    TT0 = 0;
}

void ALGO_Test_Whole(void){
    printf_ATY("\r\n\r\n");
    printf_ATY("------------------\r\n");
    printf_ATY("/* - ALGO_TEST -*/\r\n");
    ALGO_Swap_Test();
    ALGO_Sort_Test();
    ALGO_AverageInDelExtremum_Test();

}
#endif /* __DEBUG_ALGO_AlgorithmBase_ATY */

#ifdef ALGO_AlgorithmBase_ATY_Test_ATY
uint8_t ALGO_AlgorithmBase_ATY_Test_Pass;
uint8_t ALGO_AlgorithmBase_ATY_Test_Fail;
static int __aty_close_d(double a, double b, double tol){ return (a > b ? (a - b) : (b - a)) <= tol; }
static int __aty_equal_u(uint32_t a, uint32_t b){ return a == b; }
uint32_t ALGO_AlgorithmBase_ATY_Test(void){
    ALGO_AlgorithmBase_ATY_Test_Pass = 0;
    ALGO_AlgorithmBase_ATY_Test_Fail = 0;
    printf_ATY_D("%sSTART\r\n", ALGO_AlgorithmBase_ATY_TAG);

    {
        int a = -5, b = 7;
        int abs_a = ALGO_ABS(a);
        int max_ab = ALGO_MAX(a, b);
        int min_ab = ALGO_MIN(a, b);
        int ok1 = __aty_equal_u((uint32_t)abs_a, (uint32_t)5);
        int ok2 = __aty_equal_u((uint32_t)max_ab, (uint32_t)7);
        int ok3 = __aty_equal_u((uint32_t)min_ab, (uint32_t)-5);
        if(ok1) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        if(ok2) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        if(ok3) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    ABS(a) = %d, MAX = %d, MIN = %d, %s\r\n", abs_a, max_ab, min_ab, ((ok1 && ok2 && ok3) ? "PASS" : "FAIL"));
    }

    {
        int okd1 = ALGO_DECCHK('5');
        int okd2 = !ALGO_DECCHK('A');
        int okh1 = ALGO_HEXCHK('F');
        int okh2 = !ALGO_HEXCHK('G');
        if(okd1 && okd2) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        if(okh1 && okh2) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    DEC = %d/%d, HEX = %d/%d, %s\r\n", okd1, okd2, okh1, okh2, ((okd1 && okd2 && okh1 && okh2) ? "PASS" : "FAIL"));
    }

    {
        uint16_t c16 = ALGO_COMB16(0x12, 0x34);
        uint32_t c32 = ALGO_COMB32(0x12, 0x34, 0x56, 0x78);
        uint8_t l16 = ALGO_UINT16_L(0x1234);
        uint8_t h16 = ALGO_UINT16_H(0x1234);
        int okc16 = __aty_equal_u(c16, 0x1234u);
        int okc32 = __aty_equal_u(c32, 0x12345678u);
        int oklh = __aty_equal_u((uint32_t)l16, (uint32_t)0x34u) && __aty_equal_u((uint32_t)h16, (uint32_t)0x12u);
        if(okc16) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        if(okc32) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        if(oklh) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    COMB16 = 0x%x, COMB32 = 0x%x, L = 0x%x, H = 0x%x, %s\r\n", c16, c32, l16, h16, ((okc16 && okc32 && oklh) ? "PASS" : "FAIL"));
    }

    {
        int okb = __aty_equal_u((uint32_t)ALGO_BOOL(5), (uint32_t)1);
        int okn = __aty_equal_u((uint32_t)ALGO_NOT(0), (uint32_t)1);
        int oka = __aty_equal_u((uint32_t)ALGO_AND(1, 0), (uint32_t)0);
        int oko = __aty_equal_u((uint32_t)ALGO_OR(1, 0), (uint32_t)1);
        if(okb && okn && oka && oko) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    BOOL = %d, NOT = %d, AND = %d, OR = %d, %s\r\n", ALGO_BOOL(5), ALGO_NOT(0), ALGO_AND(1, 0), ALGO_OR(1, 0), ((okb && okn && oka && oko) ? "PASS" : "FAIL"));
    }

    {
        float pe = ALGO_MATH_POW_EASY(2.0f, 10);
        int pq = ALGO_MATH_POW_QUICK(2, 10);
        float pn = ALGO_MATH_POW(2.0f, 10);
        int okpe = __aty_close_d(pe, 1024.0, 1e-5);
        int okpq = __aty_equal_u((uint32_t)pq, (uint32_t)1024);
        int okpn = __aty_close_d(pn, 1024.0, 1e-3);
        if(okpe) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        if(okpq) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        if(okpn) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    POW_EASY = %f, POW_QUICK = %d, POW = %f, %s\r\n", pe, pq, pn, ((okpe && okpq && okpn) ? "PASS" : "FAIL"));
    }

    {
        int s10 = ALGO_MATH_SQRT(10);
        int s16 = ALGO_MATH_SQRT(16);
        int oks = __aty_equal_u((uint32_t)s10, (uint32_t)3) && __aty_equal_u((uint32_t)s16, (uint32_t)4);
        if(oks) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    SQRT(10) = %d, SQRT(16) = %d, %s\r\n", s10, s16, (oks ? "PASS" : "FAIL"));
    }

    {
        float le = ALGO_MATH_LogLn(2.7182818f);
        int okle = __aty_close_d(le, 1.0, 1e-3);
        if(okle) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    LN(e) = %f, %s\r\n", le, (okle ? "PASS" : "FAIL"));
    }

    {
        float sn = ALGO_Sqrt_NewtonNumber(9.0f);
        int oksi = __aty_close_d(sn, 3.0, 1e-3);
        if(oksi) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    NewtonSqrt(9) = %f, %s\r\n", sn, (oksi ? "PASS" : "FAIL"));
    }

    {
        ALGO_RC_FilterPara_t lp = {0.1, 0.0};
        double o1 = ALGO_RC_LpFilter(&lp, 0.0);
        double o2 = ALGO_RC_LpFilter(&lp, 10.0);
        double o3 = ALGO_RC_LpFilter(&lp, 10.0);
        double e2 = 10.0 * 0.1 + 0.0 * 0.9;
        double e3 = 10.0 * 0.1 + e2 * 0.9;
        int okrc = __aty_close_d(o1, 0.0, 1e-9) && __aty_close_d(o2, e2, 1e-9) && __aty_close_d(o3, e3, 1e-9);
        if(okrc) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    LP o1 = %f, o2 = %f, o3 = %f, %s\r\n", o1, o2, o3, (okrc ? "PASS" : "FAIL"));
    }

    {
        ALGO_RC_FilterPara_t hp = {0.1, 0.0};
        double h1 = ALGO_RC_HpFilter(&hp, 0.0);
        double h2 = ALGO_RC_HpFilter(&hp, 10.0);
        double h3 = ALGO_RC_HpFilter(&hp, 10.0);
        double e2 = -(10.0 - (10.0 * 0.1 + 0.0 * 0.9));
        double e3 = -(10.0 - (10.0 * 0.1 + (10.0 * 0.1 + 0.0 * 0.9) * 0.9));
        int okhp = __aty_close_d(h1, 0.0, 1e-9) && __aty_close_d(h2, e2, 1e-9) && __aty_close_d(h3, e3, 1e-9);
        if(okhp) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    HP h1 = %f, h2 = %f, h3 = %f, %s\r\n", h1, h2, h3, (okhp ? "PASS" : "FAIL"));
    }

    {
        float v1 = ALGO_NumberSuitScop(105.0f, 0.0f, 100.0f, 1.0f);
        float v2 = ALGO_NumberSuitScop(-5.0f, 0.0f, 100.0f, 1.0f);
        int okns = __aty_close_d(v1, 100.0, 1e-5) && __aty_close_d(v2, 0.0, 1e-5);
        if(okns) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    SuitScop v1 = %f, v2 = %f, %s\r\n", v1, v2, (okns ? "PASS" : "FAIL"));
    }

    {
        uint16_t whole = 2;
        float sum = 0.0f;
        float r1 = ALGO_GetRms(3, 1, sum, whole);
        sum += 3 * 3;
        float r2 = ALGO_GetRms(4, 2, sum, whole);
        int okr = __aty_close_d(r2, 3.5355, 1e-3);
        if(okr) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    RMS r1 = %f, r2 = %f, %s\r\n", r1, r2, (okr ? "PASS" : "FAIL"));
    }

    {
        double arr[5] = {1.0, 2.0, 3.0, 5.0, 8.0};
        uint16_t i1 = ALGO_BinarySearch(arr, 5, 5.0);
        uint16_t i2 = ALGO_BinarySearch(arr, 5, 4.0);
        int okb1 = __aty_equal_u((uint32_t)i1, (uint32_t)3);
        int okb2 = __aty_equal_u((uint32_t)i2, (uint32_t)3);
        if(okb1) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        if(okb2) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    BinSearch i1 = %d, i2 = %d, %s\r\n", i1, i2, ((okb1 && okb2) ? "PASS" : "FAIL"));
    }

    {
        uint8_t g8 = 0; uint8_t s8 = 0xF0; ALGO_InvertUint8_Group(&g8, &s8);
        uint16_t g16 = 0; uint16_t s16 = 0x1234; ALGO_InvertUint16_Group(&g16, &s16);
        uint32_t g32 = 0; uint32_t s32 = 0x12345678; ALGO_InvertUint32_Group(&g32, &s32);
        int oki = __aty_equal_u((uint32_t)g8, (uint32_t)0x0Fu) && __aty_equal_u((uint32_t)g16, (uint32_t)0x2C48u) && __aty_equal_u((uint32_t)g32, (uint32_t)0x1E6A2C48u);
        if(oki) ALGO_AlgorithmBase_ATY_Test_Pass++; else ALGO_AlgorithmBase_ATY_Test_Fail++;
        printf_ATY_D("    Invert g8 = 0x%x, g16 = 0x%x, g32 = 0x%x, %s\r\n", g8, g16, g32, (oki ? "PASS" : "FAIL"));
    }

    printf_ATY_D("%sEND: Pass = %d, Fail = %d\r\n", ALGO_AlgorithmBase_ATY_TAG, ALGO_AlgorithmBase_ATY_Test_Pass, ALGO_AlgorithmBase_ATY_Test_Fail);
    return ALGO_AlgorithmBase_ATY_Test_Fail;
}
#endif /* ALGO_AlgorithmBase_ATY_Test_ATY */







#endif /* __ALGO_AlgorithmBase_ATY_C */

/******************************** End Of File *********************************/