#include "ACMSim.h"
/* 逆变器非线性 */

/* End of B-zier part  */

/* 拟合法 */
#ifdef _XCUBE1
// b-phase @80 V Mini6PhaseIPMInverter
REAL sig_a2 = 7.705938744542915;
REAL sig_a3 = 67.0107635483658;

// 180V (old)
// REAL sig_a2 = 16.0575341;  // yuefei tuning gives a2' = 9.2*2
// REAL sig_a3 = 17.59278688; // yuefei tuning gives a3' = a3*5
#else
// 80 V
// REAL sig_a2 = 6.67159129;
// REAL sig_a3 = 8.42010418;

// 100 V from SlessInv paper
REAL sig_a2 = 6.7;
REAL sig_a3 = 5.6;

// 150 V
// REAL sig_a2 = 13.25723639;
// REAL sig_a3 = 5.6420585;

// 180 V
// REAL sig_a2 = 15.43046115;
// REAL sig_a3 = 5.04010863;
#endif
REAL sigmoid(REAL x)
{
    // beta
    // REAL a1 = 0.0; // *1.28430164
    // REAL a2 = 8.99347107;
    // REAL a3 = 5.37783655;

    // -------------------------------

#ifdef _XCUBE1
    /* Si Mini OW IPM Inverter*/

    // b-phase @180 V Mini6PhaseIPMInverter
    // REAL a1 = 0.0; //1.34971502
    // REAL a2 = 16.0575341;  // yuefei tuning gives a2' = 9.2*2
    // REAL a3 = 17.59278688; // yuefei tuning gives a3' = a3*5

    // b-phase @80 V Mini6PhaseIPMInverter
    // REAL a1 = 0.0; //2.10871359 ;
    // REAL a2 = 7.36725304;
    // REAL a3 = 62.98712805;
    REAL a1 = 0.0; //(1.705,
    REAL a2 = sig_a2;
    REAL a3 = sig_a3;

#else
    /* SiC Cree 3 Phase Inverter*/

    // b-phase @20 V
    // REAL a1 = 0.0; //1.25991178;
    // REAL a2 = 2.30505904;
    // REAL a3 = 12.93721814;

    // b-phase @80 V
    // REAL a1 = 0.0; //1.28430164;
    // REAL a2 = 7.78857441;
    // REAL a3 = 6.20979077;
    REAL a1 = 0.0; // 1.6997103;
    REAL a2 = sig_a2;
    6.67159129;
    REAL a3 = sig_a3;
    8.42010418;

    // b-phase @180 V
    /*最早的拟合结果*/
    // REAL a1 = 0.0; //1.50469916;
    // REAL a2 = 13.48467604;
    // REAL a3 = 5.22150403;
    /*错误！多乘了一次beta-axis转phase系数！*/
    // REAL a1 = 0.0; //1.83573529
    // REAL a2 = sig_a2; 13.36317172;
    // REAL a3 = sig_a3; 5.81981571;
    /*纠正！100个点，平均分配*/
    // REAL a1 = 0.0; //1.8357354;
    // REAL a2 = sig_a2; 15.43046115;
    // REAL a3 = sig_a3; 5.04010863;

#endif

    return a1 * x + a2 / (1.0 + exp(-a3 * x)) - a2 * 0.5;
}
REAL sigmoid_online_v2(REAL x, REAL a2, REAL a3)
{
    return a2 / (1.0 + exp(-a3 * x)) - a2 * 0.5;
}
REAL sigmoid_online(REAL x, REAL Vsat, REAL a3)
{

    REAL a2 = Vsat * 2;
    // REAL a3 = 5.22150403;
    return a2 / (1.0 + exp(-a3 * x)) - a2 * 0.5;
}

/* 真查表法 // C代码参考ui_curve_v4.py */
#define LUT_N_LC 70
#define LUT_N_HC 29
REAL lut_lc_voltage[70] = {0, -0.0105529, 0.31933, 0.364001, 0.415814, 0.489953, 0.602715, 0.769718, 0.971424, 1.21079, 1.50055, 1.83306, 2.16318, 2.54303, 2.92186, 3.24129, 3.51575, 3.75058, 3.97849, 4.16454, 4.33493, 4.49719, 4.64278, 4.76509, 4.88146, 4.99055, 5.06347, 5.16252, 5.24808, 5.30369, 5.36092, 5.44246, 5.50212, 5.5786, 5.63384, 5.69022, 5.74442, 5.79613, 5.8491, 5.89762, 5.93325, 5.98141, 6.01726, 6.06201, 6.09346, 6.13419, 6.16634, 6.19528, 6.2233, 6.25819, 6.29004, 6.31378, 6.34112, 6.3669, 6.38991, 6.4147, 6.4381, 6.46156, 6.48171, 6.49962, 6.51565, 6.53689, 6.5566, 6.57761, 6.59515, 6.60624, 6.62549, 6.64589, 6.65606, 6.67132};
REAL lut_hc_voltage[29] = {6.69023, 6.80461, 6.89879, 6.96976, 7.02613, 7.08644, 7.12535, 7.17312, 7.20858, 7.2444, 7.27558, 7.30321, 7.32961, 7.35726, 7.38272, 7.39944, 7.42055, 7.43142, 7.4416, 7.43598, 7.44959, 7.45352, 7.45434, 7.45356, 7.45172, 7.45522, 7.45602, 7.44348, 7.43926};
#define LUT_STEPSIZE_BIG 0.11641244037931034
#define LUT_STEPSIZE_SMALL 0.01237159786376811
#define LUT_STEPSIZE_BIG_INVERSE 8.59014721057018
#define LUT_STEPSIZE_SMALL_INVERSE 80.83030268294078
#define LUT_I_TURNING_LC 0.8660118504637677
#define LUT_I_TURNING_HC 4.241972621463768
#define V_PLATEAU 7.43925517763064

REAL lookup_compensation_voltage_indexed(REAL current_value)
{
    REAL abs_current_value = fabs(current_value);

    if (abs_current_value < LUT_I_TURNING_LC)
    {
        REAL float_index = abs_current_value * LUT_STEPSIZE_SMALL_INVERSE;
        int index = (int)float_index;
        REAL slope;
        if (index + 1 >= LUT_N_LC)
            slope = (lut_hc_voltage[0] - lut_lc_voltage[index]) * LUT_STEPSIZE_SMALL_INVERSE;
        else
            slope = (lut_lc_voltage[index + 1] - lut_lc_voltage[index]) * LUT_STEPSIZE_SMALL_INVERSE;
        return sign(current_value) * (lut_lc_voltage[index] + slope * (abs_current_value - index * LUT_STEPSIZE_SMALL));
    }
    else
    {
        REAL float_index = (abs_current_value - LUT_I_TURNING_LC) * LUT_STEPSIZE_BIG_INVERSE;
        int index = (int)float_index; // THIS IS A RELATIVE INDEX!
        REAL slope;
        if (index + 1 >= LUT_N_HC)
            return sign(current_value) * V_PLATEAU;
        else
            slope = (lut_hc_voltage[index + 1] - lut_hc_voltage[index]) * LUT_STEPSIZE_BIG_INVERSE;
        return sign(current_value) * (lut_hc_voltage[index] + slope * (abs_current_value - LUT_I_TURNING_LC - index * LUT_STEPSIZE_BIG));
    }
}
void get_distorted_voltage_via_LUT_indexed(REAL ial, REAL ibe, REAL *ualbe_dist)
{

    /* 查表法 */
    if (TRUE)
    {
        REAL ia, ib, ic;
        ia = 1 * (ial);
        ib = 1 * (-0.5 * ial - SIN_DASH_2PI_SLASH_3 * ibe);
        ic = 1 * (-0.5 * ial - SIN_2PI_SLASH_3 * ibe);

        REAL dist_ua = lookup_compensation_voltage_indexed(ia);
        REAL dist_ub = lookup_compensation_voltage_indexed(ib);
        REAL dist_uc = lookup_compensation_voltage_indexed(ic);

        G.ia = ia;
        G.dist_ua = dist_ua;

        // Clarke transformation（三分之二，0.5倍根号三）
        ualbe_dist[0] = 0.66666667 * (dist_ua - 0.5 * dist_ub - 0.5 * dist_uc);
        ualbe_dist[1] = 0.66666667 * 0.8660254 * (dist_ub - dist_uc); // 0.5773502695534
    }
    else
    {
        /* AB2U_AI 这宏假设了零序分量为零，即ia+ib+ic=0，但是电压并不一定满足吧，所以还是得用上面的？ */
        REAL ia, ib;            //,ic;
        ia = AB2U_AI(ial, ibe); // ia = 1 * (       ial                              );
        ib = AB2V_AI(ial, ibe); // ib = 1 * (-0.5 * ial - SIN_DASH_2PI_SLASH_3 * ibe );

        REAL dist_ua = lookup_compensation_voltage_indexed(ia);
        REAL dist_ub = lookup_compensation_voltage_indexed(ib);

        ualbe_dist[0] = UV2A_AI(dist_ua, dist_ub); // 0.66666667 * (dist_ua - 0.5*dist_ub - 0.5*dist_uc);
        ualbe_dist[1] = UV2B_AI(dist_ua, dist_ub); // 0.66666667 * 0.8660254 * ( dist_ub -     dist_uc);
    }
}

