/**
* @file ALGO_AlgorithmBase_ATY.c
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2025 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 Familiar functions of base algorithm
*
* @version
*       - 1_01_220601 > ATY
*           -# Preliminary version, first Release
********************************************************************************
*/

#ifndef __ALGO_AlgorithmBase_ATY_C
#define __ALGO_AlgorithmBase_ATY_C

#include "ALGO_AlgorithmBase_ATY.h"

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

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

/**
 * @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 temp_d * temp_d;
        else                // even n
            return x * 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;
}


/* 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 ****************************************/

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;
}


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);
}


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;
}



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

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







#ifdef __DEBUG_ALGO_AlgorithmBase_ATY
void ALGO_Swap_Test(void)
{
    uint32_t a = 0, b = 1;
    printf("\r\nALGO_Swap_Test - Before: %d - %d\r\n", a, b);
    ALGO_SWAP(uint8_t, 0, a, b);
    printf("\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("\r\nALGO_Sort_Test - Before: \r\n");
    for(uint8_t i = 0; i < 12; i++)
        printf("%x ", temp_uint32[i]);
    printf("\r\n");
    ALGO_Sort(uint32_t, 0, temp_uint32);
    printf("\r\nALGO_Sort_Test - After: \r\n");
    for(uint8_t i = 0; i < 12; i++)
        printf("%x ", temp_uint32[i]);
    printf("\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("\r\nALGO_AverageInDelExtremum_Test - Origin: \r\n");
    for(uint8_t i = 0; i < 12; i++)
        printf("%d ", temp_uint32g[i]);
    printf("\r\n");
    ALGO_AverageInDelExtremum(uint32_t, 0, temp_uint32g, temp_uint32);
    printf("\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("\r\n\r\n");
    printf("------------------\r\n");
    printf("/* - ALGO_TEST -*/\r\n");
    ALGO_Swap_Test();
    ALGO_Sort_Test();
    ALGO_AverageInDelExtremum_Test();

}

#endif /* __DEBUG_ALGO_AlgorithmBase_ATY */





#endif /* __ALGO_AlgorithmBase_ATY_C */

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