ATY_LIB/ALGO_Algorithm_ATY.c

/**
* @file ALGO_Algorithm_ATY.c
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2023 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 algorithm
*
* @version
*       - 1_01_220601 > ATY
*           -# Preliminary version, first Release
********************************************************************************
*/
#ifndef __ALGO_Algorithm_ATY_C
#define __ALGO_Algorithm_ATY_C

#include "ALGO_Algorithm_ATY.h"

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

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

/* Wave parameters ************************************************************/
// wave_lastWaveAve_Kalman use last calc signal base voltage or direct signal base voltage
uint16_t wave_lastWaveAve_Kalman = 0;
// uint16_t wave_lastWaveAve_Kalman = 1468;

/**
 * @brief   find peak and zero points from origin wave data
 * @param   inputWave origin wave data from adc
 * @param   kalmanWave wave data after kalman filter
 * @param   deepKalmanWave wave data after deep kalman filter, wave averange save to [0]
 * @param   peakZeroPoints peak and zero points
 * @param   size deal size
 * @note    inputWave/kalmanWave/deepKalmanWave/peakZeroPoints group size must >= size
 */
void WavePeakZeroPointFind(
    uint16_t* inputWave,
    uint16_t* kalmanWave,
    uint16_t* deepKalmanWave,
    uint16_t* peakZeroPoints,
    uint16_t size)
{
    uint16_t i = 0;
    ALGO_Kalman1D_S temp_kfpDeep = {1, 0, 0, 0, 0, 0};
    int tempKalmanDiff = 0;                     // size should be a multiple of 5

    /* first smoothing in kalman **********************************************/
    ALGO_Kalman1D_S temp_kfp = {1, 0, 0, 0, 0, 0};
    temp_kfp.Q = 0.000000001;
    temp_kfp.R = 0.000001;
    // kalman filter delay 12 points
    for(i = 0; i < size; i++)
        kalmanWave[i] = (uint16_t)ALGO_KalmanFilter1D(&temp_kfp, (float)inputWave[i]);

    /* find difference for peaks and troughs **********************************/
    for(i = 1; i < size; i++)
    {
        if(kalmanWave[i - 1] > kalmanWave[i])
            peakZeroPoints[i] = WAVE_BASE_AVE + 1;
        else if(kalmanWave[i - 1] < kalmanWave[i])
            peakZeroPoints[i] = WAVE_BASE_AVE - 1;
        else
            peakZeroPoints[i] = peakZeroPoints[i - 1];
    }

    /* find peaks point and add base value to display *************************/
    for(i = 1; i < size; i++)
    {
        if(peakZeroPoints[i - 1] < peakZeroPoints[i])
            peakZeroPoints[i - 1] = WAVE_DISPLAY_PEAK_HIGH;
        else if(peakZeroPoints[i - 1] > peakZeroPoints[i])
            peakZeroPoints[i - 1] = WAVE_DISPLAY_PEAK_LOW;
        else
            peakZeroPoints[i - 1] = WAVE_BASE_AVE;
    }

    /* calc averange through deep kalman **************************************/
    temp_kfpDeep.Q = 0;
    temp_kfpDeep.R = 1;
    for(i = 0; i < size; i++)
        deepKalmanWave[i] = (uint16_t)ALGO_KalmanFilter1D(&temp_kfpDeep, (float)inputWave[i]);
    deepKalmanWave[0] = deepKalmanWave[size * 3 / 5];
    for(i = size * 3 / 5; i < size * 4 / 5; i++)
        tempKalmanDiff += deepKalmanWave[i] - deepKalmanWave[size * 3 / 5];
    tempKalmanDiff /= (float)(size / 5.0f);
    deepKalmanWave[0] += tempKalmanDiff;        // wave averange
    // printf("\r\ndeepKalmanAve: %d\r\n", deepKalmanWave[0]);

    /* find zero points with rise and fall and add base value to display ******/
    for(i = 1; i < size; i++)
    {
        if((kalmanWave[i - 1] < deepKalmanWave[0]) && (kalmanWave[i] >= deepKalmanWave[0]))
        {
            if(deepKalmanWave[0] - kalmanWave[i - 1] <= kalmanWave[i] - deepKalmanWave[0])
                peakZeroPoints[i - 1] = WAVE_DISPLAY_ZERO_RAISE;
            else
                peakZeroPoints[i] = WAVE_DISPLAY_ZERO_RAISE;
        }
        else if((kalmanWave[i - 1] > deepKalmanWave[0]) && (kalmanWave[i] <= deepKalmanWave[0]))
        {
            if(kalmanWave[i - 1] - deepKalmanWave[0] <= deepKalmanWave[0] - kalmanWave[i])
                peakZeroPoints[i - 1] = WAVE_DISPLAY_ZERO_FALL;
            else
                peakZeroPoints[i] = WAVE_DISPLAY_ZERO_FALL;
        }
    }
}