/* 伪查表法（需要循环比大小，运算量不固定） */
REAL lookup_phase_current(REAL current, REAL *lut_voltage, REAL *lut_current, int length_of_lut)
{
    /* assume lut_voltage[0] is negative and lut_voltage[-1] is positive */
    int j;
    if (current < lut_current[0])
    {
        return lut_voltage[0];
    }
    else if (current > lut_current[length_of_lut - 1])
    {
        return lut_voltage[length_of_lut - 1];
    }
    else
    {
        for (j = 0; j < length_of_lut - 1; ++j)
        {
            if (current > lut_current[j] && current < lut_current[j + 1])
            {
                REAL slope = (lut_voltage[j + 1] - lut_voltage[j]) / (lut_current[j + 1] - lut_current[j]);
                return (current - lut_current[j]) * slope + lut_voltage[j];
                // return 0.5*(lut_voltage[j]+lut_voltage[j+1]); // this is just wrong! stupid!
            }
        }
    }
#if PC_SIMULATION
    printf("DEBUG inverter.c. %g, %d, %d\n", current, j, length_of_lut);
#endif
    return 0.0;
}
REAL sigmoid_a3_tune = 1.0;
void get_distorted_voltage_via_CurveFitting(REAL ual, REAL ube, REAL ial, REAL ibe, REAL *ualbe_dist)
{
    // The data are measured in stator resistance identification with amplitude invariant d-axis current and d-axis voltage.

    /* 拟合法 */
    REAL ia, ib, ic;
    ia = 1 * (ial);
    ib = 1 * (-0.5 * ial - SIN_DASH_2PI_SLASH_3 * ibe);
    ic = 1 * (-0.5 * ial - SIN_2PI_SLASH_3 * ibe);

    // offline sigmoid
    REAL dist_ua = sigmoid(ia);
    REAL dist_ub = sigmoid(ib);
    REAL dist_uc = sigmoid(ic);

    // online tunable sigmoid (experimental)
    // REAL dist_ua = sigmoid_online(ia, INV.Vsat, 17.59278688 * sigmoid_a3_tune); //INV.theta_trapezoidal);
    // REAL dist_ub = sigmoid_online(ib, INV.Vsat, 17.59278688 * sigmoid_a3_tune); //INV.theta_trapezoidal);
    // REAL dist_uc = sigmoid_online(ic, INV.Vsat, 17.59278688 * sigmoid_a3_tune); //INV.theta_trapezoidal);

    // Clarke transformation（三分之二，0.5倍根号三）
    ualbe_dist[0] = 0.66666667 * (dist_ua - 0.5 * dist_ub - 0.5 * dist_uc);
    ualbe_dist[1] = 0.66666667 * 0.8660254 * (dist_ub - dist_uc); // 0.5773502695534

    // for plot
    INV.ia = ia;
    INV.ib = ib;
    INV.ic = ic;
    INV.dist_ua = dist_ua;
    INV.dist_ub = dist_ub;
    INV.dist_uc = dist_uc;
}
void get_distorted_voltage_via_LUT(REAL ual, REAL ube, REAL ial, REAL ibe, REAL *ualbe_dist, REAL *lut_voltage, REAL *lut_current, int length_of_lut)
{
    // The data are measured in stator resistance identification with amplitude invariant d-axis current and d-axis voltage.

    /* 查表法 */
    if (TRUE)
    {
        REAL ia, ib, ic;
        ia = 1 * (ial);
        ib = 1 * (-0.5 * ial - SIN_DASH_2PI_SLASH_3 * ibe);
        ic = 1 * (-0.5 * ial - SIN_2PI_SLASH_3 * ibe);

        REAL dist_ua = lookup_phase_current(ia, lut_voltage, lut_current, length_of_lut);
        REAL dist_ub = lookup_phase_current(ib, lut_voltage, lut_current, length_of_lut);
        REAL dist_uc = lookup_phase_current(ic, lut_voltage, lut_current, length_of_lut);

        // Clarke transformation（三分之二，0.5倍根号三）
        ualbe_dist[0] = 0.66666667 * (dist_ua - 0.5 * dist_ub - 0.5 * dist_uc);
        ualbe_dist[1] = 0.66666667 * 0.8660254 * (dist_ub - dist_uc); // 0.5773502695534
    }
    else
    {
        /* AB2U_AI 这宏假设了零序分量为零，即ia+ib+ic=0，但是电压并不一定满足吧，所以还是得用上面的？ */
        REAL ia, ib;            //,ic;
        ia = AB2U_AI(ial, ibe); // ia = 1 * (       ial                              );
        ib = AB2V_AI(ial, ibe); // ib = 1 * (-0.5 * ial - SIN_DASH_2PI_SLASH_3 * ibe );
        // ic = AB2W_AI(ial, ibe); // ic = 1 * (-0.5 * ial - SIN_2PI_SLASH_3      * ibe );

        REAL dist_ua = lookup_phase_current(ia, lut_voltage, lut_current, length_of_lut);
        REAL dist_ub = lookup_phase_current(ib, lut_voltage, lut_current, length_of_lut);
        // REAL dist_uc = lookup_phase_current(ic, lut_voltage, lut_current, length_of_lut);

        // Clarke transformation（三分之二，0.5倍根号三）
        // ualbe_dist[0] = 0.66666667 * (dist_ua - 0.5*dist_ub - 0.5*dist_uc);
        // ualbe_dist[1] = 0.66666667 * 0.8660254 * ( dist_ub -     dist_uc); // 0.5773502695534

        ualbe_dist[0] = UV2A_AI(dist_ua, dist_ub); // 0.66666667 * (dist_ua - 0.5*dist_ub - 0.5*dist_uc);
        ualbe_dist[1] = UV2B_AI(dist_ua, dist_ub); // 0.66666667 * 0.8660254 * ( dist_ub -     dist_uc);
    }
}

/* Chen 2021 Linear Approximation of U-I Curve */
REAL trapezoidal_voltage_by_phase_current(REAL current, REAL V_plateau, REAL I_plateau, REAL oneOnver_I_plateau)
{
    REAL abs_current = fabs(current);
    if (abs_current < I_plateau)
    {
        return current * V_plateau * oneOnver_I_plateau;
    }
    else
    {
        return sign(current) * V_plateau;
    }
}

