#include "gtest/gtest.h"
#include <gtest/gtest.h>
#include <stdint.h>
#include <tuple>
#include "scope.h"
#include "test_user.h"
#include <cmath>

class CurSampleCalibTest : public testing::Test
{
protected:
    static void SetUpTestSuite()
    {
        CodeParaInit();
        adc_voSampleInit();
        pwm_voInit();
        scm_uwSpdFbkLpfAbsPu = 0;
    }
    virtual void SetUp() override
    {}
    virtual void TearDown() override
    {
        adc_voSampleInit();
        pwm_voInit();
        scm_uwSpdFbkLpfAbsPu = 0;
    }
};

class CurSampleCalibTest1 : public CurSampleCalibTest, public testing::WithParamInterface<::std::tuple<double, double, double>>
{};

TEST_P(CurSampleCalibTest1, SingResCalib)
{
    int    timerPrd = HW_PWM_PERIOD;
    int    timerCnt = 0, timerCntDir = 0, prdCnt = 0;
    double phase, ia, ib, ic, iaRdson, ibRdson, icRdson;
    double currentFreqHz = 100;             // unit: Hz
    double currentMod = get<0>(GetParam()); // unit: A
    double rdsonCoef = get<1>(GetParam());
    double emf = get<2>(GetParam()); // unit: V
    double loopNum = 100000000;
    double spdAbsTarget = emf * 32768 * 1000 / M_FLUX_WB / 2 / 3.14 * 1000 / FBASE; // Q15

    /* Coef Cal */
    adc_voSampleCoef(&adc_stCof);
    adc_stCof.uwIaOffset = 2048;
    adc_stCof.uwIbOffset = 2048;
    adc_stCof.uwIcOffset = 2048;
    adc_stCof.uwIdcOffset = 0;

    pwm_stGenCoefIn.uwPWMDutyMax = cp_stControlPara.swPWMMaxDuty;
    pwm_stGenCoefIn.uwPWM7To5Duty = cp_stControlPara.swPWM7to5Duty;
    pwm_stGenCoefIn.uwPWMMinSample1Pu = cp_stControlPara.swPWMMinSampleDuty1;
    pwm_stGenCoefIn.uwPWMMinSample2Pu = cp_stControlPara.swPWMMinSampleDuty2;
    pwm_stGenCoefIn.uwPWMMinSample3Pu = cp_stControlPara.swPWMMinSampleDuty3;
    pwm_stGenCoefIn.uwSampleSteadyPu = cp_stControlPara.swPWMSampleToSteady;
    pwm_stGenCoefIn.uwSingelResisSamplePu = cp_stControlPara.swPWMSampleSigR;
    pwm_stGenCoefIn.uwOvmNo = cp_stControlPara.swPWMOverMdlMode;
    pwm_stGenCoefIn.uwPWMPd = HW_PWM_PERIOD;
    pwm_voGenCoef(&pwm_stGenCoefIn, &pwm_stGenCoef);

    for (int i = 0; i < loopNum; i++)
    {
        /* Current */
        phase = currentFreqHz * 2 * 3.14 * i / (TIM0CLK_KHZ * 1000);
        ia = currentMod * sin(phase);
        ib = currentMod * sin(phase + (double)2 / 3 * 3.14);
        ic = currentMod * sin(phase + (double)4 / 3 * 3.14);

        iaRdson = ia * rdsonCoef;
        ibRdson = ib * rdsonCoef;
        icRdson = ic * rdsonCoef;

        /* Timer: centre-aligned */
        if (timerCntDir == 0)
        {
            timerCnt++;
            if (timerCnt == (timerPrd / 2))
            {
                testTimerIntFlg[TIMER0][TIMER_INT_FLAG_UP] = 1;
                timerCntDir = 1;
            }
        }
        else
        {
            timerCnt--;
            if (timerCnt == 0)
            {
                testTimerIntFlg[TIMER0][TIMER_INT_FLAG_UP] = 1;
                timerCntDir = 0;
                prdCnt++;
            }
        }

        /* ADC Trigger */
        if (timerCntDir == 0)
        {
            if (prdCnt % 2 == 1)
            {
                if (timerCnt == pwm_stGenOut.uwRdsonTrig)
                {
                    double iaReg = (double)adc_stCof.uwIaOffset - iaRdson * 100 * 2048 / ADC_IPHASE_CUR_MAX_AP; // Negative direction current
                    if (iaReg < 0)
                    {
                        iaReg = 0;
                    }
                    if (iaReg > 4096)
                    {
                        iaReg = 4096;
                    }

                    double ibReg = (double)adc_stCof.uwIbOffset - ibRdson * 100 * 2048 / ADC_IPHASE_CUR_MAX_AP; // Negative direction current
                    if (ibReg < 0)
                    {
                        ibReg = 0;
                    }
                    if (ibReg > 4096)
                    {
                        ibReg = 4096;
                    }

                    double icReg = (double)adc_stCof.uwIcOffset - icRdson * 100 * 2048 / ADC_IPHASE_CUR_MAX_AP; // Negative direction current
                    if (icReg < 0)
                    {
                        icReg = 0;
                    }
                    if (icReg > 4096)
                    {
                        icReg = 4096;
                    }

                    ADC_IDATA0(ADC0) = iaReg;
                    ADC_IDATA1(ADC0) = ibReg;
                    ADC_IDATA2(ADC0) = icReg;

                    testAdcIntFlg[ADC0][ADC_INT_FLAG_EOIC] = 1;
                }
            }
            else
            {
                if (timerCnt == pwm_stGenOut.uwSigRTrig)
                {
                    switch (pwm_stGenOut.uwSingelRSampleArea)
                    {
                    case SampleA:
                        ADC_IDATA0(ADC1) = (UWORD)(adc_stCof.uwIdcOffset + ABS(ia) * 100 * 4096 / ADC_IDC_CUR_MAX_AP);
                        if (ADC_IDATA0(ADC1) > 4096)
                        {
                            ADC_IDATA0(ADC1) = 4096;
                        }

                        break;
                    case SampleB:
                        ADC_IDATA0(ADC1) = (UWORD)(adc_stCof.uwIdcOffset + ABS(ib) * 100 * 4096 / ADC_IDC_CUR_MAX_AP);
                        if (ADC_IDATA0(ADC1) > 4096)
                        {
                            ADC_IDATA0(ADC1) = 4096;
                        }

                        break;
                    case SampleC:
                        ADC_IDATA0(ADC1) = (UWORD)(adc_stCof.uwIdcOffset + ABS(ic) * 100 * 4096 / ADC_IDC_CUR_MAX_AP);
                        if (ADC_IDATA0(ADC1) > 4096)
                        {
                            ADC_IDATA0(ADC1) = 4096;
                        }

                        break;
                    default:
                        break;
                    }

                    testAdcIntFlg[ADC1][ADC_INT_FLAG_EOIC] = 1;
                }
            }
        }

        /* ADC Interrupt */
        if (testAdcIntFlg[ADC0][ADC_INT_FLAG_EOIC] == 1)
        {
            adc_uwRdsonUReg = ADC_IDATA0(ADC0);
            adc_uwRdsonVReg = ADC_IDATA1(ADC0);
            adc_uwRdsonWReg = ADC_IDATA2(ADC0);

            testAdcIntFlg[ADC0][ADC_INT_FLAG_EOIC] = 0;
        }

        if (testAdcIntFlg[ADC1][ADC_INT_FLAG_EOIC] == 1)
        {
            adc_uwADDMAPhase1 = ADC_IDATA0(ADC1);

            testAdcIntFlg[ADC1][ADC_INT_FLAG_EOIC] = 0;
        }

        /* Api变量赋值 */
        Adcs[1].Results[HW_ADC_IA_CH] = adc_uwRdsonUReg;
        Adcs[1].Results[HW_ADC_IB_CH] = adc_uwRdsonVReg;
        Adcs[1].Results[HW_ADC_IC_CH] = adc_uwRdsonWReg;
        Adcs[2].Results[HW_ADC_IDC_CH] = adc_uwADDMAPhase1;


        /* Timer Interrupt */
        if (testTimerIntFlg[TIMER0][TIMER_INT_FLAG_UP] == 1)
        {
            if ((timerCntDir == 1) && (prdCnt % 2 == 1))
            {
                /* Speed Mock */             
                if(scm_uwSpdFbkLpfAbsPu < spdAbsTarget)
                {
                    scm_uwSpdFbkLpfAbsPu += 20;     ///< 转速需模拟实际从0上升，否则采样校准系数会计算为0
                }
                else 
                {
                    scm_uwSpdFbkLpfAbsPu = spdAbsTarget;
                }

                /* Sample and Calibration */ 
                adc_voSampleUp(&adc_stCof, &adc_stUpOut);
                adc_voSampleDown(&adc_stCof, &adc_stDownOut);
                adc_voSRCalibration(&adc_stCof, &adc_stUpOut, &adc_stDownOut);

                /* ADC Trigger Cal */ 
                double phase2 = atan2((double)currentMod * M_LQ_NOLOAD_MH / 100 / 1000000, (double )M_FLUX_WB / 1000000) ;
                pwm_stGenIn.swUalphaPu = (SWORD)(emf * 10 * sin(phase + phase2) * 16384 / cos(phase2) / VBASE); // Q14
                pwm_stGenIn.swUbetaPu = (SWORD)(emf * 10 * cos(phase + phase2) * 16384 / cos(phase2) / VBASE);  // Q14
                pwm_stGenIn.uwVdcPu = (UWORD)(36 * 10 * 16384 / VBASE);                                             // Q14
                pwm_voGen(&pwm_stGenIn, &pwm_stGenCoef, &pwm_stGenOut);

                /* Scope */ 
                UdpScope::Send(0, adc_stDownOut.slSampIcPu, adc_stUpOut.swCalibIcPu, adc_stDownOut.swIcPu);
                //UdpScope::Send(0, scm_uwSpdFbkLpfAbsPu, pwm_stGenOut.blSampleCalibFlag);

                /* Determine whether the unit test pass */
                if (prdCnt > (loopNum / HW_PWM_PERIOD - 5))
                {
                    if (!adc_stCof.blCalibCalFlag)
                    {
                        if(adc_stDownOut.ulISamplePeakPu > 32767)  
                        {
                            EXPECT_NEAR((double)adc_stDownOut.slSampIaPu * 780 / 1024, adc_stDownOut.swIaPu, 10);   ///< 系数为立即数，程序中更改后测试用例需要随之更改
                            EXPECT_NEAR((double)adc_stDownOut.slSampIbPu * 780 / 1024, adc_stDownOut.swIbPu, 10);
                            EXPECT_NEAR((double)adc_stDownOut.slSampIcPu * 780 / 1024, adc_stDownOut.swIcPu, 10);
                        }
                        // else if(adc_stDownOut.ulISamplePeakPu > 25800) 
                        // {
                        //     EXPECT_NEAR((double)adc_stDownOut.slSampIaPu * 1024 / 1024, adc_stDownOut.swIaPu, 10);  ///< 系数为立即数，程序中更改后测试用例需要随之更改
                        //     EXPECT_NEAR((double)adc_stDownOut.slSampIbPu * 1024 / 1024, adc_stDownOut.swIbPu, 10);
                        //     EXPECT_NEAR((double)adc_stDownOut.slSampIcPu * 1024 / 1024, adc_stDownOut.swIcPu, 10);
                        // }
                        else
                        {
                            EXPECT_NEAR((double)adc_stDownOut.slSampIaPu * 1024 / 1024, adc_stDownOut.swIaPu, 10);  ///< 系数为立即数，程序中更改后测试用例需要随之更改
                            EXPECT_NEAR((double)adc_stDownOut.slSampIbPu * 1024 / 1024, adc_stDownOut.swIbPu, 10);
                            EXPECT_NEAR((double)adc_stDownOut.slSampIcPu * 1024 / 1024, adc_stDownOut.swIcPu, 10);
                        }
                    }
                    else
                    {
                        if (1024 / rdsonCoef < adc_stCof.uwCalibcoefMin)
                        {
                            if (currentMod * rdsonCoef * 100 >= ADC_IPHASE_CUR_MAX_AP)
                            {
                                EXPECT_NEAR((double)adc_stDownOut.slSampIaPu * 780 / 1024, adc_stDownOut.swIaPu, 50);
                                EXPECT_NEAR((double)adc_stDownOut.slSampIbPu * 780 / 1024, adc_stDownOut.swIbPu, 50);
                                EXPECT_NEAR((double)adc_stDownOut.slSampIcPu * 780 / 1024, adc_stDownOut.swIcPu, 50);
                            }
                            else
                            {
                                EXPECT_NEAR((double)adc_stDownOut.slSampIaPu * adc_stCof.uwCalibcoefMin / 1024, adc_stDownOut.swIaPu, 10);
                                EXPECT_NEAR((double)adc_stDownOut.slSampIbPu * adc_stCof.uwCalibcoefMin / 1024, adc_stDownOut.swIbPu, 10);
                                EXPECT_NEAR((double)adc_stDownOut.slSampIcPu * adc_stCof.uwCalibcoefMin / 1024, adc_stDownOut.swIcPu, 10);
                            }
                        }
                        else if (1024 / rdsonCoef > adc_stCof.uwCalibcoefMax)
                        {
                            EXPECT_NEAR((double)adc_stDownOut.slSampIaPu * adc_stCof.uwCalibcoefMax / 1024, adc_stDownOut.swIaPu, 50);
                            EXPECT_NEAR((double)adc_stDownOut.slSampIbPu * adc_stCof.uwCalibcoefMax / 1024, adc_stDownOut.swIbPu, 50);
                            EXPECT_NEAR((double)adc_stDownOut.slSampIcPu * adc_stCof.uwCalibcoefMax / 1024, adc_stDownOut.swIcPu, 50);
                        }
                        else
                        {
                            if (currentMod < 20)
                            {
                                EXPECT_NEAR(ia * 100 * 16384 / IBASE, adc_stDownOut.swIaPu, 0.5 * 100 * 16384 / IBASE);
                                EXPECT_NEAR(ib * 100 * 16384 / IBASE, adc_stDownOut.swIbPu, 0.5 * 100 * 16384 / IBASE);
                                EXPECT_NEAR(ic * 100 * 16384 / IBASE, adc_stDownOut.swIcPu, 0.5 * 100 * 16384 / IBASE);
                            }
                            else
                            {
                                EXPECT_NEAR(ia * 100 * 16384 / IBASE, adc_stDownOut.swIaPu, 1.5 * 100 * 16384 / IBASE); 
                                EXPECT_NEAR(ib * 100 * 16384 / IBASE, adc_stDownOut.swIbPu, 1.5 * 100 * 16384 / IBASE);
                                EXPECT_NEAR(ic * 100 * 16384 / IBASE, adc_stDownOut.swIcPu, 1.5 * 100 * 16384 / IBASE);
                            }
                        }
                    }
                }
            }

            testTimerIntFlg[TIMER0][TIMER_INT_FLAG_UP] = 0;
        }
    }
}

INSTANTIATE_TEST_SUITE_P(DiffCurMod, CurSampleCalibTest1,
                         ::testing::Combine(::testing::Values(5, 19, 39, 55), ::testing::Values(0.4, 0.8, 2.1, 6), ::testing::Values(1, 2, 10)));