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

class PwrLimTest : public testing::Test
{
protected:
    PmsmSimUtModelClass *sys1;
    McStatus             McStatus1;
    McPuBase             McPuBase1;

    virtual void SetUp() override
    {
        sys1 = new PmsmSimUtModelClass();
        sys1->initialize();
        MotorSimHelper::RestoreDefaultParam(sys1);
        sys1->PmsmSimUt_P.Params.Ts      = 1.0 / FTBC_HZ;
        sys1->PmsmSimUt_P.Params.SpdCtrl = 1;
        sys1->PmsmSimUt_P.Params.B       = 0;
        sys1->PmsmSimUt_P.Params.Jm      = M_JD * 1e-7;
        sys1->PmsmSimUt_P.Params.Pn      = M_POLE_PAIRS;
        sys1->PmsmSimUt_P.Params.R       = M_RS_OHM * 1e-5;
        sys1->PmsmSimUt_P.Params.Ld      = M_LD_NOLOAD_MH * 1e-8;
        sys1->PmsmSimUt_P.Params.Lq      = M_LQ_NOLOAD_MH * 1e-8;
        sys1->PmsmSimUt_P.Params.Flux    = M_FLUX_WB * 1e-6;

        sys1->PmsmSimUt_P.Params.SKp  = (sys1->PmsmSimUt_P.Params.Jm * 20 * 2 * 3.14 / sys1->PmsmSimUt_P.Params.Flux) / 30 * 3.14;
        sys1->PmsmSimUt_P.Params.SKi  = 20 * 2 * 3.14 * sys1->PmsmSimUt_P.Params.SKp;
        sys1->PmsmSimUt_P.Params.CKpd = (300 * 2 * 3.14) * sys1->PmsmSimUt_P.Params.Ld;
        sys1->PmsmSimUt_P.Params.CKid = (300 * 2 * 3.14) * sys1->PmsmSimUt_P.Params.R;
        sys1->PmsmSimUt_P.Params.CKpq = (300 * 2 * 3.14) * sys1->PmsmSimUt_P.Params.Lq;
        sys1->PmsmSimUt_P.Params.CKiq = (300 * 2 * 3.14) * sys1->PmsmSimUt_P.Params.R;

        sys1->PmsmSimUt_reset();

        McPuBaseIn puBaseIn = {
            .Pairsb = MOTOR_PAIRS,
            .UbVt   = VBASE,
            .IbAp   = IBASE,
            .FbHz   = FBASE,
        };
        McPuBaseInit(&McPuBase1, &puBaseIn);

        McStatusInit(&McStatus1, &McPuBase1);

        CodeParaInit();
        pwr_voPwrLimInit();
        asr_voSpdPIInit();
    }

    virtual void TearDown() override
    {
        sys1->terminate();
        delete sys1;

        pwr_voPwrLimInit();
        asr_voSpdPIInit();
    }
};


class PwrLimTest2 : public PwrLimTest, public testing::WithParamInterface<::std::tuple<int,int>>
{};

