#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"


class TorqueSensorTest : public testing::Test
{
protected:
    static void SetUpTestSuite()
    {
        torsensor_voTorSensorInit();
    }
    virtual void SetUp() override
    {

    }
    virtual void TearDown() override
    {
        torsensor_voTorSensorInit();
        torsensor_stTorSensorCof.uwTorqueOffsetConfirmFlg = FALSE;
        /* Coef Reset */
        torsensor_stTorSensorCof.uwTorqueOffsetOrign = 0;
        torsensor_stTorSensorCof.uwTorqueOffsetNow1 = 0;
        torsensor_stTorSensorCof.uwTorqueOffsetNow2 = 0;
        torsensor_stTorSensorCof.uwTorqueOffsetNow3 = 0;
        torsensor_stTorSensorCof.uwTorqueOffsetNow4 = 0;
    }
};

class TorqueSensorTest1 : public TorqueSensorTest, public testing::WithParamInterface<double>
{};

TEST_P(TorqueSensorTest1, TorqCal)
{
    /* Coef Cal */
    torsensor_stTorSensorCof.uwBikeTorStep1ADC = 1310;
    torsensor_stTorSensorCof.uwBikeTorStep2ADC = 1806;
    torsensor_stTorSensorCof.uwBikeTorStep3ADC = 2411;
    torsensor_stTorSensorCof.uwBikeTorStep4ADC = 3009;
    torsensor_stTorSensorCof.uwBikeTorStep1RealNm = 25;
    torsensor_stTorSensorCof.uwBikeTorStep2RealNm = 50;
    torsensor_stTorSensorCof.uwBikeTorStep3RealNm = 75;
    torsensor_stTorSensorCof.uwBikeTorStep4RealNm = 100;
    hw_uwADC0[7] = 720;   // 上电零点
    Adcs[0].Results[HW_ADC_TORQ_CH] = hw_uwADC0[7];
    torsensor_voTorSensorCof();

    /* Torque Input */
    double torqVol = GetParam();
    hw_uwADC0[7] = (uint16_t) (torqVol * 4096 / 3.3);
    Adcs[0].Results[HW_ADC_TORQ_CH] = hw_uwADC0[7];

    /* Torque Pu Cal */
    torsensor_voTorADC();

    double reg2Pu1 = (double)(torsensor_stTorSensorCof.uwBikeTorStep1RealNm - 0) * 16777216 / 
                    (torsensor_stTorSensorCof.uwBikeTorStep1ADC - torsensor_stTorSensorCof.uwTorqueOffset) /TORQUEBASE; // Q24
    double reg2Pu2 = (double)(torsensor_stTorSensorCof.uwBikeTorStep2RealNm - torsensor_stTorSensorCof.uwBikeTorStep1RealNm) * 16777216 / 
                     (torsensor_stTorSensorCof.uwBikeTorStep2ADC - torsensor_stTorSensorCof.uwBikeTorStep1ADC) / TORQUEBASE;  // Q24
    double reg2Pu3 = (double)(torsensor_stTorSensorCof.uwBikeTorStep3RealNm - torsensor_stTorSensorCof.uwBikeTorStep2RealNm) * 16777216 / 
                     (torsensor_stTorSensorCof.uwBikeTorStep3ADC - torsensor_stTorSensorCof.uwBikeTorStep2ADC) / TORQUEBASE; // Q24    
    double reg2Pu4 = (double)(torsensor_stTorSensorCof.uwBikeTorStep4RealNm - torsensor_stTorSensorCof.uwBikeTorStep3RealNm) * 16777216 / 
                     (torsensor_stTorSensorCof.uwBikeTorStep4ADC - torsensor_stTorSensorCof.uwBikeTorStep3ADC) / TORQUEBASE; // Q24

    double torqPu = 0;
    if(hw_uwADC0[7] <= torsensor_stTorSensorCof.uwTorqueOffset)     
    {
        torqPu = 0;
    }    
    else if(hw_uwADC0[7] <= torsensor_stTorSensorCof.uwBikeTorStep1ADC)   
    {
        torqPu = (double)(hw_uwADC0[7] - torsensor_stTorSensorCof.uwTorqueOffset)* reg2Pu1 / 1024;
    }
    else if(hw_uwADC0[7] <= torsensor_stTorSensorCof.uwBikeTorStep2ADC)   
    {
        torqPu = torsensor_stTorSensorCof.uwBikeTorStep1NmPu + (double)(hw_uwADC0[7] - torsensor_stTorSensorCof.uwBikeTorStep1ADC) * reg2Pu2 / 1024;
    }
    else if(hw_uwADC0[7] <= torsensor_stTorSensorCof.uwBikeTorStep3ADC)   
    {
        torqPu = torsensor_stTorSensorCof.uwBikeTorStep2NmPu + (double)(hw_uwADC0[7] - torsensor_stTorSensorCof.uwBikeTorStep2ADC) * reg2Pu3 / 1024;  
    }
    else if(hw_uwADC0[7] <= torsensor_stTorSensorCof.uwBikeTorStep4ADC)   
    {
        torqPu = torsensor_stTorSensorCof.uwBikeTorStep3NmPu + (double)(hw_uwADC0[7] - torsensor_stTorSensorCof.uwBikeTorStep3ADC) * reg2Pu4 / 1024;
    }
    else
    {
        torqPu = torsensor_stTorSensorCof.uwBikeTorStep4NmPu;
    }

    EXPECT_NEAR(torsensor_stTorSensorOut.uwTorquePu , torqPu, 2);

}

INSTANTIATE_TEST_SUITE_P(DiffTorq, TorqueSensorTest1, ::testing::Values(0, 0.1, 0.8, 1.2, 1.7, 2.5, 1.9));