/* ParkSul2012 梯形波 */
void inverterNonlinearity_Initialization()
{
    INV.gamma_theta_trapezoidal = GAIN_THETA_TRAPEZOIDAL;
#ifdef _XCUBE1
    INV.Vsat = 16.0575341 / 2; // 6.67054; // 180 V SiC
#else
    // INV.Vsat = sig_a2*0.5; //6.74233802;
    INV.Vsat = 7.84; // 150 V
#endif

    INV.gain_Vsat = 0 * 10;

    INV.thetaA = 0;
    INV.cos_thetaA = 1;
    INV.sin_thetaA = 0;

    // --
    INV.u_comp[0] = 0;
    INV.u_comp[1] = 0;
    INV.u_comp[2] = 0;
    INV.ual_comp = 0;
    INV.ube_comp = 0;
    INV.uDcomp_atA = 0;
    INV.uQcomp_atA = 0;

    INV.iD_atA = 0;
    INV.iQ_atA = 0;
    INV.I5_plus_I7 = 0;
    INV.I5_plus_I7_LPF = 0.0;
    INV.theta_trapezoidal = 11.0 * M_PI_OVER_180; // in rad

#ifdef _XCUBE1
    INV.I_plateau_Max = 2.0;
    INV.I_plateau_Min = 0.2;
#else
    INV.I_plateau_Max = 1.0;
    INV.I_plateau_Min = 0.1;
#endif
    INV.I_plateau = 0.7;
    INV.V_plateau = 1.5 * sig_a2 * 0.5;
    INV.gamma_I_plateau = 10.0;
    INV.gamma_V_plateau = 0.0; // this is updated by the estimated disturbance to the sinusoidal flux model.

#if PC_SIMULATION
    INV.sig_a2 = 1.0 * sig_a2;
    INV.sig_a3 = 1.5 * sig_a3; // = shape parameter
#else
    INV.sig_a2 = sig_a2; // = Plateau * 2
    INV.sig_a3 = sig_a3; // = shape parameter
#endif

    INV.w6 = 1;
    INV.w12 = 0;
    INV.w18 = 0;
    INV.gamma_a2 = 400;
    INV.gamma_a3 = 125;
}
#define trapezoidal_voltage_by_current_vector_angle u_comp_per_phase
REAL u_comp_per_phase(REAL Vsat, REAL thetaA, REAL theta_trapezoidal, REAL oneOver_theta_trapezoidal)
{
    REAL compensation;

    // if(thetaA>0){
    //         compensation = Vsat;
    // }else{ // thetaA < 0
    //         compensation = -Vsat;
    // }

    if (thetaA > 0)
    {
        if (thetaA < theta_trapezoidal)
        {
            compensation = thetaA * Vsat * oneOver_theta_trapezoidal;
        }
        else if (thetaA > M_PI - theta_trapezoidal)
        {
            compensation = (M_PI - thetaA) * Vsat * oneOver_theta_trapezoidal;
        }
        else
        {
            compensation = Vsat;
        }
    }
    else
    { // thetaA < 0
        if (-thetaA < theta_trapezoidal)
        {
            compensation = -thetaA * -Vsat * oneOver_theta_trapezoidal;
        }
        else if (-thetaA > M_PI - theta_trapezoidal)
        {
            compensation = (M_PI + thetaA) * -Vsat * oneOver_theta_trapezoidal;
        }
        else
        {
            compensation = -Vsat;
        }
    }

    return compensation;
}
REAL lpf1_inverter(REAL x, REAL y_tminus1)
{
    // #define LPF1_RC 0.6 // 0.7, 0.8, 1.2, 3太大滤得过分了 /* 观察I5+I7_LPF的动态情况进行确定 */
    // #define ALPHA_RC (TS/(LPF1_RC + TS)) // TODO：优化
    // return y_tminus1 + ALPHA_RC * (x - y_tminus1); // 0.00020828993959591752
    return y_tminus1 + 0.00020828993959591752 * (x - y_tminus1);
}
REAL shift2pi(REAL thetaA)
{
    if (thetaA > M_PI)
    {
        return thetaA - 2 * M_PI;
    }
    else if (thetaA < -M_PI)
    {
        return thetaA + 2 * M_PI;
    }
    else
    {
        return thetaA;
    }
}
// REAL watch_theta_trapezoidal = 0.0;
void Modified_ParkSul_Compensation(void)
{

    /* Park12/14
     * */
    // Phase A current's fundamental component transformation
    INV.thetaA = -M_PI * 1.5 + (*CTRL).I->theta_d_elec + atan2((*CTRL).I->idq_cmd[1], (*CTRL).I->idq_cmd[0]); /* Q: why -pi*(1.5)? */ /* ParkSul2014 suggests to use PLL to extract thetaA from current command */
    INV.thetaA = shift2pi(INV.thetaA); /* Q: how to handle it when INV.thetaA jumps between pi and -pi? */                            // 这句话绝对不能省去，否则C相的梯形波会出错。

    INV.cos_thetaA = cosf(INV.thetaA);
    INV.sin_thetaA = sinf(INV.thetaA);
    INV.iD_atA = AB2M(IS_C(0), IS_C(1), INV.cos_thetaA, INV.sin_thetaA);
    INV.iQ_atA = AB2T(IS_C(0), IS_C(1), INV.cos_thetaA, INV.sin_thetaA);

    INV.I5_plus_I7 = INV.iD_atA * sinf(6 * INV.thetaA);                     /* Q: Why sinf? Why not cosf? 和上面的-1.5*pi有关系吗？ */
    INV.I5_plus_I7_LPF = lpf1_inverter(INV.I5_plus_I7, INV.I5_plus_I7_LPF); /* lpf1 for inverter */

    INV.I11_plus_I13 = INV.iD_atA * sinf(12 * INV.thetaA);
    INV.I11_plus_I13_LPF = lpf1_inverter(INV.I11_plus_I13, INV.I11_plus_I13_LPF);

    INV.I17_plus_I19 = INV.iD_atA * sinf(18 * INV.thetaA);
    INV.I17_plus_I19_LPF = lpf1_inverter(INV.I17_plus_I19, INV.I17_plus_I19_LPF);

    // The adjusting of theta_t via 6th harmonic magnitude
    INV.theta_trapezoidal += CL_TS * INV.gamma_theta_trapezoidal // *fabs((*CTRL).I->cmd_speed_rpm)
                             * (INV.I5_plus_I7_LPF + 0 * INV.I11_plus_I13_LPF + 0 * INV.I17_plus_I19_LPF);
    // 两种方法选其一：第一种可用于sensorless系统。
    if (FALSE)
    {
        /* 利用theta_t的饱和特性辨识Vsat（次优解） */
    }
    else
    {
        // 由于实际逆变器有一个调制比低引入5、7次谐波的问题，所以最佳theta_t不在11度附近了。
        if (INV.theta_trapezoidal >= 25 * M_PI_OVER_180)
        {                                               // 17
            INV.theta_trapezoidal = 25 * M_PI_OVER_180; // 17
        }
        if (INV.theta_trapezoidal <= 0.223 * M_PI_OVER_180)
        {
            INV.theta_trapezoidal = 0.223 * M_PI_OVER_180;
        }
    }

#if PC_SIMULATION == TRUE
    // 用偏小的Vsat，观察theta_t的收敛是否直达下界？
    // if((*CTRL).timebase>23){
    //     INV.Vsat = 9.0/6.0 * 16.0575341/2;
    // }else if((*CTRL).timebase>20){
    //     INV.Vsat = 7.0/6.0 * 16.0575341/2;
    // }else if((*CTRL).timebase>15){
    //     INV.Vsat = 6.0/6.0 * 16.0575341/2;
    // }else if((*CTRL).timebase>10){
    //     INV.Vsat = 5.0/6.0 * 16.0575341/2;
    // }else if((*CTRL).timebase>5){
    //     INV.Vsat = 4.0/6.0 * 16.0575341/2;
    // }

    // /* Adaptive Vsat based on position error */
    // INV.Vsat += CL_TS * INV.gain_Vsat * sinf(ENC.theta_d_elec - ELECTRICAL_POSITION_FEEDBACK) * sign(ENC.omg_elec);
    // if (INV.Vsat>15){
    //     INV.Vsat = 15;
    // }else if(INV.Vsat<0){
    //     INV.Vsat = 0;
    // }
#else
    /* Adaptive Vsat based on position error */
    /* TO use this, you muast have a large enough stator current */
    /* TO use this, you muast have a large enough stator current */
    /* TO use this, you muast have a large enough stator current */
    INV.Vsat += CL_TS * INV.gain_Vsat * sinf(ENC.theta_d_elec - ELECTRICAL_POSITION_FEEDBACK) * sign(ENC.omg_elec);
    if (INV.Vsat > 15)
    {
        INV.Vsat = 15;
    }
    else if (INV.Vsat < 0)
    {
        INV.Vsat = 0;
    }
#endif

#if INVERTER_NONLINEARITY == 3 // [ModelLUT]
    // INV.Vsat = 6.67054;
#elif INVERTER_NONLINEARITY == 2 // [ModelSigmoid]
    // INV.Vsat = sigmoid(100)*1.0;
    // INV.Vsat = sigmoid(100)*0.95;
    // INV.Vsat = sigmoid(100)*1.05;
#elif INVERTER_NONLINEARITY == 1 // [ModelSul96]
    REAL TM = _Toff - _Ton - _Tdead + _Tcomp; // Sul1996
    REAL Udist = (_Udc * TM * CL_TS_INVERSE - _Vce0 - _Vd0) / 6.0;
    INV.Vsat = 3 * fabs(Udist); // 4 = 2*sign(ia) - sign(ib) - sign(ic) when ia is positive and ib/ic is negative
                                // but Vsat is phase voltage distortion maximum, so we need to add a zero sequence voltage of Udist*(sign(ia) + sign(ib) + sign(ic)),
                                // so, it is 3 * |Udist| as Vsat.
    static int bool_printed = FALSE;
    if (bool_printed == FALSE)
    {
        printf("\tVsat = %g V; Udist = %g.\n", INV.Vsat, Udist);
        bool_printed = TRUE;
    }
#endif

    REAL oneOver_theta_trapezoidal = 1.0 / INV.theta_trapezoidal;
    // watch_theta_trapezoidal = INV.theta_trapezoidal / M_PI_OVER_180;
    INV.u_comp[0] = u_comp_per_phase(INV.Vsat, INV.thetaA, INV.theta_trapezoidal, oneOver_theta_trapezoidal);
    INV.u_comp[1] = u_comp_per_phase(INV.Vsat, shift2pi(INV.thetaA - TWO_PI_OVER_3), INV.theta_trapezoidal, oneOver_theta_trapezoidal);
    INV.u_comp[2] = u_comp_per_phase(INV.Vsat, shift2pi(INV.thetaA - 2 * TWO_PI_OVER_3), INV.theta_trapezoidal, oneOver_theta_trapezoidal);

    // REAL ia,ib,ic;
    // ia = 1 * (       IS_C(0)                                  );
    // ib = 1 * (-0.5 * IS_C(0) - SIN_DASH_2PI_SLASH_3 * IS_C(1) );
    // ic = 1 * (-0.5 * IS_C(0) - SIN_2PI_SLASH_3      * IS_C(1) );
    // #define MISMATCH_A3 1.1
    // INV.u_comp[0] = sigmoid_online(ia, INV.Vsat, 5.22150403 * MISMATCH_A3); //INV.theta_trapezoidal);
    // INV.u_comp[1] = sigmoid_online(ib, INV.Vsat, 5.22150403 * MISMATCH_A3); //INV.theta_trapezoidal);
    // INV.u_comp[2] = sigmoid_online(ic, INV.Vsat, 5.22150403 * MISMATCH_A3); //INV.theta_trapezoidal);

    /* 相补偿电压Clarke为静止正交坐标系电压。 */
    // 改成恒幅值变换
    INV.ual_comp = 0.66666666667 * (INV.u_comp[0] - 0.5 * INV.u_comp[1] - 0.5 * INV.u_comp[2]);
    INV.ube_comp = 0.66666666667 * 0.86602540378 * (INV.u_comp[1] - INV.u_comp[2]);
    // INV.ual_comp = SQRT_2_SLASH_3      * (INV.u_comp[0] - 0.5*INV.u_comp[1] - 0.5*INV.u_comp[2]); // sqrt(2/3.)
    // INV.ube_comp = 0.70710678118654746 * (                    INV.u_comp[1] -     INV.u_comp[2]); // sqrt(2/3.)*sinf(2*pi/3) = sqrt(2/3.)*(sqrt(3)/2)

    // 区分补偿前的电压和补偿后的电压：
    // (*CTRL).ual, (*CTRL).ube 是补偿前的电压！
    // (*CTRL).ual + INV.ual_comp, (*CTRL).ube + INV.ube_comp 是补偿后的电压！

    // for plotting
    INV.uDcomp_atA = AB2M(INV.ual_comp, INV.ube_comp, INV.cos_thetaA, INV.sin_thetaA);
    INV.uQcomp_atA = AB2T(INV.ual_comp, INV.ube_comp, INV.cos_thetaA, INV.sin_thetaA);
}