/* First wave find ************************************************************/
uint16_t wave_firstWavePeakPoint = 0, wave_firstWaveZeroPoint = 0;
/**
 * @brief   find peak and zero points from origin wave data
 * @param   kalmanWave wave data from kalman filter
 * @param   peakZeroPoints peak and zero points
 * @param   size deal size
 * @note    kalmanWave/peakZeroPoints group size must >= size
 */
void WaveFirstPulseFind(uint16_t* kalmanWave, uint16_t* peakZeroPoints, uint16_t size)
{
    // zero point must near mid of high peak and low peak, then count 1 valid wave data
    // step 1: find one high peak higher than vol
    // step 2: this high peak near last zero point is rise and neaxt zero point is fall
    // step 3(todo): high point subtract last zero and next zero, two space little than a value, save this high peak point
    // step 4: find low peak like this
    // step 5: find next 10 point like this
    uint16_t i = 0, j = 0, k = 0;
    uint8_t firstWaveAnalyseStep = 0, firstWaveAnalyseCount = 0;
    uint8_t secondZeroPoint = 0;
    wave_firstWavePeakPoint = 0;
    wave_firstWaveZeroPoint = 0;
    for(i = 1; i < size; i++)
    {
        if((firstWaveAnalyseStep == 0)
            && (peakZeroPoints[i] == WAVE_DISPLAY_PEAK_HIGH))
        {
            if(kalmanWave[i] > FIRST_THRESHOLD_HIGH)
            {
                if(firstWaveAnalyseCount == 0)
                    wave_firstWavePeakPoint = i;
                for(j = i - 1; j > 0; j--)
                {
                    if(peakZeroPoints[j] != WAVE_BASE_AVE)
                    {
                        if(peakZeroPoints[j] == WAVE_DISPLAY_ZERO_RAISE)
                        {
                            if(firstWaveAnalyseCount == 0)
                                wave_firstWaveZeroPoint = j;
                            for(k = i + 1; k < size; k++)
                            {
                                if(peakZeroPoints[k] != WAVE_BASE_AVE)
                                {
                                    if(peakZeroPoints[k] == WAVE_DISPLAY_ZERO_FALL)
                                    {
                                        firstWaveAnalyseStep = 2;
                                    }
                                    else
                                    {
                                        wave_firstWavePeakPoint = 0;
                                        wave_firstWaveZeroPoint = 0;
                                        firstWaveAnalyseStep = 0;
                                        firstWaveAnalyseCount = 0;
                                    }
                                    break;
                                }
                            }
                        }
                        else
                        {
                            wave_firstWavePeakPoint = 0;
                            wave_firstWaveZeroPoint = 0;
                            firstWaveAnalyseStep = 0;
                            firstWaveAnalyseCount = 0;
                        }
                        break;
                    }
                }
            }
            else
            {
                wave_firstWavePeakPoint = 0;
                wave_firstWaveZeroPoint = 0;
                firstWaveAnalyseStep = 0;
                firstWaveAnalyseCount = 0;
            }
        }
        else if((firstWaveAnalyseStep == 2)
            && (peakZeroPoints[i] == WAVE_DISPLAY_PEAK_LOW))
        {
            if(kalmanWave[i] < FIRST_THRESHOLD_LOW)
            {
                for(j = i - 1; j > 0; j--)
                {
                    if(peakZeroPoints[j] != WAVE_BASE_AVE)
                    {
                        if(peakZeroPoints[j] == WAVE_DISPLAY_ZERO_FALL)
                        {
                            if(secondZeroPoint == 0)
                            {
                                secondZeroPoint = j;
                                if(((j - wave_firstWavePeakPoint) > (wave_firstWavePeakPoint - wave_firstWaveZeroPoint)
                                    && (j - wave_firstWavePeakPoint) - (wave_firstWavePeakPoint - wave_firstWaveZeroPoint) > 5)
                                    || ((wave_firstWavePeakPoint - wave_firstWaveZeroPoint) > (j - wave_firstWavePeakPoint)
                                        && (wave_firstWavePeakPoint - wave_firstWaveZeroPoint) - (j - wave_firstWavePeakPoint) > 5))
                                    break;
                            }
                            for(k = i + 1; k < size; k++)
                            {
                                if(peakZeroPoints[k] != WAVE_BASE_AVE)
                                {
                                    if(peakZeroPoints[k] == WAVE_DISPLAY_ZERO_RAISE)
                                    {
                                        firstWaveAnalyseStep = 4;
                                    }
                                    else
                                    {
                                        wave_firstWavePeakPoint = 0;
                                        wave_firstWaveZeroPoint = 0;
                                        firstWaveAnalyseStep = 0;
                                        firstWaveAnalyseCount = 0;
                                    }
                                    break;
                                }
                            }
                        }
                        else
                        {
                            wave_firstWavePeakPoint = 0;
                            wave_firstWaveZeroPoint = 0;
                            firstWaveAnalyseStep = 0;
                            firstWaveAnalyseCount = 0;
                        }
                        break;
                    }
                }
            }
            else
            {
                wave_firstWavePeakPoint = 0;
                wave_firstWaveZeroPoint = 0;
                firstWaveAnalyseStep = 0;
                firstWaveAnalyseCount = 0;
            }
        }
        if(firstWaveAnalyseStep == 4)
        {
            firstWaveAnalyseStep = 0;
            firstWaveAnalyseCount++;
            if(firstWaveAnalyseCount >= WAVE_FIRST_COUNT)
                break;
        }
    }
    if(firstWaveAnalyseCount < WAVE_FIRST_COUNT)
    {
        wave_firstWavePeakPoint = 0;
        wave_firstWaveZeroPoint = 0;
        firstWaveAnalyseStep = 0;
        firstWaveAnalyseCount = 0;
    }
}


