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

class TorqObsTest : 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 * 15 * 2 * 3.14 / sys1->PmsmSimUt_P.Params.Flux) / 30 * 3.14;
        sys1->PmsmSimUt_P.Params.SKi  = 15 * 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;

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

        McStatusInit(&McStatus1, &McPuBase1);

        torqobs_voInit();
    }

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

        torqobs_voInit();
    }
};

class TorqObsTest1 : public TorqObsTest, public testing::WithParamInterface<::std::tuple<double, double, double>>
{};

TEST_P(TorqObsTest1, TorqObs)
{
    double torqPuMax1 = 0, torqPuMax2 = 0;
    double mod = get<1>(GetParam());
    double freq = get<2>(GetParam());
    double band = 50;

    /* Coef Cal */
    torqobs_stCoefIn.uwUbVt = VBASE;
    torqobs_stCoefIn.uwIbAp = IBASE;
    torqobs_stCoefIn.uwFbHz = FBASE;
    torqobs_stCoefIn.uwFTbsHz = FTBS_HZ;
    torqobs_stCoefIn.uwPairs = M_POLE_PAIRS;
    torqobs_stCoefIn.uwMtJm = M_JD; 
    torqobs_stCoefIn.uwMtFlxWb = M_FLUX_WB;
    torqobs_stCoefIn.uwWtcHz = band; 
    torqobs_stCoefIn.uwRatioJm = 0; // ASR_SPD_INER_RATE;
    torqobs_voCoef(&torqobs_stCoefIn, &torqobs_stCoef);  

    /* Speed Ref */
    sys1->PmsmSimUt_P.Params.CustomSpdCtrl  = 0;
    sys1->PmsmSimUt_U.CtrlIn.WmRpm = get<0>(GetParam());
 
    for (int i = 0; i < 10 / sys1->PmsmSimUt_P.Params.Ts; i++)
    {
        /* Motor Load Torque */
        double phase = 2 * 3.14 * freq * i * sys1->PmsmSimUt_P.Params.Ts;
        sys1->PmsmSimUt_U.CtrlIn.Tm = 1 + mod * sin(phase);

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

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

        /* Torque Observer */
        if(i % (FTBC_HZ / FTBS_HZ) == 0)
        {
            torqobs_stCalIn.swIqMaxPu = 55 * 100 * 16384 / IBASE;
            torqobs_stCalIn.swIqMinPu = -55 * 100 * 16384 / IBASE;
            torqobs_stCalIn.swIqfbkPu = McStatus1.Pu.swIq;
            torqobs_stCalIn.swSpdPu = McStatus1.Pu.swElecOmega;
            torqobs_voCal(&torqobs_stCalIn, &torqobs_stCoef, &torqobs_stCalOut);
        }

        double torqPu = (double)sys1->PmsmSimUt_U.CtrlIn.Tm * 1000 * 16384 / McStatus1.Base->uwTqbNm;
        
        /* Get Torque Max */ 
        if(i > 1 / sys1->PmsmSimUt_P.Params.Ts)
        {
            if(torqPuMax1 < torqobs_stCalOut.swTorqObsPu)  
            {
                torqPuMax1 = torqobs_stCalOut.swTorqObsPu;
            }
            if(torqPuMax2 < torqPu)
            {
                torqPuMax2 = torqPu;
            }
        }

        //UdpScope::Send(0, torqobs_stCalOut.swSpdObsPu, McStatus1.Pu.swElecOmega, torqPu, torqobs_stCalOut.swTorqObsPu, torqobs_stCalOut.swIqLoadPu);
    }

    if(freq <= band)
    {
        EXPECT_TRUE((torqPuMax1 / torqPuMax2) >= 0.707 && (torqPuMax1 / torqPuMax2) <= 1.01);  ///< band处为-3dB(0.707), P控制无超调小于0dB(1)
    }

}

INSTANTIATE_TEST_SUITE_P(DiffTorq, TorqObsTest1, ::testing::Combine(::testing::Values(500, 5000), ::testing::Values(0, 0.5, 1), ::testing::Values(1, 16, 50)));