void Online_PAA_Based_Compensation(void)
{

    // /* 角度反馈: (*CTRL).I->theta_d_elec or ELECTRICAL_POSITION_FEEDBACK? */
    // INV.thetaA = ELECTRICAL_POSITION_FEEDBACK;
    // // I (current vector amplitude)
    // INV.iD_atA = sqrt(IS_C(0)*IS_C(0) + IS_C(1)*IS_C(1));

    /* 在Park2012中，a相电流被建模成了sin函数，一个sin函数的自变量角度放在正交坐标系下看就是在交轴上的，所以要-1.5*pi */

    // Phase A current's fundamental component transformation
    /* 这里使用哪个角度的关键不在于是有感的角度还是无感的角度，而是你FOC电流控制器（Park变换）用的角度是哪个？ */
    if ((*CTRL).S->go_sensorless)
    {
        INV.thetaA = -M_PI * 1.5 + ELECTRICAL_POSITION_FEEDBACK + atan2((*CTRL).I->idq_cmd[1], (*CTRL).I->idq_cmd[0]); /* Q: why -pi*(1.5)? */ /* ParkSul2014 suggests to use PLL to extract thetaA from current command */
    }
    else
    {
        INV.thetaA = -M_PI * 1.5 + (*CTRL).I->theta_d_elec + atan2((*CTRL).I->idq_cmd[1], (*CTRL).I->idq_cmd[0]); /* Q: why -pi*(1.5)? */ /* ParkSul2014 suggests to use PLL to extract thetaA from current command */
    }
    INV.thetaA = shift2pi(INV.thetaA); /* Q: how to handle it when INV.thetaA jumps between pi and -pi? */ // 这句话绝对不能省去，否则C相的梯形波会出错。

    INV.cos_thetaA = cosf(INV.thetaA);
    INV.sin_thetaA = sinf(INV.thetaA);
    INV.iD_atA = AB2M(IS_C(0), IS_C(1), INV.cos_thetaA, INV.sin_thetaA);
    INV.iQ_atA = AB2T(IS_C(0), IS_C(1), INV.cos_thetaA, INV.sin_thetaA);

    if (FALSE)
    {
        /* Use q-axis current in phase A angle (Tentative and Failed) */
        INV.I5_plus_I7 = INV.iQ_atA * cosf(6 * INV.thetaA);
        INV.I5_plus_I7_LPF = lpf1_inverter(INV.I5_plus_I7, INV.I5_plus_I7_LPF);

        INV.I11_plus_I13 = INV.iQ_atA * cosf(12 * INV.thetaA);
        INV.I11_plus_I13_LPF = lpf1_inverter(INV.I11_plus_I13, INV.I11_plus_I13_LPF);

        INV.I17_plus_I19 = INV.iQ_atA * cosf(18 * INV.thetaA);
        INV.I17_plus_I19_LPF = lpf1_inverter(INV.I17_plus_I19, INV.I17_plus_I19_LPF);
    }
    else
    {
        /* Use d-axis current in phase A angle */
        INV.I5_plus_I7 = INV.iD_atA * sinf(6 * INV.thetaA);                     /* Q: Why sinf? Why not cosf? 和上面的-1.5*pi有关系吗？ */
        INV.I5_plus_I7_LPF = lpf1_inverter(INV.I5_plus_I7, INV.I5_plus_I7_LPF); /* lpf1 for inverter */

        INV.I11_plus_I13 = INV.iD_atA * sinf(12 * INV.thetaA);
        INV.I11_plus_I13_LPF = lpf1_inverter(INV.I11_plus_I13, INV.I11_plus_I13_LPF);

        INV.I17_plus_I19 = INV.iD_atA * sinf(18 * INV.thetaA);
        INV.I17_plus_I19_LPF = lpf1_inverter(INV.I17_plus_I19, INV.I17_plus_I19_LPF);
    }

#if PC_SIMULATION
    // INV.gamma_a2 = 0.0;
    // INV.gamma_a3 = 0.0;
#endif

    /* Online Update Sigmoid a3 */
    // if((*CTRL).timebase>35){
    //     INV.gamma_I_plateau = 0.0;
    // }
    INV.sig_a3 -= CL_TS * INV.gamma_a3 // *fabs((*CTRL).I->cmd_speed_rpm)
                  * (INV.w6 * INV.I5_plus_I7_LPF + INV.w12 * INV.I11_plus_I13_LPF + INV.w18 * INV.I17_plus_I19_LPF);
#ifdef _XCUBE2
    if (INV.sig_a3 > 50)
    {
        INV.sig_a3 = 50;
    }
    else if (INV.sig_a3 < 0.5)
    {
        INV.sig_a3 = 0.5;
    }
#endif
#ifdef _XCUBE1
    if (INV.sig_a3 > 100)
    {
        INV.sig_a3 = 100;
    }
    else if (INV.sig_a3 < 5)
    {
        INV.sig_a3 = 5;
    }
#endif

    // (*CTRL).S->Motor_or_Generator = sign((*CTRL).I->omg_elec * (*CTRL).I->idq_cmd[1]);
    // (*CTRL).S->Motor_or_Generator = sign((*CTRL).I->cmd_omg_elec);

    /* Online Update Sigmoid a2 */
    if ((*CTRL).timebase > 2)
    {
        /* Sensorless: Adaptive a2 based on flux amplitude error */
        // use linear FE
        // INV.sig_a2 += CL_TS * -100 * AFEOE.output_error_dq[0];

        // use nonlinear (saturation) FE
        INV.sig_a2 += CL_TS * -INV.gamma_a2 * (*CTRL).S->Motor_or_Generator * (MOTOR.KActive - FE.htz.psi_2_ampl_lpf);

        /* Sensored: Adaptive a2 based on position error */
        /* To use this, you must have a large enough stator current */
        /* 这个好像只对梯形波（电流值的函数）有效 ……*/
        // INV.sig_a2 += CL_TS * INV.gain_Vsat * sinf(ENC.theta_d_elec - ELECTRICAL_POSITION_FEEDBACK) * (*CTRL).S->Motor_or_Generator;
    }
    if (INV.sig_a2 > 40)
    {
        INV.sig_a2 = 40;
    }
    else if (INV.sig_a2 < 2)
    {
        INV.sig_a2 = 2;
    }

    /* Chen2021: linear approximation of u-i curve */
    // if((*CTRL).timebase>35){
    //     INV.gamma_I_plateau = 0.0;
    // }
    // INV.I_plateau += CL_TS * 0 * INV.gamma_I_plateau \
    //                         // *fabs((*CTRL).I->cmd_speed_rpm)
    //                         *(    1*INV.I5_plus_I7_LPF
    //                             + 0*INV.I11_plus_I13_LPF
    //                             + 0*INV.I17_plus_I19_LPF
    //                          );
    // if(INV.I_plateau > INV.I_plateau_Max){
    //     INV.I_plateau = INV.I_plateau_Max;
    // }else if(INV.I_plateau < INV.I_plateau_Min){
    //     INV.I_plateau = INV.I_plateau_Min;
    // }

    /* Chen2021SlessInv 覆盖 */
    if (INV.gamma_I_plateau != 0)
    {
        REAL ia_cmd = ((*CTRL).O->iab_cmd[0]);
        REAL ib_cmd = (-0.5 * (*CTRL).O->iab_cmd[0] - SIN_DASH_2PI_SLASH_3 * (*CTRL).O->iab_cmd[1]);
        REAL ic_cmd = (-0.5 * (*CTRL).O->iab_cmd[0] - SIN_2PI_SLASH_3 * (*CTRL).O->iab_cmd[1]);
        REAL oneOver_I_plateau = 1.0 / INV.I_plateau;
        INV.u_comp[0] = trapezoidal_voltage_by_phase_current(ia_cmd, INV.V_plateau, INV.I_plateau, oneOver_I_plateau);
        INV.u_comp[1] = trapezoidal_voltage_by_phase_current(ib_cmd, INV.V_plateau, INV.I_plateau, oneOver_I_plateau);
        INV.u_comp[2] = trapezoidal_voltage_by_phase_current(ic_cmd, INV.V_plateau, INV.I_plateau, oneOver_I_plateau);

        /* Online Sigmoid a3 覆盖 覆盖 */
        INV.u_comp[0] = sigmoid_online_v2(ia_cmd, INV.sig_a2, INV.sig_a3);
        INV.u_comp[1] = sigmoid_online_v2(ib_cmd, INV.sig_a2, INV.sig_a3);
        INV.u_comp[2] = sigmoid_online_v2(ic_cmd, INV.sig_a2, INV.sig_a3);
    }

    /* 相补偿电压Clarke为静止正交坐标系电压。 */
    // 改成恒幅值变换
    INV.ual_comp = 0.66666666667 * (INV.u_comp[0] - 0.5 * INV.u_comp[1] - 0.5 * INV.u_comp[2]);
    INV.ube_comp = 0.66666666667 * 0.86602540378 * (INV.u_comp[1] - INV.u_comp[2]);
    // INV.ual_comp = SQRT_2_SLASH_3      * (INV.u_comp[0] - 0.5*INV.u_comp[1] - 0.5*INV.u_comp[2]); // sqrt(2/3.)
    // INV.ube_comp = 0.70710678118654746 * (                    INV.u_comp[1] -     INV.u_comp[2]); // sqrt(2/3.)*sinf(2*pi/3) = sqrt(2/3.)*(sqrt(3)/2)

    // 区分补偿前的电压和补偿后的电压：
    // (*CTRL).ual, (*CTRL).ube 是补偿前的电压！
    // (*CTRL).ual + INV.ual_comp, (*CTRL).ube + INV.ube_comp 是补偿后的电压！
}

