#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 ConsVoltBrakeTest : public testing::Test
{
protected:
    McStatus      McStatus1;
    McPuBase      McPuBase1;

    static void SetUpTestSuite()
    {

    }
    virtual void SetUp() override
    {
        McPuBaseIn puBaseIn = {
            .Pairsb = MOTOR_PAIRS,
            .UbVt   = VBASE,
            .IbAp   = IBASE,
            .FbHz   = FBASE,
        };
        McPuBaseInit(&McPuBase1, &puBaseIn);
        McStatusInit(&McStatus1, &McPuBase1);

        CodeParaInit();
        cvb_voBrakeInit();
        cof_voSysInit();
    }
    virtual void TearDown() override
    {

    }
};

class ConsVoltBrakeTest1 : public ConsVoltBrakeTest, public testing::WithParamInterface<::std::tuple<int,int,int>>
{};

TEST_P(ConsVoltBrakeTest1, ConsVoltBrake)
{
    /* Coef Cal */
    cvb_stBrakeCoefIn.uwVdcCvbVt = cp_stControlPara.swCvbConstantVolBrakeV;
    cvb_stBrakeCoefIn.uwLowSpdRpm = cp_stControlPara.swCvbConstantSpdLowRpm;
    cvb_stBrakeCoefIn.swIqRefMaxAp = cp_stMotorPara.swIpeakMaxA;
    cvb_stBrakeCoefIn.swIdRefMaxAp = cp_stMotorPara.swIdMaxA;
    cvb_stBrakeCoefIn.swIdRefMinAp = cp_stMotorPara.swIdMinA;
    cvb_stBrakeCoefIn.uwVBaseVt = VBASE;
    cvb_stBrakeCoefIn.uwIBaseAp = IBASE;
    cvb_stBrakeCoefIn.uwFBaseHz = FBASE;
    cvb_stBrakeCoefIn.uwMotorPairs = cp_stMotorPara.swMotrPolePairs;
    cvb_voBrakeCoef(&cvb_stBrakeCoefIn, &cvb_stBrakeCoef);

    s16 udc = get<0>(GetParam());       ///< unit: 0.1V
    s16 spd = get<1>(GetParam());       ///< unit: rpm
    s16 iqrefIn = get<2>(GetParam());   ///< unit: 0.01A

    McStatus1.Pu.swUdc = ((u32)udc << 14)/ McPuBase1.uwUbVt;
    McStatus1.Pu.swElecOmega = ((s32)spd << 15)/ McPuBase1.uwVbRpm; ///< Q15
    McStatus1.Pu.swElecAngle = 200;   ///< Q15

    for (int i = 0; i < 100 ; i++)
    {
        /* Constant voltage brake */
        cvb_stBrakeIn.uwVdcLpfPu = McStatus1.Pu.swUdc;
        cvb_stBrakeIn.swIdRefPu = cvb_stBrakeOut.swIdRefPu;
        cvb_stBrakeIn.swIqRefPu = ((s32)iqrefIn << 14) / IBASE;         
        cvb_stBrakeIn.swSpdPu = McStatus1.Pu.swElecOmega;
        cvb_stBrakeIn.uwAngelPu = McStatus1.Pu.swElecAngle;
        cvb_stBrakeIn.uwSpdLpfAbsPu = ABS(McStatus1.Pu.swElecOmega);
        cvb_voBrake(&cvb_stBrakeIn,&cvb_stBrakeCoef,&cvb_stBrakeOut);

        double iqlim,iqrefOut,elecAngle,delta;
        /* Ajust Iqlim according to Vdc */
        if(McStatus1.Pu.swUdc > cvb_stBrakeCoef.uwVdcStartCvbPu)
        {
            iqlim = (double)(McStatus1.Pu.swUdc - cvb_stBrakeCoef.uwVdcCvbPu) * cvb_stBrakeCoef.swKcvb / 512;   ///< Q14 = Q14 + Q9 - Q9
        }
        else
        {
            iqlim = -cvb_stBrakeCoef.swIqLimMaxPu;
        }
        if(iqlim > 0)
        {
            iqlim = 0;
        }
        iqlim = -iqlim;
        if(iqlim >  cvb_stBrakeCoef.swIqLimMaxPu)
        {
            iqlim = cvb_stBrakeCoef.swIqLimMaxPu;
        }  
        if ((McStatus1.Pu.swUdc < cvb_stBrakeCoef.uwVdcStartCvbPu) && (cvb_stBrakeCoef.swKcvb > 16384) && (iqlim == 0)) // avoid overflow
        {
            iqlim = cvb_stBrakeCoef.swIqLimMaxPu;
        }

        /* Ajust Electrical Angle according to Vdc */
        if(McStatus1.Pu.swUdc  < cvb_stBrakeCoef.uwVdcCvbPu)
        {
            delta = 0;
        }
        else if(McStatus1.Pu.swUdc  < cvb_stBrakeCoef.uwVdcLagAngelPu)
        {
            delta = ((double)(McStatus1.Pu.swUdc  - cvb_stBrakeCoef.uwVdcCvbPu) * cvb_stBrakeCoef.uwKAnglePu) / 128;    ///< Q15
        }
        else 
        {
            delta = ((double)(cvb_stBrakeCoef.uwVdcLagAngelPu - cvb_stBrakeCoef.uwVdcCvbPu) *  cvb_stBrakeCoef.uwKAnglePu) / 128;
        }

        /* Output Iqref limit */
        if(McStatus1.Pu.swElecOmega > 0)
        {
            if(cvb_stBrakeIn.swIqRefPu < -iqlim)
            {
                iqrefOut = -iqlim;
            }
            else
            {
                iqrefOut = cvb_stBrakeIn.swIqRefPu;
            }
            elecAngle = McStatus1.Pu.swElecAngle - delta;
        }
        else
        {
            if(cvb_stBrakeIn.swIqRefPu > iqlim)
            {
                iqrefOut = iqlim;
            }
            else
            {
                iqrefOut = cvb_stBrakeIn.swIqRefPu;
            }
            elecAngle = McStatus1.Pu.swElecAngle + delta;
        }
        
        if(elecAngle < 0)
        {
            elecAngle=  elecAngle + 65536;  
        }   
        else 
        {
            elecAngle =  elecAngle;
        }
 
        /* Ajust Idref according to Vdc */
        double idrefOut;
        if((cvb_stBrakeIn.swIqRefPu < 0 && McStatus1.Pu.swElecOmega > 0) || (cvb_stBrakeIn.swIqRefPu > 0 && McStatus1.Pu.swElecOmega < 0))
        {
            if(ABS(McStatus1.Pu.swElecOmega) < cvb_stBrakeCoef.uwLowSpdPu)
            {
                idrefOut =  cvb_stBrakeIn.swIdRefPu + cvb_stBrakeCoef.swIdRcyPu;
            }
            else
            {
                idrefOut =  cvb_stBrakeIn.swIdRefPu - cvb_stBrakeCoef.swIdDrpPu;
            }   

        }
        else
        {
            idrefOut =   cvb_stBrakeIn.swIdRefPu;
        }
        
        /* Output Idref limit */
        if(idrefOut <  cvb_stBrakeCoef.swIdRefMinPu)
        {
            idrefOut =  cvb_stBrakeCoef.swIdRefMinPu;
        }
        else if(idrefOut >  cvb_stBrakeCoef.swIdRefMaxPu)
        {
            idrefOut =  cvb_stBrakeCoef.swIdRefMaxPu;
        }
        else 
        {
            idrefOut = idrefOut;
        }

        EXPECT_NEAR(iqrefOut,  cvb_stBrakeOut.swIqRefPu, 2);
        EXPECT_NEAR(idrefOut,  cvb_stBrakeOut.swIdRefPu, 2);
        EXPECT_NEAR(elecAngle, cvb_stBrakeOut.uwAngelPu, 2);
    }
}

INSTANTIATE_TEST_SUITE_P(DiffUdc, ConsVoltBrakeTest1,
                         ::testing::Combine(::testing::Values(CVB_CONSTANT_VOL_BRAKE_V - 20, CVB_CONSTANT_VOL_BRAKE_V + 10), ::testing::Values(-2000,0,2000), ::testing::Values(-200,0,200)));