/* Calc wave period and frequence *********************************************/
uint16_t wave_quarterSpace = 0;
/**
 * @brief   calculate period and frequence
 * @param   peakZeroPoints wave peak and zero points after WavePeakZeroPointFind()
 * @param   firstWavePoint first wave pulse zero point, where to start analyse
 * @param   size deal size
 * @note    peakZeroPoints group size must >= size, function suit freq < 100KHz
 */
void WaveParamCalc(uint16_t* peakZeroPoints, uint16_t firstWavePoint, uint16_t size)
{
    uint16_t i = 0;
    float wavePeriod = 0.0, waveFreq = 0.0;

    uint16_t lastPoint = 0;
    uint16_t lastSpace = 0, newSpace = 0;
    uint16_t repetitionCount = 0;
    ALGO_Kalman1D_S temp_kfpDeep = {1, 0, 0, 0, 0, 0};
    temp_kfpDeep.Q = 0;
    temp_kfpDeep.R = 1;
    for(i = firstWavePoint; i < size; i++)
    {
        if(peakZeroPoints[i] != WAVE_BASE_AVE)
        {
            newSpace = i - lastPoint;
            if(repetitionCount < 2000)
            {
                // filter big changes at begining
                if((newSpace < lastSpace + 3) && (newSpace + 3 > lastSpace) && (newSpace > 5))
                {
                    repetitionCount++;
                    if(temp_kfpDeep.L_P == 1)
                        temp_kfpDeep.L_P = newSpace;
                    // carry, float to uint
                    wave_quarterSpace = ALGO_KalmanFilter1D(&temp_kfpDeep, (float)newSpace) + 0.6f;
                    // printf("$%d %d;", newSpace, wave_quarterSpace);
                }
                else
                    repetitionCount = 0;
            }
            else
            {
                // printf("$0 %d;", i);
                break;
            }
            lastSpace = newSpace;
            lastPoint = i;
        }
    }

    if(wave_quarterSpace > 0)
    {
        wavePeriod = wave_quarterSpace * 4.0f * ADC_TIME_UNIT;
        waveFreq = 1000.0f / wavePeriod;
        // printf("$%d %f %f;", wave_quarterSpace, wavePeriod, waveFreq);

        peakZeroPoints[0] = wave_quarterSpace;
    }
}

/* Calc wave vpp voltage ******************************************************/
uint16_t wave_maxVpp = 0;
/**
 * @brief   find peak and zero points from origin wave data
 * @param   kalmanWave wave data from kalman filter
 * @param   peakZeroPoints peak and zero points
 * @param   size deal size
 * @note    kalmanWave/peakZeroPoints group size must >= size
 */
void WaveVppCalc(uint16_t* kalmanWave, uint16_t* peakZeroPoints, uint16_t size)
{
    uint16_t i = 0;
    uint16_t lastWaveValue = 0;
    float maxVppVoltagge;
    for(i = 0; i < size; i++)
    {
        if((peakZeroPoints[i] == WAVE_DISPLAY_PEAK_HIGH) || (peakZeroPoints[i] == WAVE_DISPLAY_PEAK_LOW))
        {
            if(lastWaveValue != 0)
            {
                if(wave_maxVpp < kalmanWave[i] - lastWaveValue)
                    wave_maxVpp = kalmanWave[i] - lastWaveValue;
                // printf("$%d %d %d %d;", kalmanWave[i], lastWaveValue, wave_maxVpp);
            }
            lastWaveValue = kalmanWave[i];
        }
    }

    maxVppVoltagge = 2.5 * (wave_maxVpp * WAVE_KALMAN_ATTENUATION) / 4096.0f;
}