/* MAIN */
void main_inverter_voltage_command(int bool_use_iab_cmd)
{
    REAL Ia, Ib;

    if (bool_use_iab_cmd)
    {
        Ia = (*CTRL).O->iab_cmd[0];
        Ib = (*CTRL).O->iab_cmd[1];
    }
    else
    {
        Ia = (*CTRL).I->iab[0];
        Ib = (*CTRL).I->iab[1];
    }

    /* We use iab_cmd instead of iab to look-up */

    if (G.FLAG_INVERTER_NONLINEARITY_COMPENSATION == 0)
    {

        (*CTRL).O->uab_cmd_to_inverter[0] = (*CTRL).O->uab_cmd[0];
        (*CTRL).O->uab_cmd_to_inverter[1] = (*CTRL).O->uab_cmd[1];

        (*CTRL).O->uab_cmd_to_inverter[0 + 2] = (*CTRL).O->uab_cmd[0 + 2];
        (*CTRL).O->uab_cmd_to_inverter[1 + 2] = (*CTRL).O->uab_cmd[1 + 2];

/* For scope only */
#if PC_SIMULATION
        REAL ualbe_dist[2];
        get_distorted_voltage_via_CurveFitting((*CTRL).O->uab_cmd[0], (*CTRL).O->uab_cmd[1], Ia, Ib, ualbe_dist);
        INV.ual_comp = ualbe_dist[0];
        INV.ube_comp = ualbe_dist[1];
#endif
    }
    else if (G.FLAG_INVERTER_NONLINEARITY_COMPENSATION == 4)
    {

        REAL ualbe_dist[2];
        get_distorted_voltage_via_LUT_indexed(Ia, Ib, ualbe_dist);
        (*CTRL).O->uab_cmd_to_inverter[0] = (*CTRL).O->uab_cmd[0] + ualbe_dist[0];
        (*CTRL).O->uab_cmd_to_inverter[1] = (*CTRL).O->uab_cmd[1] + ualbe_dist[1];

        /* For scope only */
        INV.ual_comp = ualbe_dist[0];
        INV.ube_comp = ualbe_dist[1];
    }
    else if (G.FLAG_INVERTER_NONLINEARITY_COMPENSATION == 3)
    {
/* 查表法-补偿 */
// // Measured in Simulation when the inverter is modelled according to the experimental measurements
// #define LENGTH_OF_LUT  19
// REAL lut_current_ctrl[LENGTH_OF_LUT] = {-3.78, -3.36, -2.94, -2.52, -2.1, -1.68, -1.26, -0.839998, -0.419998, -0, 0.420002, 0.840002, 1.26, 1.68, 2.1, 2.52, 2.94, 3.36, 3.78};
// // REAL lut_voltage_ctrl[LENGTH_OF_LUT] = {-6.41808, -6.41942, -6.39433, -6.36032, -6.25784, -6.12639, -5.79563, -5.35301, -3.61951, 0, 3.5038, 5.24969, 5.73176, 6.11153, 6.24738, 6.35941, 6.43225, 6.39274, 6.39482};
// #define MANUAL_CORRECTION 1.1 // [V]
// REAL lut_voltage_ctrl[LENGTH_OF_LUT] = {
//     -6.41808+MANUAL_CORRECTION,
//     -6.41942+MANUAL_CORRECTION,
//     -6.39433+MANUAL_CORRECTION,
//     -6.36032+MANUAL_CORRECTION,
//     -6.25784+MANUAL_CORRECTION,
//     -6.12639+MANUAL_CORRECTION,
//     -5.79563+MANUAL_CORRECTION,
//     -5.35301+MANUAL_CORRECTION,
//     -3.61951, 0, 3.5038,
//      5.24969-MANUAL_CORRECTION,
//      5.73176-MANUAL_CORRECTION,
//      6.11153-MANUAL_CORRECTION,
//      6.24738-MANUAL_CORRECTION,
//      6.35941-MANUAL_CORRECTION,
//      6.43225-MANUAL_CORRECTION,
//      6.39274-MANUAL_CORRECTION,
//      6.39482-MANUAL_CORRECTION};

// #define LENGTH_OF_LUT  100
// REAL lut_current_ctrl[LENGTH_OF_LUT] = {-4.116, -4.032, -3.948, -3.864, -3.78, -3.696, -3.612, -3.528, -3.444, -3.36, -3.276, -3.192, -3.108, -3.024, -2.94, -2.856, -2.772, -2.688, -2.604, -2.52, -2.436, -2.352, -2.268, -2.184, -2.1, -2.016, -1.932, -1.848, -1.764, -1.68, -1.596, -1.512, -1.428, -1.344, -1.26, -1.176, -1.092, -1.008, -0.923998, -0.839998, -0.755998, -0.671998, -0.587998, -0.503998, -0.419998, -0.335999, -0.251999, -0.168, -0.084, -0, 0.084, 0.168, 0.252001, 0.336001, 0.420002, 0.504002, 0.588002, 0.672002, 0.756002, 0.840002, 0.924002, 1.008, 1.092, 1.176, 1.26, 1.344, 1.428, 1.512, 1.596, 1.68, 1.764, 1.848, 1.932, 2.016, 2.1, 2.184, 2.268, 2.352, 2.436, 2.52, 2.604, 2.688, 2.772, 2.856, 2.94, 3.024, 3.108, 3.192, 3.276, 3.36, 3.444, 3.528, 3.612, 3.696, 3.78, 3.864, 3.948, 4.032, 4.116, 4.2};
// REAL lut_voltage_ctrl[LENGTH_OF_LUT] = {-6.48905, -6.49021, -6.49137, -6.49253, -6.49368, -6.49227, -6.49086, -6.48945, -6.48803, -6.48662, -6.47993, -6.47323, -6.46653, -6.45983, -6.45313, -6.44465, -6.43617, -6.42769, -6.41921, -6.41072, -6.38854, -6.36637, -6.34419, -6.32202, -6.29984, -6.27187, -6.2439, -6.21593, -6.18796, -6.15999, -6.09216, -6.02433, -5.9565, -5.88867, -5.82083, -5.73067, -5.6405, -5.55033, -5.46016, -5.36981, -5.02143, -4.67305, -4.32467, -3.97629, -3.62791, -2.90251, -2.17689, -1.45126, -0.725632, -1e-06, 0.702441, 1.40488, 2.10732, 2.80976, 3.5122, 3.86321, 4.21409, 4.56497, 4.91585, 5.26649, 5.36459, 5.46268, 5.56078, 5.65887, 5.75696, 5.8346, 5.91224, 5.98987, 6.0675, 6.14513, 6.17402, 6.20286, 6.2317, 6.26054, 6.28938, 6.31347, 6.33755, 6.36164, 6.38572, 6.40981, 6.4303, 6.4508, 6.47129, 6.49178, 6.49105, 6.48483, 6.4786, 6.47238, 6.46616, 6.45994, 6.46204, 6.46413, 6.46623, 6.46832, 6.47042, 6.47202, 6.47363, 6.47524, 6.47684, 6.47843};

// Experimental measurements
// #define LENGTH_OF_LUT 21
// REAL lut_current_ctrl[LENGTH_OF_LUT] = {-4.19999, -3.77999, -3.36001, -2.94002, -2.51999, -2.10004, -1.68004, -1.26002, -0.840052, -0.419948, 5.88754e-06, 0.420032, 0.839998, 1.26003, 1.67998, 2.10009, 2.51996, 2.87326, 3.36001, 3.78002, 4.2};
// REAL lut_voltage_ctrl[LENGTH_OF_LUT] = {-5.20719, -5.2079, -5.18934, -5.15954, -5.11637, -5.04723, -4.93463, -4.76367, -4.42522, -3.46825, 0.317444, 3.75588, 4.55737, 4.87773, 5.04459, 5.15468, 5.22904, 5.33942, 5.25929, 5.28171, 5.30045};

// Experimental measurements 03-18-2021
#define LENGTH_OF_LUT 41
        REAL lut_current_ctrl[LENGTH_OF_LUT] = {-4.20001, -3.98998, -3.78002, -3.57, -3.36002, -3.14999, -2.93996, -2.72993, -2.51998, -2.31003, -2.1, -1.88996, -1.67999, -1.46998, -1.25999, -1.05001, -0.839962, -0.62995, -0.420046, -0.210048, 1.39409e-05, 0.209888, 0.420001, 0.629998, 0.840008, 1.05002, 1.25999, 1.47002, 1.68001, 1.89001, 2.10002, 2.31, 2.51999, 2.73004, 2.94, 3.14996, 3.35995, 3.57001, 3.77999, 3.98998, 4.2};
        REAL lut_voltage_ctrl[LENGTH_OF_LUT] = {-5.75434, -5.74721, -5.72803, -5.70736, -5.68605, -5.66224, -5.63274, -5.59982, -5.56391, -5.52287, -5.47247, -5.40911, -5.33464, -5.25019, -5.14551, -5.00196, -4.80021, -4.48369, -3.90965, -2.47845, -0.382101, 2.02274, 3.7011, 4.35633, 4.71427, 4.94376, 5.10356, 5.22256, 5.31722, 5.39868, 5.46753, 5.5286, 5.57507, 5.62385, 5.66235, 5.70198, 5.73617, 5.76636, 5.79075, 5.81737, 5.83632};

        REAL ualbe_dist[2];
        get_distorted_voltage_via_LUT((*CTRL).O->uab_cmd[0], (*CTRL).O->uab_cmd[1], Ia, Ib, ualbe_dist, lut_voltage_ctrl, lut_current_ctrl, LENGTH_OF_LUT);
        (*CTRL).O->uab_cmd_to_inverter[0] = (*CTRL).O->uab_cmd[0] + ualbe_dist[0];
        (*CTRL).O->uab_cmd_to_inverter[1] = (*CTRL).O->uab_cmd[1] + ualbe_dist[1];

        /* For scope only */
        INV.ual_comp = ualbe_dist[0];
        INV.ube_comp = ualbe_dist[1];
    }
    else if (G.FLAG_INVERTER_NONLINEARITY_COMPENSATION == 2)
    {
        /* 拟合法-补偿 */
        REAL ualbe_dist[2] = {0.0, 0.0};
        get_distorted_voltage_via_CurveFitting((*CTRL).O->uab_cmd[0], (*CTRL).O->uab_cmd[1], Ia, Ib, ualbe_dist);
        (*CTRL).O->uab_cmd_to_inverter[0] = (*CTRL).O->uab_cmd[0] + ualbe_dist[0];
        (*CTRL).O->uab_cmd_to_inverter[1] = (*CTRL).O->uab_cmd[1] + ualbe_dist[1];

        /* For scope only */
        INV.ual_comp = ualbe_dist[0];
        INV.ube_comp = ualbe_dist[1];
    }
    else if (G.FLAG_INVERTER_NONLINEARITY_COMPENSATION == 1)
    {
        /* 梯形波自适应 */
        Modified_ParkSul_Compensation();
        (*CTRL).O->uab_cmd_to_inverter[0] = (*CTRL).O->uab_cmd[0] + INV.ual_comp;
        (*CTRL).O->uab_cmd_to_inverter[1] = (*CTRL).O->uab_cmd[1] + INV.ube_comp;
    }
    else if (G.FLAG_INVERTER_NONLINEARITY_COMPENSATION == 5)
    {
        /* Sigmoid自适应 */
        Online_PAA_Based_Compensation();
        (*CTRL).O->uab_cmd_to_inverter[0] = (*CTRL).O->uab_cmd[0] + INV.ual_comp;
        (*CTRL).O->uab_cmd_to_inverter[1] = (*CTRL).O->uab_cmd[1] + INV.ube_comp;
    }
}