TEST_P(PwrLimTest2, PwrLim)
{
    /* Coef Cal */
    pwr_stPwrLimCofIn.swPwrLimW           = cp_stControlPara.swPwrLimitValWt;
    pwr_stPwrLimCofIn.uwPwrErrW           = cp_stControlPara.swPwrLimitErrWt;
    pwr_stPwrLimCofIn.swIqMaxAp           = cp_stMotorPara.swIpeakMaxA;
    pwr_stPwrLimCofIn.uwIBaseAp           = IBASE;
    pwr_stPwrLimCofIn.uwUbVt              = VBASE;
    pwr_stPwrLimCofIn.uwPwrLimPIKp        = cp_stControlPara.swPwrLimitKpPu;
    pwr_stPwrLimCofIn.uwPwrLimPIKi        = cp_stControlPara.swPwrLimitKiPu;
    pwr_stPwrLimCofIn.swPwrLimStartTempCe = cp_stControlPara.swAlmPwrLimitStartTempVal;
    pwr_stPwrLimCofIn.swAlarmTempCe       = cp_stControlPara.swAlmOverHeatCeVal;
    pwr_voPwrLimCof(&pwr_stPwrLimCofIn, &pwr_stPwrLimCof);

    mth_voLPFilterCoef((1000000 / cp_stControlPara.swPwrLimitLPFFre), FTBC_HZ, &scm_stMotoPwrInLpf.uwKx);

    asr_stSpdPICoefIn.uwUbVt    = VBASE;
    asr_stSpdPICoefIn.uwIbAp    = IBASE;
    asr_stSpdPICoefIn.uwFbHz    = FBASE;
    asr_stSpdPICoefIn.uwFTbsHz  = FTBS_HZ;
    asr_stSpdPICoefIn.uwPairs   = cp_stMotorPara.swMotrPolePairs;
    asr_stSpdPICoefIn.uwMtJm    = cp_stMotorPara.swJD;
    asr_stSpdPICoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb;
    asr_stSpdPICoefIn.uwMcoef   = cp_stControlPara.swAsrPIM;
    asr_stSpdPICoefIn.uwWvcHz   = cp_stControlPara.swAsrPIBandwidth;
    asr_stSpdPICoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate;
    asr_voSpdPICoef(&asr_stSpdPICoefIn, &asr_stSpdPICoef);

    /* Custom Spd Ctrl */
    sys1->PmsmSimUt_P.Params.CustomSpdCtrl = 1;
    int16_t spdRefRpm                      = 3000;

    for (int i = 0; i < 10 / sys1->PmsmSimUt_P.Params.Ts; i++)
    {
        /* Speed Ref */
        // spdRefRpm += 1;
        // if(spdRefRpm > 3000)
        // {
        //     spdRefRpm = 3000;
        // }
        int16_t spdRefPu = ((int32_t)spdRefRpm << 15) / McStatus1.Base->uwVbRpm;

        /* Motor Load Torque */
        // sys1->PmsmSimUt_U.CtrlIn.Tm += 0.001;
        // if(sys1->PmsmSimUt_U.CtrlIn.Tm > 2)
        // {
        //     sys1->PmsmSimUt_U.CtrlIn.Tm = 2;
        // }
        sys1->PmsmSimUt_U.CtrlIn.Tm = 1;  ///< 注意：加载需加到恒功率区以测试功率是否被限制,但是太大会导致仿真中转速负增加最终溢出,需要算一下

        /* Pu Value to Named Value */
        MotorSimHelper::SetModelICmdPu(sys1, 0, asr_stSpdPIOut.swIqRefPu, McStatus1.Base);

        /* Motor Control */
        sys1->step();

        /* Named Value to Pu Value */
        MotorSimHelper::TransitModelOutput(sys1, &McStatus1);

        /* Power Cal */
        double ualpha = 0.6667 * (sys1->PmsmSimUt_Y.Out.Uabc[0] - 0.5 * sys1->PmsmSimUt_Y.Out.Uabc[1] - 0.5 * sys1->PmsmSimUt_Y.Out.Uabc[2]);
        double ubeta  = 0.57735 * (sys1->PmsmSimUt_Y.Out.Uabc[1] - sys1->PmsmSimUt_Y.Out.Uabc[2]);
        double ud     = ualpha * cos(sys1->PmsmSimUt_Y.Out.Theta) + ubeta * sin(sys1->PmsmSimUt_Y.Out.Theta);
        double uq     = -ualpha * sin(sys1->PmsmSimUt_Y.Out.Theta) + ubeta * cos(sys1->PmsmSimUt_Y.Out.Theta);
        double udPu   = ud * 10 * 16384 / VBASE;
        double uqPu   = uq * 10 * 16384 / VBASE;

        // int16_t power = ((int32_t)McStatus1.Pu.swIalpha * McStatus1.Pu.swUalpha + (int32_t)McStatus1.Pu.swIbeta * McStatus1.Pu.swUbeta) >> 13; //Q15
        int16_t power = ((int32_t)udPu * McStatus1.Pu.swId + (int32_t)uqPu * McStatus1.Pu.swIq) >> 13; // Q15
        mth_voLPFilter(power, &scm_stMotoPwrInLpf);

        /* Power Limit Control */
        pwr_stPwrLimIn.swMotorPwrPu = scm_stMotoPwrInLpf.slY.sw.hi;
        pwr_stPwrLimIn.swPCBTemp    = get<0>(GetParam());
        pwr_stPwrLimIn.uwBatCap     = get<1>(GetParam());
        pwr_voPwrLimPI(&pwr_stPwrLimIn, &pwr_stPwrLimCof, &pwr_stPwrLimOut2); // Q14

        /* Speed Control */
        if (i % (FTBC_HZ / FTBS_HZ) == 0)
        {
            asr_stSpdPIIn.swSpdRefPu = spdRefPu;
            asr_stSpdPIIn.swSpdFdbPu = McStatus1.Pu.swElecOmega;
            asr_stSpdPIIn.swIqMaxPu  = ABS(pwr_stPwrLimOut2.swIqLimPu);
            asr_stSpdPIIn.swIqMinPu  = -ABS(pwr_stPwrLimOut2.swIqLimPu);
            asr_voSpdPI(&asr_stSpdPIIn, &asr_stSpdPICoef, &asr_stSpdPIOut);
        }

        UdpScope::Send(0, pwr_stPwrLimOut2.swPwrLimActualPu, power, pwr_stPwrLimOut2.swIqLimPu, asr_stSpdPIOut.swIqRefPu, McStatus1.Pu.swIq,
                       McStatus1.Pu.swElecOmega);
    }

    /* 判断单元测试是否通过 */
    /* 判断实际功率是否在限制功率以下 */
    EXPECT_LE(scm_stMotoPwrInLpf.slY.sw.hi, (pwr_stPwrLimOut2.swPwrLimActualPu + 2));

    /* 判断限制电流与设计值是否一致 */
    if(ABS(pwr_stPwrLimOut2.swPwrLimActualPu - scm_stMotoPwrInLpf.slY.sw.hi) > 10)
    {
        double Iqlim;
        if (pwr_stPwrLimIn.swPCBTemp < pwr_stPwrLimCof.swPwrLimStartTemp)
        {
            Iqlim = pwr_stPwrLimCof.swIqMaxPu;
        }
        else if(pwr_stPwrLimIn.swPCBTemp < pwr_stPwrLimCof.swPwrLimEndTemp)
        {
            Iqlim = pwr_stPwrLimCof.swIqMaxPu - ((int32_t)(pwr_stPwrLimIn.swPCBTemp - pwr_stPwrLimCof.swPwrLimStartTemp) * pwr_stPwrLimCof.swIqMaxPu * 83 / (pwr_stPwrLimCof.swPwrLimEndTemp - pwr_stPwrLimCof.swPwrLimStartTemp) >> 7);
        }
        else
        {
            Iqlim = (int32_t)pwr_stPwrLimCof.swIqMaxPu * 45 >> 7;
        }

        EXPECT_NEAR(Iqlim, pwr_stPwrLimOut2.swIqLimPu, 2);
    }
    else 
    {
        EXPECT_NEAR((pwr_stPwrLimCof.swIqMaxPu + pwr_stPwrLimOut2.swIqLimDetaSum), pwr_stPwrLimOut2.swIqLimPu, 2);
    }
}

INSTANTIATE_TEST_SUITE_P(DiffTemp, PwrLimTest2,::testing::Combine(::testing::Values(60, 95, 100),::testing::Values(100, 15, 10, 0)));