void WaveWholeProcess(
    uint16_t* inputWave,
    uint16_t* kalmanWave,
    uint16_t* deepKalmanWave,
    uint16_t* peakZeroPoints,
    uint16_t size)
{
    wave_firstWavePeakPoint = 0;
    wave_firstWaveZeroPoint = 0;
    wave_quarterSpace = 0;
    wave_maxVpp = 0;
    WavePeakZeroPointFind(inputWave, kalmanWave, deepKalmanWave, peakZeroPoints, size);
    WaveFirstPulseFind(kalmanWave, peakZeroPoints, size);
    WaveParamCalc(peakZeroPoints, wave_firstWaveZeroPoint, size);
    WaveVppCalc(kalmanWave, peakZeroPoints, size);
    wave_lastWaveAve_Kalman = deepKalmanWave[0];
}




#ifdef __DEBUG_WAVE_ATY
uint16_t kalmanWave_Test[TEST_WAVE_SIZE] = {0};
uint16_t deepKalmanWave_Test[TEST_WAVE_SIZE] = {0};
uint16_t peakZeroPoints_Test[TEST_WAVE_SIZE] = {0};
void WaveAnalyse_Test(uint16_t* testWaveDataIn, uint8_t WAVE_DISPLAY_FLAG)
{
    uint16_t i = 0;

    // debug WaveAnalyse test
    // IWDG_ENABLE_WRITE_ACCESS(&hiwdg);
    // hiwdg.Instance->PR = IWDG_PRESCALER_256;
    // hiwdg.Instance->RLR = 0xFFFFFFFF;
    // HAL_IWDG_Refresh(&hiwdg);
    WaveWholeProcess(testWaveDataIn, kalmanWave_Test, deepKalmanWave_Test, peakZeroPoints_Test, TEST_WAVE_SIZE);

    // WAVE_DISPLAY_FLAG = 1;
    if(WAVE_DISPLAY_FLAG)
    {
        if(WAVE_DISPLAY_FLAG == 1)
        {
            // for(uint16_t i = 0; i < TEST_WAVE_SIZE; i++)
            for(i = 0; i < 2000; i++)
            {
                // this "for" need feed watch dog or close it
                // HAL_IWDG_Refresh(&hiwdg);
                printf("$");
                printf("%04d ", testWaveDataIn[i]);
                printf("%04d ", kalmanWave_Test[i]);
                // printf("%04d ", deepKalmanWave_Test[i]);
                // printf("%04d ", deepKalmanWave_Test[0]);             // wave averange
                printf("%04d ", peakZeroPoints_Test[i]);
                printf("%04d ", peakZeroPoints_Test[0] * 200);        // wave 1/4 period points space
                if(i == wave_firstWavePeakPoint)
                    printf("%d ", 3000);
                else if(i == wave_firstWaveZeroPoint)
                    printf("%d ", 0);
                else
                    printf("%d ", deepKalmanWave_Test[0]);
                printf("%d ", wave_firstWavePeakPoint * 10);
                // printf("%f ", wave_firstWavePeakPoint * ADC_TIME_UNIT);
                // printf("%d ", wave_quarterSpace);
                // printf("%f ", wave_quarterSpace * 4.0f * ADC_TIME_UNIT);
                // printf("%f ", 1000.0f / (wave_quarterSpace * 4.0f * ADC_TIME_UNIT));
                // printf("%d ", wave_maxVpp);
                // printf("%f ", 2.5 * (wave_maxVpp * WAVE_KALMAN_ATTENUATION) / 4096.0f);
                printf(";");
            }
        }
        else if(WAVE_DISPLAY_FLAG == 2)
        {
            printf("$");
            printf("%d ", wave_firstWavePeakPoint);
            // printf("%f ", wave_firstWavePeakPoint * ADC_TIME_UNIT);
            printf("%d ", wave_quarterSpace * 40);
            // printf("%d ", wave_quarterSpace);
            // printf("%f ", wave_quarterSpace * 4.0f * ADC_TIME_UNIT);
            // printf("%f ", 1000.0f / (wave_quarterSpace * 4.0f * ADC_TIME_UNIT));
            printf("%d ", wave_maxVpp);
            // printf("%f ", 2.5 * (wave_maxVpp * WAVE_KALMAN_ATTENUATION) / 4096.0f);
            printf(";");
        }

        // printf("$%d;", 0);
    }
}
#endif /* __DEBUG_WAVE_ATY */



// float AdcVoltageCalc(uint16_t data_t)
// {
//     return ((data_t * REF_VOLTAGE_2_5) / 4096);
// }