#if MACHINE_TYPE % 10 == 2

void init_experiment()
{

#if ENABLE_COMMISSIONING
    // init_COMM(); // 参数自整定初始化，不要在这里做！
#endif

    init_CTRL(); // 控制器结构体初始化

#if MACHINE_TYPE == 2
    init_pmsm_observers(); // 永磁电机观测器初始化
    init_FE();
#else
    rk4_init(); // 龙格库塔法结构体初始化
    observer_init();
#endif
}

// 初始化函数
void init_CTRL()
{

    allocate_CTRL(CTRL);

    /* Basic quantities */
    (*CTRL).timebase = 0.0;

    /* Machine parameters */
    // elec
    (*CTRL).motor->R = INIT_R;
    (*CTRL).motor->KE = INIT_KE; // * (0.1/0.1342); // 【实验编号：】
    (*CTRL).motor->Ld = INIT_LD;
    (*CTRL).motor->Lq = INIT_LQ;
    (*CTRL).motor->Lq_inv = 1.0 / (*CTRL).motor->Lq;
    (*CTRL).motor->DeltaL = (*CTRL).motor->Ld - (*CTRL).motor->Lq; // for IPMSM
    (*CTRL).motor->KActive = (*CTRL).motor->KE;                    // TODO:

#if MACHINE_TYPE % 10 == 1
    (*CTRL).motor->Rreq = U_MOTOR_RREQ;
#endif
    // mech
    (*CTRL).motor->npp = INIT_NPP;
    (*CTRL).motor->npp_inv = 1.0 / (*CTRL).motor->npp;
    (*CTRL).motor->Js = INIT_JS * (1.0 + LOAD_INERTIA);
    (*CTRL).motor->Js_inv = 1.0 / (*CTRL).motor->Js;

    // /* Peripheral configurations */

    /* Inverter */
    (*CTRL).inv->filter_pole = 3000 * 2 * M_PI;
    inverterNonlinearity_Initialization();
    G.FLAG_INVERTER_NONLINEARITY_COMPENSATION = INVERTER_NONLINEARITY_COMPENSATION_INIT;
    G.FLAG_TUNING_CURRENT_SCALE_FACTOR = TUNING_CURRENT_SCALE_FACTOR_INIT;

    /* Capture */
    (*CTRL).cap->flag_nonlinear_filtering = FALSE;
    (*CTRL).cap->flag_bad_U_capture = FALSE;
    (*CTRL).cap->flag_bad_V_capture = FALSE;
    (*CTRL).cap->flag_bad_W_capture = FALSE;
    (*CTRL).cap->good_capture_U[0] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_U[1] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_U[2] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_U[3] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_V[0] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_V[1] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_V[2] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_V[3] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_W[0] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_W[1] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_W[2] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->good_capture_W[3] = SYSTEM_HALF_PWM_MAX_COUNT;
    (*CTRL).cap->ECapIntCount[0] = 0;
    (*CTRL).cap->ECapIntCount[1] = 0;
    (*CTRL).cap->ECapIntCount[2] = 0;
    (*CTRL).cap->ECapPassCount[0] = 0;
    (*CTRL).cap->ECapPassCount[1] = 0;
    (*CTRL).cap->ECapPassCount[2] = 0;

    /* Peripheral configurations */
    //(*CTRL).enc->OffsetCountBetweenIndexAndUPhaseAxis = 0;
    //(*CTRL).enc->theta_d_offset = (*CTRL).enc->OffsetCountBetweenIndexAndUPhaseAxis * CNT_2_ELEC_RAD;

    /* Console */
    // See init_experiment_overwrite() in CJHMainISR.c
    G.flag_do_inverter_characteristics = SELF_COMM_INVERTER;
    G.overwrite_vdc = 20;

    /* Black Box Model | Controller quantities */
    #ifdef USE_BEZIER
    set_points(&BzController);
    #endif

    // PID调谐
    ACMSIMC_PIDTuner();

    // PID regulators
    (*CTRL).S->iM = &pid1_iM;
    (*CTRL).S->iT = &pid1_iT;
    (*CTRL).S->spd = &pid1_spd;
    (*CTRL).S->pos = &pid1_pos;

    // commands
    (*CTRL).I->cmd_psi = INIT_KE;
    // (*CTRL).I->idq_cmd[0] = 0.0;
    // (*CTRL).I->idq_cmd[1] = 0.0;
    // // error
    // (*CTRL).omg_ctrl_err = 0.0;
    // (*CTRL).speed_ctrl_err = 0.0;
    // // feedback
    // (*CTRL).I->omg_elec = 0.0;
    // (*CTRL).I->theta_d_elec = 0.0;
    // (*CTRL).I->iab[0] = 0.0;
    // (*CTRL).I->iab[1] = 0.0;
    // (*CTRL).I->idq[0] = 0.0;
    // (*CTRL).I->idq[1] = 0.0;
    // (*CTRL).psi_mu_al__fb = 0.0;
    // (*CTRL).psi_mu_be__fb = 0.0;
    // (*CTRL).Tem = 0.0;
    // // indirect field oriented control
    (*CTRL).S->cosT = 1.0;
    (*CTRL).S->sinT = 0.0;
    (*CTRL).S->cosT_compensated_1p5omegaTs = 1.0;
    (*CTRL).S->sinT_compensated_1p5omegaTs = 0.0;
    (*CTRL).S->cosT2 = 1.0;
    (*CTRL).S->sinT2 = 0.0;
    // (*CTRL).S->omega_syn = 0.0;
    (*CTRL).S->the_vc_count = 1; // starts from 1

    (*CTRL).S->ctrl_strategy = CONTROL_STRATEGY;
    (*CTRL).S->go_sensorless = SENSORLESS_CONTROL;
}

