/**
* @file ALGO_WaveAnalyse_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 wave analyse from ad
*
* @version
*       - 1_01_220601 > ATY
*           -# Preliminary version, first Release
*           - todo
********************************************************************************
*/
#ifndef __ALGO_WaveAnalyse_ATY_C
#define __ALGO_WaveAnalyse_ATY_C

#include "ALGO_WaveAnalyse_ATY.h"

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

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

// 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.0);
    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.6;
                    // 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.0 * ADC_TIME_UNIT;
        waveFreq = 1000.0 / 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.0;
}




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.0 * ADC_TIME_UNIT);
                // printf("%f ", 1000.0 / (wave_quarterSpace * 4.0 * ADC_TIME_UNIT));
                // printf("%d ", wave_maxVpp);
                // printf("%f ", 2.5 * (wave_maxVpp * WAVE_KALMAN_ATTENUATION) / 4096.0);
                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.0 * ADC_TIME_UNIT);
            // printf("%f ", 1000.0 / (wave_quarterSpace * 4.0 * ADC_TIME_UNIT));
            printf("%d ", wave_maxVpp);
            // printf("%f ", 2.5 * (wave_maxVpp * WAVE_KALMAN_ATTENUATION) / 4096.0);
            printf(";");
        }

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


#endif /* __ALGO_WaveAnalyse_ATY_C */

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