class TorqueSensorTest2 : public TorqueSensorTest, public testing::WithParamInterface<::std::tuple<double,double>>
{};
TEST_P(TorqueSensorTest2, OffsetUpdate)
{
    /* Coef Cal */
    torsensor_stTorSensorCof.uwBikeTorStep1ADC = 1200;
    torsensor_stTorSensorCof.uwBikeTorStep2ADC = 1900;
    torsensor_stTorSensorCof.uwBikeTorStep3ADC = 2600;
    torsensor_stTorSensorCof.uwBikeTorStep4ADC = 3100;
    torsensor_stTorSensorCof.uwBikeTorStep1RealNm = 25;
    torsensor_stTorSensorCof.uwBikeTorStep2RealNm = 50;
    torsensor_stTorSensorCof.uwBikeTorStep3RealNm = 75;
    torsensor_stTorSensorCof.uwBikeTorStep4RealNm = 100;
    hw_uwADC0[7] = 500;    // 上电零点
    Adcs[0].Results[HW_ADC_TORQ_CH] = hw_uwADC0[7];
    torsensor_voTorSensorCof();

    /* Torque Input */
    double minReg = get<0>(GetParam());
    double maxReg = get<1>(GetParam());
    if(minReg > maxReg)
    {
        minReg = maxReg;
    }
    if(maxReg > torsensor_stTorSensorCof.uwBikeTorStep1ADC)
    {
        maxReg = torsensor_stTorSensorCof.uwBikeTorStep1ADC;
    }

    int j = 0;
    for(int i = 0; i < 10000000 ; i++)
    {
        if(i % TORQUE_1S_1MSCNT == 0)
        {
            j++;
        }

        if(i < (TORQUE_1S_1MSCNT * j - TORQUE_1S_1MSCNT / 2))
        {
            hw_uwADC0[7] = (uint16_t)minReg;
        }
        else if(i < TORQUE_1S_1MSCNT * j)
        {
            hw_uwADC0[7] = (uint16_t)maxReg;
        }
        else 
        {
            // do nothing 
        }

        Adcs[0].Results[HW_ADC_TORQ_CH] = hw_uwADC0[7];

        /* Torque Pu Cal */
        torsensor_voTorADC();
        torsensor_voOffsetUpdate();

        //UdpScope::Send(0, torsensor_stTorSensorCof.uwTorqueOffset); 
        //UdpScope::Send(0, torsensor_stTorSensorCof.uwTorqueOffsetConfirmFlg); 
        
    }

    if(maxReg - minReg <= 40)
    {
        EXPECT_NEAR(torsensor_stTorSensorCof.uwTorqueOffset, (maxReg + minReg)/2, 2);
    }
    else
    {
        EXPECT_NEAR(torsensor_stTorSensorCof.uwTorqueOffset, 500, 2);
    }
}

INSTANTIATE_TEST_SUITE_P(DiffTorqOffset, TorqueSensorTest2,
                         ::testing::Combine(::testing::Values(480,495,500), ::testing::Values(510, 530, 600)));

                         
class TorquePowerOnTest: public testing::Test
{
protected:
    static void SetUpTestSuite()
    {
        torsensor_voTorSensorInit();
    }
    virtual void SetUp() override
    {

    }
    virtual void TearDown() override
    {
        torsensor_voTorSensorInit();
        torsensor_stTorSensorCof.uwTorqueOffsetConfirmFlg = FALSE;
    }
};

class TorquePowerOnTest1 : public TorquePowerOnTest, public testing::WithParamInterface<double>
{};

int i = 0;
double offsetLast[4] = {0,0,0,0};
TEST_P(TorquePowerOnTest1, OffsetUpdate)
{

    /* Coef Cal */
    torsensor_stTorSensorCof.uwBikeTorStep1ADC = 1310;
    torsensor_stTorSensorCof.uwBikeTorStep2ADC = 1806;
    torsensor_stTorSensorCof.uwBikeTorStep3ADC = 2411;
    torsensor_stTorSensorCof.uwBikeTorStep4ADC = 3009;
    torsensor_stTorSensorCof.uwBikeTorStep1RealNm = 25;
    torsensor_stTorSensorCof.uwBikeTorStep2RealNm = 50;
    torsensor_stTorSensorCof.uwBikeTorStep3RealNm = 75;
    torsensor_stTorSensorCof.uwBikeTorStep4RealNm = 100;
    hw_uwADC0[7] = (uint16_t)GetParam();   // 上电零点
    Adcs[0].Results[HW_ADC_TORQ_CH] = hw_uwADC0[7];
    torsensor_voTorSensorCof();

    i++;
    if(i > 1)
    {
        double offsetAvg = 0;
        if(offsetLast[0] == 0)
        {
            offsetAvg = torsensor_stTorSensorCof.uwTorqueOffsetOrign;
        }  
        else 
        {
            offsetAvg = (offsetLast[0] + offsetLast[1] + offsetLast[2] + offsetLast[3]) / 4;
        }

        if(abs((double)hw_uwADC0[7] - offsetAvg) > 200)
        {
            EXPECT_NEAR(torsensor_stTorSensorCof.uwTorqueOffset, offsetAvg, 2);
        }
        else 
        {
            offsetLast[0] = offsetLast[1];
            offsetLast[1] = offsetLast[2];
            offsetLast[2] = offsetLast[3];
            offsetLast[3] = torsensor_stTorSensorCof.uwTorqueOffset;

            EXPECT_NEAR(torsensor_stTorSensorCof.uwTorqueOffset, hw_uwADC0[7], 2);
        }
    }
}

INSTANTIATE_TEST_SUITE_P(DiffTorqOffset, TorquePowerOnTest1, ::testing::Values(500, 536, 600, 810, 630, 680, 460, 1000, 350, 2000, 520));