/* id=0控制 */
void null_d_control(int set_current_loop, REAL set_iq_cmd, REAL set_id_cmd)
{
// 定义局部变量，减少对CTRL的直接调用
#define MOTOR (*(*CTRL).motor)

    /// 5. 转速环（使用反馈转速）
    if ((*CTRL).S->the_vc_count++ >= SPEED_LOOP_CEILING)
    {
        (*CTRL).S->the_vc_count = 1;

        // 速度信号通通变成机械角速度 mech
        pid1_spd.Ref = (*CTRL).I->cmd_omg_elec * MOTOR.npp_inv;
        pid1_spd.Fbk = (*CTRL).I->omg_elec * MOTOR.npp_inv;

#ifdef USE_BEZIER
        control_output(&pid1_spd, &BzController);
#else
        // 使用线性控制器
        // pid1_spd.Ki = 0;
        // pid1_spd.Kp = 0;
        pid1_spd.calc(&pid1_spd);
#endif

        (*CTRL).I->idq_cmd[1] = pid1_spd.Out;

        /*给死q轴电流 unit：安培*/
        // (*CTRL).I->idq_cmd[1] = 4;
        // (*CTRL).I->idq_cmd[1+2] = pid1_spd.Out;
    }
// 磁链环
#if CONTROL_STRATEGY == NULL_D_AXIS_CURRENT_CONTROL
    // (*CTRL).I->cmd_rotor_flux_Wb = 0.0;
    // (*CTRL).I->idq_cmd[0] = (*CTRL).I->cmd_rotor_flux_Wb / MOTOR.Ld;

#if PC_SIMULATION == TRUE
    REAL IMIN = 2;
    // if((*CTRL).timebase>23){
    //     IMIN = 8;
    // }else if((*CTRL).timebase>20){
    //     IMIN = 6;
    // }else if((*CTRL).timebase>15){
    //     IMIN = 5;
    // }else if((*CTRL).timebase>10){
    //     IMIN = 3;
    // }else if((*CTRL).timebase>5){
    //     IMIN = 2;
    // }
#else
    REAL IMIN = 2; // 2;
#endif
    /* 逆变器非线性在线校正更新theta_t需要空载时电流不为零 */
    // if(fabs((*CTRL).I->idq_cmd[1])<IMIN){
    //     (*CTRL).I->idq_cmd[0] = IMIN;
    // }else{
    //     (*CTRL).I->idq_cmd[0] = 0;
    // }

    /* Overwrite if set_id_cmd is set [Both are POSITIVE] */
    // if(set_id_cmd!=0){
    (*CTRL).I->idq_cmd[0] = set_id_cmd;
    // (*CTRL).I->idq_cmd[0+2] = set_id_cmd;
    // }
#else
    (*CTRL).I->cmd_rotor_flux_Wb = MOTOR.Ld * set_id_cmd;
    (*CTRL).I->idq_cmd[0] = set_id_cmd;
#if PC_SIMULATION
    printf("CONTROL_STRATEGY Not Implemented: %s", CONTROL_STRATEGY);
    getch();
#endif
#endif
    // 转矩 For luenberger position observer for HFSI
    // TODO：这里需要1.5倍吗？
    // (*CTRL).I->Tem     = 1.5 * MOTOR.npp * (MOTOR.KE + (MOTOR.Ld-MOTOR.Lq)*(*CTRL).I->idq[0]) * (*CTRL).I->idq[1];
    // (*CTRL).I->Tem_cmd = 1.5 * MOTOR.npp * (MOTOR.KE + (MOTOR.Ld-MOTOR.Lq)*(*CTRL).I->idq_cmd[0]) * (*CTRL).I->idq_cmd[1];
    (*CTRL).I->Tem = MOTOR.npp * (MOTOR.KE + (MOTOR.Ld - MOTOR.Lq) * (*CTRL).I->idq[0]) * (*CTRL).I->idq[1];
    (*CTRL).I->Tem_cmd = MOTOR.npp * (MOTOR.KE + (MOTOR.Ld - MOTOR.Lq) * (*CTRL).I->idq_cmd[0]) * (*CTRL).I->idq_cmd[1];

/// 5.Sweep 扫频将覆盖上面产生的励磁、转矩电流指令
#if EXCITATION_TYPE == EXCITATION_SWEEP_FREQUENCY
#if SWEEP_FREQ_C2V == TRUE
    pid1_iq.Ref = set_iq_cmd;
#endif
#if SWEEP_FREQ_C2C == TRUE
    pid1_iq.Ref = 0.0;
    pid1_id.Ref = set_iq_cmd; // 故意反的
#endif
#endif

    // debug
    // set_current_loop = 1;
    // set_iq_cmd = 1;

    /// 6. 电流环
    // d-axis
    pid1_id.Fbk = (*CTRL).I->idq[0];
    pid1_id.Ref = (*CTRL).I->idq_cmd[0];
    pid1_id.calc(&pid1_id);
    // pid2_id.Fbk = (*CTRL).I->idq[0+2];
    // pid2_id.Ref = (*CTRL).I->idq_cmd[0+2];
    // pid2_id.calc(&pid2_id);
    // q-axis
    pid1_iq.Fbk = (*CTRL).I->idq[1];
    pid1_iq.Ref = (*CTRL).I->idq_cmd[1];
    if (set_current_loop == 1)
    {
        pid1_iq.Ref = set_iq_cmd;
        (*CTRL).I->idq_cmd[1] = set_iq_cmd;
    }
    pid1_iq.calc(&pid1_iq);

// pid2_iq.Fbk = (*CTRL).I->idq[1+2];
// pid2_iq.Ref = (*CTRL).I->idq_cmd[1+2]; if(set_current_loop==1){pid2_iq.Ref = set_iq_cmd;}
// pid2_iq.calc(&pid2_iq);
// 电压电流解耦
#if VOLTAGE_CURRENT_DECOUPLING_CIRCUIT == TRUE
    REAL decoupled_d_axis_voltage = pid1_id.Out - pid1_iq.Fbk * MOTOR.Lq * (*CTRL).I->omg_elec;
    REAL decoupled_q_axis_voltage = pid1_iq.Out + (MOTOR.KE + pid1_id.Fbk * MOTOR.Ld) * (*CTRL).I->omg_elec;
#else
    REAL decoupled_d_axis_voltage = pid1_id.Out;
    REAL decoupled_q_axis_voltage = pid1_iq.Out;
#endif

    (*CTRL).O->udq_cmd[0] = decoupled_d_axis_voltage;
    (*CTRL).O->udq_cmd[1] = decoupled_q_axis_voltage;
    // (*CTRL).O->udq_cmd[0+2] = pid2_id.Out;
    // (*CTRL).O->udq_cmd[1+2] = pid2_iq.Out;
    // (*CTRL).O->udq_cmd[1] = 20;
}