// void BatVoltageGet(void) // 12V
// {
//     mainReg.powerQuantity = AdcVoltageCalc(lowAdData[1]) * 1000;
//     mainReg.chargeState = 0;
//     if(mainReg.powerQuantity >= (FULL_BAT_VOLTAGE + 300))   // higher 3V more than one bat
//     {
//         mainReg.machineState |= SELFCHECK_WRONGBAT;
//     }
//     else if((mainReg.powerQuantity >= (FULL_BAT_VOLTAGE - 20))
//         && (mainReg.powerQuantity < (FULL_BAT_VOLTAGE + 300)))
//     {
//         mainReg.chargeState = 1;
//     }
//     // BV: mainReg.maxBatVoltage, y: battery level(*100%), x: detect voltage value
//     // BV*41/42->100(fullBV), BV*37/42->20(twentyBV), BV*30/42->0(emptyBV)
//     // BV*39/42->80(eightyBV)(adjustable)
//     //
//     // y1=a*x*x+b*x+c, 100~20
//     // 100=fullBV<-x, 80=eightyBV<-x, 20=twentyBV<-x
//     // -> a=-8820/(BV*BV) -> b=17220/BV -> c=-8305
//     // y1=-8820*x*x/(BV*BV)+17220*x/BV-8305
//     //
//     // y2=a*x*x+b*x+c, 20~0
//     // -b/2*a=emptyBV -> b=-2*emptyBV*a
//     // 0=a*emptyBV*emptyBV+b*emptyBV+c -> c=emptyBV*emptyBV*a
//     // -> y2=a*x*x-2*emptyBV*a*x+a*emptyBV*emptyBV
//     // 20=a*twentyBV*twentyBV-2*emptyBV*a*twentyBV+a*emptyBV*emptyBV
//     // -> a=720/(BV*BV) -> b=-7200/(7*BV) -> c=18000/49
//     // y2=720*x*x/(BV*BV)-7200*x/(7*BV)+18000/49
//     if(mainReg.powerQuantity >= FULL_BAT_VOLTAGE)
//     {
//         mainReg.powerQuantity = 100 * 100;
//     }
//     else if((mainReg.powerQuantity < FULL_BAT_VOLTAGE) && (mainReg.powerQuantity >= TWENTY_BAT_VOLTAGE))
//     {
//         mainReg.powerQuantity =
//             ((17220.0f * (float)mainReg.powerQuantity / (float)mainReg.maxBatVoltage)
//                 - ((8820.0f * (float)mainReg.powerQuantity * (float)mainReg.powerQuantity)
//                     / ((float)mainReg.maxBatVoltage * (float)mainReg.maxBatVoltage))
//                 - 8305.0f) * 100;
//     }
//     else if((mainReg.powerQuantity < TWENTY_BAT_VOLTAGE) && (mainReg.powerQuantity > EMPTY_BAT_VOLTAGE))
//     {
//         mainReg.powerQuantity =
//             (((720.0f * (float)mainReg.powerQuantity * (float)mainReg.powerQuantity)
//                 / ((float)mainReg.maxBatVoltage * (float)mainReg.maxBatVoltage))
//                 - ((7200.0f * (float)mainReg.powerQuantity) / (7 * (float)mainReg.maxBatVoltage))
//                 + (18000.0f / 49.0f)) * 100;
//     }
//     else// if(mainReg.powerQuantity <= EMPTY_BAT_VOLTAGE)
//     {
//         mainReg.powerQuantity = 0;
//         mainReg.machineState |= SELFCHECK_LOWPOWER;
//     }
// }


// 17 significant digits
double ALGO_MATH_Pow(double num, uint8_t n)
{
    uint8_t i = 0;
    double result = 1;
    for(i = 0; i < n; i++)
        result *= num;
    return result;
}

double ALGO_MATH_LogLn(double num)
{
    // take the first 15+1 terms to estimate
    int N = 15;
    int k, nk;
    double x, xx, y;
    x = (num - 1) / (num + 1);
    xx = x * x;
    nk = 2 * N + 1;
    y = 1.0 / nk;
    for(k = N; k > 0; k--)
    {
        nk = nk - 2;
        y = 1.0 / nk + xx * y;
    }
    return 2.0 * x * y;
}


/**
 * @brief   Calculate temperature from ntc resistance
 * @param   Rntc Current NTC resistance value
 * @param   R25 NTC standard resistance value at 25C
 * @param   B B value of NTC
 * @return  Current temperature in Celsius
 * @note    T25: Kelvin temperature at 25C = 298.15 = ALGO_TEMP_CtoT(25)
 *          R25: NTC standard resistance value at 25C like 10K,5K,100K...
 *          B: B value of NTC like 3435,3950...
 *          Rntc: Current NTC resistance value
 *          Tn: Actual Kelvin temperature(Cn = Tn-273.15)
 *          B = (lnR25 - lnRntc)/(1/T25 - 1/Tn)
 */
float ALGO_ResToKelvinTemp(float Rntc, float R25, float B)
{
    float Tn = 0.0;
    float Cn = 0.0;

    float temp_f[2];

    temp_f[0] = (ALGO_MATH_LogLn(R25) - ALGO_MATH_LogLn(Rntc)) / B;
    temp_f[1] = (1 / ALGO_TEMP_CtoT(25)) - temp_f[0];
    Tn = 1 / temp_f[1];
    Cn = ALGO_TEMP_TtoC(Tn);

    return Cn;
}

