#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 PwrLimTest1 : public PwrLimTest, public testing::WithParamInterface<double>
{};

TEST_P(PwrLimTest1, BandCal)
{

}

INSTANTIATE_TEST_SUITE_P(DiffBand, PwrLimTest1, testing::Values(20, 50, 100));

class PwrLimTest2 : public PwrLimTest, public testing::WithParamInterface<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.uwPwrLimSTARTCe = cp_stControlPara.swAlmPwrLimitStartTempVal;
    pwr_stPwrLimCofIn.uwPwrLimENDCe = 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 = 2;

        /* 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 power1 = ((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.swIqrefPu = asr_stSpdPIOut.swIqRefPu;                   
        pwr_stPwrLimIn.PCBTemp = GetParam(); //adc_stUpOut.PCBTemp;
        pwr_voPwrLim2(&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.swMotorPwrLimitActualPu, power, pwr_stPwrLimOut2.swIqLimPu, asr_stSpdPIOut.swIqRefPu, spdRefPu,  McStatus1.Pu.swElecOmega);
    }

    EXPECT_NEAR(pwr_stPwrLimOut2.swMotorPwrLimitActualPu, scm_stMotoPwrInLpf.slY.sw.hi, 2);
}

INSTANTIATE_TEST_SUITE_P(DiffTemp, PwrLimTest2, ::testing::Values(20, 90, 150));