REAL used_theta_d_elec = 0.0;

REAL controller(REAL set_rpm_speed_command,
                int set_current_loop, REAL set_iq_cmd, REAL set_id_cmd,
                int flag_overwrite_theta_d, REAL Overwrite_Current_Frequency,
                REAL angle_shift_for_first_inverter,
                REAL angle_shift_for_second_inverter)
{

    /// 0. 参数时变
    // if (fabs((*CTRL).timebase-2.0)<CL_TS){
    //     printf("[Runtime] Rotor resistance of the simulated IM has changed!\n");
    //     ACM.alpha = 0.5*IM_ROTOR_RESISTANCE / IM_MAGNETIZING_INDUCTANCE;
    //     ACM.rreq = ACM.alpha*ACM.Lmu;
    //     ACM.rr   = ACM.alpha*(ACM.Lm+ACM.Llr);
    // }

    /// 1. 生成转速指令
    (*CTRL).I->cmd_position_rad = 0.0;                                           // mechanical
    (*CTRL).I->cmd_speed_rpm = set_rpm_speed_command;                            // mechanical
    (*CTRL).I->cmd_omg_elec = (*CTRL).I->cmd_speed_rpm * RPM_2_ELEC_RAD_PER_SEC; // electrical
    /*最终生成cmd_omg_elec单位为rad/s，国际单位好评！*/

    /// 2. 生成磁链指令
    if (set_id_cmd == 0.0)
    {
        // if(set_iq_cmd<1){
        //     set_id_cmd = 1; // 2;
        // }
    }

    /// 3. 调用观测器：估计的电气转子位置和电气转子转速反馈
    // MOTOR_observers();
    if ((*CTRL).S->go_sensorless == TRUE)
    {
        // （无感）
        (*CTRL).I->omg_elec = ELECTRICAL_SPEED_FEEDBACK;        // harnefors.omg_elec;
        (*CTRL).I->theta_d_elec = ELECTRICAL_POSITION_FEEDBACK; // harnefors.theta_d;
    }

    /// 4. 帕克变换（使用反馈位置）
    if (flag_overwrite_theta_d)
    {
        if (fabs(Overwrite_Current_Frequency) > 0)
        {
            used_theta_d_elec += CL_TS * Overwrite_Current_Frequency * 2 * M_PI;
            if (used_theta_d_elec > M_PI)
                used_theta_d_elec -= 2 * M_PI;
            if (used_theta_d_elec < -M_PI)
                used_theta_d_elec += 2 * M_PI;
        }
        else
        {
            used_theta_d_elec = 0.0;
        }
    }
    else
    {
        used_theta_d_elec = (*CTRL).I->theta_d_elec;
    }
    (*CTRL).S->cosT = cosf(used_theta_d_elec + angle_shift_for_first_inverter);
    (*CTRL).S->sinT = sinf(used_theta_d_elec + angle_shift_for_first_inverter);
    (*CTRL).I->idq[0] = AB2M((*CTRL).I->iab[0], (*CTRL).I->iab[1], (*CTRL).S->cosT, (*CTRL).S->sinT);
    (*CTRL).I->idq[1] = AB2T((*CTRL).I->iab[0], (*CTRL).I->iab[1], (*CTRL).S->cosT, (*CTRL).S->sinT);

    // (*CTRL).S->cosT2 = cosf(used_theta_d_elec + angle_shift_for_second_inverter);
    // (*CTRL).S->sinT2 = sinf(used_theta_d_elec + angle_shift_for_second_inverter);
    // (*CTRL).I->idq[0+2] = AB2M((*CTRL).I->iab[0+2], (*CTRL).I->iab[1+2], (*CTRL).S->cosT2, (*CTRL).S->sinT2);
    // (*CTRL).I->idq[1+2] = AB2T((*CTRL).I->iab[0+2], (*CTRL).I->iab[1+2], (*CTRL).S->cosT2, (*CTRL).S->sinT2);

    /* 更新依赖于dq轴电流的物理量 */
    // MOTOR.KActive = MOTOR.KE + (MOTOR.Ld - MOTOR.Lq) * (*CTRL).I->idq_cmd[0];
    MOTOR.KActive = MOTOR.KE + (MOTOR.Ld - MOTOR.Lq) * (*CTRL).I->idq[0];

/// 调用具体的控制器
#if CONTROL_STRATEGY == NULL_D_AXIS_CURRENT_CONTROL
    null_d_control(set_current_loop, set_iq_cmd, set_id_cmd);
    (*CTRL).S->Motor_or_Generator = sign((*CTRL).I->omg_elec * (*CTRL).I->idq_cmd[1]); // sign((*CTRL).I->cmd_omg_elec);
#endif

    /// 7. 反帕克变换
    // See D:\Users\horyc\Downloads\Documents\2003 TIA Bae SK Sul A compensation method for time delay of.pdf
    // (*CTRL).S->cosT_compensated_1p5omegaTs = cosf(used_theta_d_elec + 1.5*(*CTRL).I->omg_elec*CL_TS);
    // (*CTRL).S->sinT_compensated_1p5omegaTs = sinf(used_theta_d_elec + 1.5*(*CTRL).I->omg_elec*CL_TS);
    (*CTRL).S->cosT_compensated_1p5omegaTs = (*CTRL).S->cosT;
    (*CTRL).S->sinT_compensated_1p5omegaTs = (*CTRL).S->sinT;
    (*CTRL).O->uab_cmd[0] = MT2A((*CTRL).O->udq_cmd[0], (*CTRL).O->udq_cmd[1], (*CTRL).S->cosT_compensated_1p5omegaTs, (*CTRL).S->sinT_compensated_1p5omegaTs);
    (*CTRL).O->uab_cmd[1] = MT2B((*CTRL).O->udq_cmd[0], (*CTRL).O->udq_cmd[1], (*CTRL).S->cosT_compensated_1p5omegaTs, (*CTRL).S->sinT_compensated_1p5omegaTs);
    (*CTRL).O->iab_cmd[0] = MT2A((*CTRL).I->idq_cmd[0], (*CTRL).I->idq_cmd[1], (*CTRL).S->cosT_compensated_1p5omegaTs, (*CTRL).S->sinT_compensated_1p5omegaTs);
    (*CTRL).O->iab_cmd[1] = MT2B((*CTRL).I->idq_cmd[0], (*CTRL).I->idq_cmd[1], (*CTRL).S->cosT_compensated_1p5omegaTs, (*CTRL).S->sinT_compensated_1p5omegaTs);

    // (*CTRL).O->uab_cmd[0+2] = MT2A((*CTRL).O->udq_cmd[0+2], (*CTRL).O->udq_cmd[1+2], (*CTRL).S->cosT2, (*CTRL).S->sinT2);
    // (*CTRL).O->uab_cmd[1+2] = MT2B((*CTRL).O->udq_cmd[0+2], (*CTRL).O->udq_cmd[1+2], (*CTRL).S->cosT2, (*CTRL).S->sinT2);

    /// 8. 补偿逆变器非线性
    main_inverter_voltage_command(1);

/// 9. 结束
#if PC_SIMULATION
    // for plot
    ACM.rpm_cmd = set_rpm_speed_command;
    // (*CTRL).speed_ctrl_err = set_rpm_speed_command*RPM_2_ELEC_RAD_PER_SEC - (*CTRL).I->omg_elec;
#endif

    return used_theta_d_elec;
}

#endif