/*
Pure water density with temperature at one atmosphere, 1.0g/cm3 or 1000kg/m3
y = 2E-05x3 - 0.0059x2 + 0.0141x + 1000.1 R2 = 0.9998 (Upper 4)
y = -0.0085x2 + 0.067x + 999.84 R2 = 0.9999 (Below 4)
(International Temperature Scale 1990 pure water density table)
*/
float ALGO_WaterDensityFromTemp(float temperature)
{
    float waterDensity = 0.0;
    if(temperature > 4)
        waterDensity =
        0.00002 * ALGO_MATH_Pow(temperature, 3)
        - 0.0059 * ALGO_MATH_Pow(temperature, 2)
        + 0.0141 * ALGO_MATH_Pow(temperature, 1)
        + 1000.1;
    else
        waterDensity =
        -0.0085 * ALGO_MATH_Pow(temperature, 2)
        + 0.067 * ALGO_MATH_Pow(temperature, 1)
        + 999.84;
#ifdef __DEBUG_ALGO_Algorithm_ATY
    printf("\r\nCurrent water density: %f", waterDensity);
#endif /* __DEBUG_ALGO_Algorithm_ATY */

    return waterDensity;
}


/*
<table><tr><th>名称Ch</th><td>名称En</td><td>名称EnB</td><td>符号</td><td>单位</td><td>公式</td><td>换算</td><td>备注</td><td></td></tr><tr><th>电阻</th><td>resistance</td><td>Resistance</td><td>R</td><td>Ω(欧姆)(Ohm)</td><td>U/I</td><td>1000</td><td></td><td></td></tr><tr><th>电导</th><td>conductance</td><td>Conductance</td><td>G</td><td>S(西门子)(1/Ω)</td><td>1/R</td><td>1000</td><td></td><td></td></tr><tr><th>电导池（电极）常数（系数）</th><td>cell constant</td><td>Cell Constant</td><td>K/Kcell</td><td>1/m</td><td>L/A</td><td>10</td><td>S:面积, L:距离(长度)</td><td></td></tr><tr><th>电阻率</th><td>resistivity</td><td>Resistivity</td><td>ρ</td><td>Ω*m</td><td>R*A/L</td><td></td><td></td><td></td></tr><tr><th>电导率</th><td>conductivity</td><td>Conductivity</td><td>σ/κ</td><td>S/m</td><td>L/R*A(1/ρ)</td><td>1S/m = 10000uS/cm</td><td></td><td></td></tr><tr><th>摩尔电导率</th><td>molar conductivity</td><td>Molar Conductivity</td><td>Λm</td><td>S*m^2/mol</td><td>κ/c(κ·VY)</td><td></td><td></td><td></td></tr><tr><th>物质的量</th><td>amount of substance</td><td>Amount Of Substance</td><td>n</td><td>mol(摩尔)</td><td>m/M(N/NA)</td><td>1000</td><td></td><td></td></tr><tr><th>摩尔量/摩尔质量</th><td>molal weight</td><td>Molal Weight</td><td>M</td><td>g/mol</td><td>1000</td><td></td><td></td><td></td></tr><tr><th>物质的量浓度/摩尔浓度</th><td>molarity</td><td>Molarity</td><td>c</td><td>mol/L</td><td>n/V</td><td>1L=1dm^3=1000cm^3</td><td></td><td></td></tr><tr><th>密度</th><td>density</td><td>Density</td><td>ρ</td><td>kg/m^3(g/L)</td><td>m/V</td><td></td><td></td><td></td></tr><tr><th>溶液密度</th><td>density</td><td>Density</td><td>ρY</td><td>kg/L(g/L)</td><td>mY/VY</td><td></td><td></td><td></td></tr><tr><th>质量浓度/质量-体积浓度</th><td>mass concentration</td><td>Mass Concentration</td><td>ρn</td><td>kg/L(g/L)</td><td>mZ/VY</td><td></td><td></td><td></td></tr><tr><th>质量分数/质量百分浓度/溶质质量分数</th><td>mass fraction</td><td>Mass Fraction</td><td>ω/C%</td><td>100%</td><td>mZ/mY</td><td></td><td></td><td></td></tr><tr><th>比重/相对浓度</th><td>specific gravitys</td><td>Specific Gravitys</td><td>s.g.</td><td>1</td><td>ρY/ρC</td><td>ρC一般为水ρw=1000g/L</td><td></td><td></td></tr></table>

| **NameCh**                             | **NameEn**          | **NameEnB**         | **Symbol**  | **Unit**       | **formula** | **conversion**        | **comment**          |
|:--------------------------------------:|:-------------------:|:-------------------:|:-----------:|:--------------:|:-----------:|:---------------------:|:--------------------:|
| **电阻**                               | resistance          | Resistance          | R           | Ω(欧姆)(Ohm)   | U/I         | 1000                  |
| **电导**                               | conductance         | Conductance         | G           | S(西门子)(1/Ω) | 1/R         | 1000                  |
| **电导池（电极）常数（系数）**         | cell constant       | Cell Constant       | K/Kcell     | 1/m            | L/A         | 10                    | S:面积, L:距离(长度) |
| **电阻率**                             | resistivity         | Resistivity         | ρ           | Ω*m            | R*A/L       |
| **电导率**                             | conductivity        | Conductivity        | σ/κ         | S/m            | L/R*A(1/ρ)  | 1S/m = 10000uS/cm     |
| **摩尔电导率**                         | molar conductivity  | Molar Conductivity  | Λm          | S*m^2/mol      | κ/c(κ·VY)   |
| **物质的量**                           | amount of substance | Amount Of Substance | n           | mol(摩尔)      | m/M(N/NA)   | 1000                  |
| **摩尔量/摩尔质量**                    | molal weight        | Molal Weight        | M           | g/mol          | 1000        |
| **物质的量浓度/摩尔浓度**              | molarity            | Molarity            | c           | mol/L          | n/V         | 1L=1dm^3=1000cm^3     |
| **密度**                               | density             | Density             | ρ           | kg/m^3(g/L)    | m/V         |
| **溶液密度**                           | density             | Density             | ρY          | kg/L(g/L)      | mY/VY       |
| **质量浓度/质量-体积浓度**             | mass concentration  | Mass Concentration  | ρn          | kg/L(g/L)      | mZ/VY       |
| **质量分数/质量百分浓度/溶质质量分数** | mass fraction       | Mass Fraction       | ω/C%        | 100%           | mZ/mY       |
| **比重/相对浓度**                      | specific gravitys   | Specific Gravitys   | s.g.        | 1              | ρY/ρC       | ρC一般为水ρw=1000g/L  |
*/


/**
 * @brief   Calculate Electric Conductance in S from Ohm
 * @param   Res resistance value in Ohm
 * @return  Electric Conductance value in uS
 */
float ALGO_EConductanceFromRes(float Res)
{
    float electricConductance = ((Res > 0) ? (100000 / Res) : 0);
#ifdef __DEBUG_ALGO_Algorithm_ATY
    printf("\r\nElectric Conductance: %f uS", electricConductance);
    printf("\r\nElectric Conductance: %f S-", electricConductance / 100000);
#endif /* __DEBUG_ALGO_Algorithm_ATY */

    return electricConductance;
}


/**
 * @brief   Calculate resistivity in Ohm*cm
 * @param   Res resistance value in Ohm
 * @param   A area of electrode in cm^2
 * @param   L length between electrodes in cm
 * @return  resistivity value in Ohm*cm
 * @note    1Ohm*m = 100Ohm*cm
 */
float ALGO_ResistivityFromRes(float Res, float A, float L)
{
    float resistivity = ((Res > 0) ? (Res * A / L) : 0);
#ifdef __DEBUG_ALGO_Algorithm_ATY
    printf("\r\nResistivity: %f Ohm*cm", resistivity);
    printf("\r\nResistivity: %f Ohm*m-", resistivity / 100);
#endif /* __DEBUG_ALGO_Algorithm_ATY */

    return resistivity;
}

/**
 * @brief   Calculate conductivity in uS/cm
 * @param   Res resistance value in Ohm
 * @param   A area of electrode in cm^2
 * @param   L length between electrodes in cm
 * @return  conductivity value in uS/cm
 * @note    1S/m = 10000uS/cm
 */
float ALGO_ConductivityFromRes(float Res, float A, float L)
{
    float conductivity = ((Res > 0) ? (1000000 * L / (Res * A)) : 0);
#ifdef __DEBUG_ALGO_Algorithm_ATY
    printf("\r\nConductivity: %f uS/cm", conductivity);
    printf("\r\nConductivity: %f S/m-", conductivity / 10000);
#endif /* __DEBUG_ALGO_Algorithm_ATY */

    return conductivity;
}

/**
 * @brief   Calculate molarity in mol/L
 * @param   Conductivity conductivity in uS/cm
 * @param   MC molar conductivity in S*cm^2/mol
 * @return  molarity value in mol/L
 * @note    1L = 1dm^3 =  1000cm^3
 */
// TODO: not reality, MC is changed
#define MC_NaCl 126.5       // S*cm^2/mol, Na+Cl = 50.1+76.4
float ALGO_MolarityFromConductivity(float Conductivity, float MC)
{
    // MC * 10000: change to uS*dm^2/mol
    // 10 * Conductivity: change to us/dm
    float molarity = ((Conductivity > 0) ? (10 * Conductivity / (MC * 10000)) : 0);
    // float molarity = ((Conductivity > 0) ? (10 * Conductivity / 2) : 0);
#ifdef __DEBUG_ALGO_Algorithm_ATY
    printf("\r\nMolarity: %f mol/cm^3-", molarity / 1000);
    printf("\r\nMolarity: %f mol/L", molarity);
#endif /* __DEBUG_ALGO_Algorithm_ATY */

    return molarity;
}



/**
 * @brief   Calculate mass concentration in g/L
 * @param   Molarity molarity in mol/L
 * @param   MM molar mass in g/mol
 * @return  density value in g/L
 */
#define MM_NaCl 58.44       // g/mol, Na+Cl = 22.99+35.45
float ALGO_MassConcentrationFromMolarity(float Molarity, float MM)
{
    float massConcentration = ((Molarity > 0) ? (Molarity * MM) : 0);
#ifdef __DEBUG_ALGO_Algorithm_ATY
    printf("\r\nMass Concentration: %f g/L", massConcentration);
#endif /* __DEBUG_ALGO_Algorithm_ATY */

    return massConcentration;
}










#define REF_VOLTAGE_2_5 2.5
float AdcVoltageCalc(uint16_t data_t)
{
    return ((data_t * REF_VOLTAGE_2_5) / 4096);
}

float AdcTempResCalc(uint16_t data_t)
{
    // NTC pull up resistance connect to the same voltage as Vref/VDDA in MCU
    return ((data_t * 10.0) / (4096 - data_t));
}





/*
// Least square polynomial fitting
uint8_t LSPF_Calc(uint8_t dataSize, uint8_t rank)
{
#include "math.h"
#define maxn 60
#define RANK_MAX 3
    // double x[maxn] = {0, 0.25, 0.5, 0.75, 1};
    // double y[maxn] = {1, 1.283, 1.649, 2.212, 2.178};
    // double x[maxn] = {0, 5, 10, 15, 20};//, 25, 30, 35, 40, 45, 50, 55};
    // double y[maxn] = {0, 1.27, 2.16, 2.86, 3.44};//, 3.87, 4.15, 4.37, 4.51, 4.58, 4.02, 4.64};
    double x[maxn] = {0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55};
    double y[maxn] = {0, 1.27, 2.16, 2.86, 3.44, 3.87, 4.15, 4.37, 4.51, 4.58, 4.02, 4.64};

    double atemp[2 * (RANK_MAX + 1)] = {0}, btemp = 0, a[RANK_MAX + 1][RANK_MAX + 1], b[RANK_MAX + 1] = {0};
    int i, j, k;

    for(i = 0; i < dataSize; i++)
    {
        for(j = 0; j < rank + 1; j++)
            b[j] += pow(x[i], j) * y[i];

        for(j = 0; j < (2 * rank + 1); j++)
            atemp[j] += pow(x[i], j);
    }
    atemp[0] = dataSize;
    // 构建线性方程组系数矩阵，b[]不变
    for(i = 0; i < rank + 1; i++)
    {
        k = i;
        for(j = 0; j < rank + 1; j++)
            a[i][j] = atemp[k++];
    }
    // 以下为高斯列主元消去法解线性方程组
    for(k = 0; k < rank + 1 - 1; k++)
    {   // n - 1列
        int column = k;
        double mainelement = a[k][k];
        // 找主元素
        for(i = k; i < rank + 1; i++)
            if(fabs(a[i][k]) > mainelement)
            {
                mainelement = fabs(a[i][k]);
                column = i;
            }
        // 交换两行
        for(j = k; j < rank + 1; j++)
        {
            double atemp = a[k][j];
            a[k][j] = a[column][j];
            a[column][j] = atemp;
        }
        btemp = b[k];
        b[k] = b[column];
        b[column] = btemp;
        // 消元过程
        for(i = k + 1; i < rank + 1; i++)
        {
            double Mik = a[i][k] / a[k][k];
            for(j = k; j < rank + 1; j++)
                a[i][j] -= Mik * a[k][j];
            b[i] -= Mik * b[k];
        }
    }
    // 回代过程
    b[rank + 1 - 1] /= a[rank + 1 - 1][rank + 1 - 1];
    for(i = rank + 1 - 2; i >= 0; i--)
    {
        double sum = 0;
        for(j = i + 1; j < rank + 1; j++)
            sum += a[i][j] * b[j];
        b[i] = (b[i] - sum) / a[i][i];
    }

    //高斯列主元消去法结束，输出
    // [0] = c, [1] = b (* x)
    for(i = 0; i < rank + 1; i++)
        UartSendFloatStr(b[i]);

    return 0;
}
*/







#endif /* __ALGO_Algorithm_ATY_C */

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