FFIS_ctrl.cpp

/*
 * FFIS_ctrl.cpp
 *
 * Online computation of optimal feedforward current setpoints for synchronous
 * machines with time-varying parameters. See FFIS_ctrl.h for the full
 * reference and method description.
 *
 *  Created on: 9. 1. 2018
 *      Author: Antonin Glac, University of West Bohemia (UWB)
 */
#include "FFIS_ctrl.h"
#include <math.h>
#include "poly34f.h"


S_FFIS_Matrix_Q FFIS_MTpC;
S_FFIS_Matrix_Q FFIS_MTpV;
S_FFIS_Matrix_Q FFIS_MCurr;
S_FFIS_Matrix_Q FFIS_NTorq;
S_FFIS_Matrix_Q FFIS_NVolt;


#if(FFIS_CONST_PARAM)
float FFIS_MTPA_MCurr_d = 0;
float FFIS_MTPA_MCurr_q = 0;
#endif //(FFIS_CONST_PARAM)


S_FFIS_Input FFIS_Input;
S_FFIS_Output FFIS_Output;

S_FFIS_Machine_Param FFIS_Param;
S_FFIS_Machine_Param_Norm FFIS_Param_Norm;

void FFIS_optimal_control_init_call()
{
    FFIS_optimal_control_init(&FFIS_Param, &FFIS_Param_Norm, &FFIS_Input);
}

void FFIS_optimal_control_call()
{

    FFIS_optimal_control(&FFIS_Param, &FFIS_Param_Norm, &FFIS_Input, &FFIS_Output);

}



void FFIS_matrixInit_Par(S_FFIS_Machine_Param *Param, S_FFIS_Machine_Param_Norm *Param_Norm)        //Matrices dependent on the machine parameters
{
    // MTPC/MTPA hyperbola
    FFIS_MTpC.x11 = (Param_Norm->Lsd_N-Param_Norm->Lsq_N) * 0.5 * 1.5 * Param->pp;
    FFIS_MTpC.x12 = Param_Norm->Lm_N * 1.5 * Param->pp;
    FFIS_MTpC.x22 = -(Param_Norm->Lsd_N-Param_Norm->Lsq_N) * 0.5 * 1.5 * Param->pp;

#if (FFIS_PSI_Q == 1)
    FFIS_MTpC.x1 = 0.25 * Param_Norm->Psi_d_N * 1.5 * Param->pp;
    FFIS_MTpC.x2 = 0.25 * Param_Norm->Psi_q_N * 1.5 * Param->pp;
#else //(FFIS_PSI_Q == 1)
    FFIS_MTpC.x1 = 0.25 * Param_Norm->Psi_d_N * 1.5 * Param->pp;
    FFIS_MTpC.x2 = 0;
#endif //(FFIS_PSI_Q == 1)
    FFIS_MTpC.x0 = 0;

    //Maximum current circle
    FFIS_MCurr.x11 = 1;
    FFIS_MCurr.x12 = 0;
    FFIS_MCurr.x22 = 1;
    FFIS_MCurr.x1 = 0;
    FFIS_MCurr.x2 = 0;
    FFIS_MCurr.x0 = -1.0 * Param_Norm->Ism_N * Param_Norm->Ism_N;

}

void FFIS_matrixInit_M( S_FFIS_Machine_Param *Param, S_FFIS_Machine_Param_Norm *Param_Norm, S_FFIS_Input *in)      //Matrices dependent on machine speed and torque request
{
    //Torque hyperbola
    FFIS_NTorq.x11 = -Param_Norm->Lm_N * 1.5 * Param->pp;
    FFIS_NTorq.x12 = (Param_Norm->Lsd_N-Param_Norm->Lsq_N)* 0.5 * 1.5 * Param->pp;
    FFIS_NTorq.x22 =  Param_Norm->Lm_N * 1.5 * Param->pp;
#if (FFIS_PSI_Q == 1)
    FFIS_NTorq.x1 = -Param_Norm->Psi_q_N * 0.5 * 1.5 * Param->pp;
    FFIS_NTorq.x2 =  Param_Norm->Psi_d_N * 0.5 * 1.5 * Param->pp;
#else //(FFIS_PSI_Q == 1)
    FFIS_NTorq.x1 = 0;
    FFIS_NTorq.x2 = Param_Norm->Psi_d_N * 0.5 * 1.5 * Param->pp;
#endif //(FFIS_PSI_Q == 1)

    // The torque hyperbola is evaluated in per-unit currents (id_N = id/Isfn),
    // so its constant term must be normalized by Usfn*Isfn. The original
    // embedded source divided by (Usfn/Isfn), i.e. it was off by a factor of
    // Isfn^2, which placed the hyperbola far outside the current circle.
    FFIS_NTorq.x0 = -1.0 * in->mmref * Param_Norm->wk_nom / (Param->Usfn * Param->Isfn);


    //Voltage ellipse
    FFIS_NVolt.x11 = Param_Norm->Rs_N*Param_Norm->Rs_N-2*Param_Norm->wk_N*Param_Norm->Rs_N*Param_Norm->Lm_N+ Param_Norm->wk_N*Param_Norm->wk_N*(Param_Norm->Lsd_N*Param_Norm->Lsd_N+Param_Norm->Lm_N*Param_Norm->Lm_N);
    FFIS_NVolt.x12 = Param_Norm->wk_N*Param_Norm->Rs_N*(Param_Norm->Lsd_N-Param_Norm->Lsq_N)+Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lm_N*(Param_Norm->Lsd_N+Param_Norm->Lsq_N);
    FFIS_NVolt.x22 = Param_Norm->Rs_N*Param_Norm->Rs_N+2*Param_Norm->wk_N*Param_Norm->Rs_N*Param_Norm->Lm_N+ Param_Norm->wk_N*Param_Norm->wk_N*(Param_Norm->Lsq_N*Param_Norm->Lsq_N+Param_Norm->Lm_N*Param_Norm->Lm_N);
#if (FFIS_PSI_Q == 1)
    FFIS_NVolt.x1 = Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lsd_N*Param_Norm->Psi_d_N - Param_Norm->Psi_q_N * Param_Norm->wk_N * (Param_Norm->Rs_N - Param_Norm->Lm_N*Param_Norm->wk_N);
    FFIS_NVolt.x2 = Param_Norm->wk_N*Param_Norm->Psi_d_N*(Param_Norm->Rs_N+Param_Norm->wk_N*Param_Norm->Lm_N) + Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lsq_N*Param_Norm->Psi_q_N;
    FFIS_NVolt.x0 = Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Psi_d_N*Param_Norm->Psi_d_N + Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Psi_q_N*Param_Norm->Psi_q_N - Param_Norm->Usf_N*Param_Norm->Usf_N;
#else //(FFIS_PSI_Q == 1)
    FFIS_NVolt.x1 = Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lsd_N*Param_Norm->Psi_d_N;
    FFIS_NVolt.x2 = Param_Norm->wk_N*Param_Norm->Psi_d_N*(Param_Norm->Rs_N+Param_Norm->wk_N*Param_Norm->Lm_N);
    FFIS_NVolt.x0 = Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Psi_d_N*Param_Norm->Psi_d_N - Param_Norm->Usf_N*Param_Norm->Usf_N;
#endif //(FFIS_PSI_Q == 1)


    //hyperbola MTPV
    FFIS_MTpV.x11 = 1.5*Param->pp*(Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lm_N*Param_Norm->Lm_N*(Param_Norm->Lsd_N+Param_Norm->Lsq_N)+0.5*(Param_Norm->Lsd_N-Param_Norm->Lsq_N)*(Param_Norm->Rs_N*Param_Norm->Rs_N+Param_Norm->wk_N*Param_Norm->wk_N*(Param_Norm->Lsd_N*Param_Norm->Lsd_N+Param_Norm->Lm_N*Param_Norm->Lm_N)));
    FFIS_MTpV.x12 = 1.5*Param->pp*(0.5*Param_Norm->Lm_N*(2*Param_Norm->Rs_N*Param_Norm->Rs_N+Param_Norm->wk_N*Param_Norm->wk_N*(Param_Norm->Lsd_N*Param_Norm->Lsd_N+Param_Norm->Lsq_N*Param_Norm->Lsq_N+2*Param_Norm->Lm_N*Param_Norm->Lm_N)));
    FFIS_MTpV.x22 = 1.5*Param->pp*(Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lm_N*Param_Norm->Lm_N*(Param_Norm->Lsd_N+Param_Norm->Lsq_N)-0.5*(Param_Norm->Lsd_N-Param_Norm->Lsq_N)*(Param_Norm->Rs_N*Param_Norm->Rs_N+Param_Norm->wk_N*Param_Norm->wk_N*(Param_Norm->Lsq_N*Param_Norm->Lsq_N+Param_Norm->Lm_N*Param_Norm->Lm_N)));
#if (FFIS_PSI_Q == 1)
    FFIS_MTpV.x1 = 1.5*Param->pp*(((Param_Norm->Rs_N*Param_Norm->Rs_N+Param_Norm->wk_N*Param_Norm->wk_N*(2*Param_Norm->Lsd_N*Param_Norm->Lsd_N-Param_Norm->Lsq_N*Param_Norm->Lsd_N+3*Param_Norm->Lm_N*Param_Norm->Lm_N))*Param_Norm->Psi_d_N*0.25 + (Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lm_N*(Param_Norm->Lsd_N+Param_Norm->Lsq_N))*Param_Norm->Psi_q_N*0.5));
    FFIS_MTpV.x2 = 1.5*Param->pp*((Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lm_N*(Param_Norm->Lsd_N+Param_Norm->Lsq_N))*Param_Norm->Psi_d_N*0.5 + ((Param_Norm->Rs_N*Param_Norm->Rs_N+Param_Norm->wk_N*Param_Norm->wk_N*(2*Param_Norm->Lsd_N*Param_Norm->Lsd_N-Param_Norm->Lsq_N*Param_Norm->Lsd_N+3*Param_Norm->Lm_N*Param_Norm->Lm_N))*Param_Norm->Psi_q_N*0.25));
    FFIS_MTpV.x0 = 0.75*Param->pp*Param_Norm->wk_N*Param_Norm->wk_N* (Param_Norm->Lsd_N*Param_Norm->Psi_d_N*Param_Norm->Psi_d_N + 2*Param_Norm->Lm_N*Param_Norm->Psi_d_N*Param_Norm->Psi_q_N + Param_Norm->Lsq_N*Param_Norm->Psi_q_N*Param_Norm->Psi_q_N);

#else //(FFIS_PSI_Q == 1)
    FFIS_MTpV.x1 = 1.5*Param->pp*((Param_Norm->Rs_N*Param_Norm->Rs_N+Param_Norm->wk_N*Param_Norm->wk_N*(2*Param_Norm->Lsd_N*Param_Norm->Lsd_N-Param_Norm->Lsq_N*Param_Norm->Lsd_N+3*Param_Norm->Lm_N*Param_Norm->Lm_N))*Param_Norm->Psi_d_N*0.25);
    FFIS_MTpV.x2 = 1.5*Param->pp*((Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lm_N*(Param_Norm->Lsd_N+Param_Norm->Lsq_N))*Param_Norm->Psi_d_N*0.5);
    FFIS_MTpV.x0 = 0.75*Param->pp*Param_Norm->wk_N*Param_Norm->wk_N*Param_Norm->Lsd_N*Param_Norm->Psi_d_N*Param_Norm->Psi_d_N;
#endif //(FFIS_PSI_Q == 1)


#if (FFIS_PSI_Q == 1)
    Param_Norm->w_mech_MCurr_NVolt_id0 =     -(2 * Param->Isfn * Param->Psi_d * Param->Rs - sqrt((2 * Param->Lm * Param->Rs * Param->Isfn * Param->Isfn + 2 * Param->Psi_d * Param->Rs * Param->Isfn) * (2 * Param->Lm * Param->Rs * Param->Isfn * Param->Isfn + 2 * Param->Psi_d * Param->Rs * Param->Isfn) - (Param->Isfn * Param->Isfn * Param->Rs * Param->Rs - in->U_dc * 0.5 * in->U_dc * 0.5) * (4 * Param->Isfn * (2 * Param->Lm * Param->Psi_d + 2 * Param->Lsq * Param->Psi_q) + 4 * Param->Psi_d * Param->Psi_d + 4 * Param->Psi_q * Param->Psi_q + 4 * Param->Isfn * Param->Isfn * (Param->Lm * Param->Lm + Param->Lsq * Param->Lsq))) + 2 * Param->Isfn * Param->Isfn * Param->Lm * Param->Rs) / (2 * Param->Isfn * (2 * Param->Lm * Param->Psi_d + 2 * Param->Lsq * Param->Psi_q) + 2 * Param->Psi_d * Param->Psi_d + 2 * Param->Psi_q * Param->Psi_q + 2 * Param->Isfn * Param->Isfn * (Param->Lm * Param->Lm + Param->Lsq * Param->Lsq)) / Param->pp;
    Param_Norm->w_mech_MCurr_NVolt_id0_neg = -(sqrt((2 * Param->Lm * Param->Rs * Param->Isfn * Param->Isfn + 2 * Param->Psi_d * Param->Rs * Param->Isfn) * (2 * Param->Lm * Param->Rs * Param->Isfn * Param->Isfn + 2 * Param->Psi_d * Param->Rs * Param->Isfn) - (Param->Isfn * Param->Isfn * Param->Rs * Param->Rs - in->U_dc * 0.5 * in->U_dc * 0.5)*(4 * Param->Isfn * (2 * Param->Lm * Param->Psi_d + 2 * Param->Lsq * Param->Psi_q) + 4 * Param->Psi_d * Param->Psi_d + 4 * Param->Psi_q * Param->Psi_q + 4 * Param->Isfn * Param->Isfn * (Param->Lm * Param->Lm + Param->Lsq * Param->Lsq))) + 2 * Param->Isfn * Param->Psi_d * Param->Rs + 2 * Param->Isfn * Param->Isfn * Param->Lm * Param->Rs) / (2 * Param->Isfn * (2 * Param->Lm * Param->Psi_d + 2 * Param->Lsq * Param->Psi_q) + 2 * Param->Psi_d * Param->Psi_d + 2 * Param->Psi_q * Param->Psi_q + 2 * Param->Isfn * Param->Isfn * (Param->Lm * Param->Lm + Param->Lsq * Param->Lsq)) / Param->pp;

#else //(FFIS_PSI_Q == 1)
    Param_Norm->w_mech_MCurr_NVolt_id0 = -(2*Param->Psi_d*Param->Isfn*Param->Rs - sqrt((2*Param->Lm*Param->Rs*Param->Isfn*Param->Isfn + 2*Param->Psi_d*Param->Rs*Param->Isfn)*(2*Param->Lm*Param->Rs*Param->Isfn*Param->Isfn + 2*Param->Psi_d*Param->Rs*Param->Isfn) - (Param->Isfn*Param->Isfn*Param->Rs*Param->Rs - in->U_dc * 0.5*in->U_dc * 0.5)*(4*Param->Psi_d*Param->Psi_d + 4*Param->Isfn*Param->Isfn*(Param->Lm*Param->Lm + Param->Lsq*Param->Lsq) + 8*Param->Psi_d*Param->Isfn*Param->Lm)) + 2*Param->Isfn*Param->Isfn*Param->Lm*Param->Rs)/(2*Param->Psi_d*Param->Psi_d + 2*Param->Isfn*Param->Isfn*(Param->Lm*Param->Lm + Param->Lsq*Param->Lsq) + 4*Param->Psi_d*Param->Isfn*Param->Lm) / Param->pp;
    Param_Norm->w_mech_MCurr_NVolt_id0_neg = -(sqrt((2*Param->Lm*Param->Rs*Param->Isfn*Param->Isfn + 2*Param->Psi_d*Param->Rs*Param->Isfn)*(2*Param->Lm*Param->Rs*Param->Isfn*Param->Isfn + 2*Param->Psi_d*Param->Rs*Param->Isfn) - (Param->Isfn*Param->Isfn*Param->Rs*Param->Rs - in->U_dc * 0.5*in->U_dc * 0.5)*(4*Param->Psi_d*Param->Psi_d + 4*Param->Isfn*Param->Isfn*(Param->Lm*Param->Lm + Param->Lsq*Param->Lsq) + 8*Param->Psi_d*Param->Isfn*Param->Lm)) + 2*Param->Psi_d*Param->Isfn*Param->Rs + 2*Param->Isfn*Param->Isfn*Param->Lm*Param->Rs)/(2*Param->Psi_d*Param->Psi_d + 2*Param->Isfn*Param->Isfn*(Param->Lm*Param->Lm + Param->Lsq*Param->Lsq) + 4*Param->Psi_d*Param->Isfn*Param->Lm) / Param->pp;
#endif //(FFIS_PSI_Q == 1)



}

uint16_t FFIS_intersection(S_FFIS_Matrix_Q *A, S_FFIS_Matrix_Q *B, S_FFIS_Idq4s *i_out)
{
#define ZERO_TOL (9.0e-4)       //experimentally tuned for 32bit float

    S_FFIS_Matrix_Q M,D;
    float a0inv = 0;
    float b0inv = 0;
    static float adj_xi = 0;
    static float xi4 = 0;
    static float xi3N = 0,xi2N = 0,xi1N = 0,xi0N = 0;
    static float roots_xi[4];
    static float Isdq_2[4];
    int status_xi = 0;
    int it = 0;
    uint16_t translate = 0;
    float alfa_s, beta_s, alfa_s_inv, beta_s_inv;

    int sort4_i = 0;
    int sort4_j = 0;
    int tmp_sort = 0;

    // You can choose your own translation normalized values
    float xs1 = 0.5;        //case (iii) - Hackl_2017 (65)
    float xs2 = 0.3;


    if(( fabs(A->x0)>ZERO_TOL ) && ( fabs(B->x0)>ZERO_TOL ))
    {

        a0inv = 1.0 / A->x0;
        b0inv = 1.0 / B->x0;

        if(fabs(a0inv) > fabs(b0inv))
        {
            M = *B;
            D.x11 = A->x11 * a0inv - B->x11 * b0inv;
            D.x12 = A->x12 * a0inv - B->x12 * b0inv;
            D.x22 = A->x22 * a0inv - B->x22 * b0inv;

            D.x1 = A->x1 * a0inv - B->x1 * b0inv;
            D.x2 = A->x2 * a0inv - B->x2 * b0inv;
        }
        else
        {
            M = *A;
            D.x11 = A->x11 * a0inv - B->x11 * b0inv;
            D.x12 = A->x12 * a0inv - B->x12 * b0inv;
            D.x22 = A->x22 * a0inv - B->x22 * b0inv;

            D.x1 = A->x1 * a0inv - B->x1 * b0inv;
            D.x2 = A->x2 * a0inv - B->x2 * b0inv;

        }
    }
    else if(( fabs(A->x0)<ZERO_TOL ) && ( fabs(B->x0)>ZERO_TOL ))
    {
        D = *A;
        M = *B;
    }
    else if(( fabs(A->x0)>ZERO_TOL ) && ( fabs(B->x0)<ZERO_TOL ))
    {
        D = *B;
        M = *A;
    }
    else
    {
        alfa_s = 2*A->x1*xs1 + 2*A->x2*xs2 + xs1*(A->x11*xs1 + A->x12*xs2) + xs2*(A->x12*xs1 + A->x22*xs2); //alfa_s
        beta_s = 2*B->x1*xs1 + 2*B->x2*xs2 + xs1*(B->x11*xs1 + B->x12*xs2) + xs2*(B->x12*xs1 + B->x22*xs2); //beta_s
        alfa_s_inv = 1.0/alfa_s;
        beta_s_inv = 1.0/beta_s;


        D.x11 = A->x11 * alfa_s_inv - B->x11 * beta_s_inv;
        D.x12 = A->x12 * alfa_s_inv - B->x12 * beta_s_inv;
        D.x22 = A->x22 * alfa_s_inv - B->x22 * beta_s_inv;

        D.x1 = (A->x1 + A->x11*xs1 + A->x12*xs2) * alfa_s_inv - (B->x1 + B->x11*xs1 + B->x12*xs2) * beta_s_inv;
        D.x2 = (A->x2 + A->x12*xs1 + A->x22*xs2) * alfa_s_inv - (B->x2 + B->x12*xs1 + B->x22*xs2) * beta_s_inv;

        D.x0 = 0;


        M.x11 = B->x11;
        M.x12 = B->x12;
        M.x22 = B->x22;

        M.x1 = (B->x1 + B->x11*xs1 + B->x12*xs2);
        M.x2 = (B->x2 + B->x12*xs1 + B->x22*xs2);

        M.x0 = beta_s;

        translate = 1;
    }

    // Roots computation

    xi4 = 1/M.x0;

    xi3N = xi4 * (4*D.x2*M.x1 - 4*D.x1*M.x2) ;
    xi2N = xi4 * (4*D.x2*D.x2*M.x11 + 4*D.x1*D.x1*M.x22 - 2*D.x12*D.x12*M.x0 - 4*D.x1*D.x2*M.x12 + 4*D.x1*D.x12*M.x2 - 4*D.x2*D.x11*M.x2 + 4*D.x2*D.x12*M.x1 - 4*D.x1*D.x2*M.x12 - 4*D.x1*D.x22*M.x1 + 2*D.x11*D.x22*M.x0);
    xi1N = xi4 * (4*D.x1*D.x12*D.x12*M.x2 - 4*D.x2*D.x12*D.x12*M.x1 - 4*D.x2*D.x2*D.x11*M.x12 + 8*D.x2*D.x2*D.x12*M.x11 - 4*D.x2*D.x2*D.x11*M.x12 - 8*D.x1*D.x1*D.x12*M.x22 + 4*D.x1*D.x1*D.x22*M.x12 + 4*D.x1*D.x1*D.x22*M.x12 + 8*D.x1*D.x2*D.x11*M.x22 - 8*D.x1*D.x2*D.x22*M.x11 - 4*D.x1*D.x11*D.x22*M.x2 + 4*D.x2*D.x11*D.x22*M.x1);
    xi0N = xi4 * (D.x12*D.x12*D.x12*D.x12*M.x0 + 4*D.x2*D.x2*D.x12*D.x12*M.x11 + 4*D.x1*D.x1*D.x22*D.x22*M.x11 + 4*D.x1*D.x1*D.x12*D.x12*M.x22 + 4*D.x2*D.x2*D.x11*D.x11*M.x22 + D.x11*D.x11*D.x22*D.x22*M.x0 - 4*D.x1*D.x12*D.x12*D.x12*M.x2 - 4*D.x2*D.x12*D.x12*D.x12*M.x1 + 4*D.x1*D.x2*D.x12*D.x12*M.x12 + 4*D.x2*D.x11*D.x12*D.x12*M.x2 - 4*D.x1*D.x11*D.x22*D.x22*M.x1 + 4*D.x1*D.x2*D.x12*D.x12*M.x12 + 4*D.x1*D.x12*D.x12*D.x22*M.x1 - 4*D.x2*D.x11*D.x11*D.x22*M.x2 - 4*D.x2*D.x2*D.x11*D.x12*M.x12 - 4*D.x2*D.x2*D.x11*D.x12*M.x12 - 4*D.x1*D.x1*D.x12*D.x22*M.x12 - 4*D.x1*D.x1*D.x12*D.x22*M.x12 - 2*D.x11*D.x12*D.x12*D.x22*M.x0 - 8*D.x1*D.x2*D.x11*D.x12*M.x22 + 4*D.x1*D.x2*D.x11*D.x22*M.x12 - 8*D.x1*D.x2*D.x12*D.x22*M.x11 + 4*D.x1*D.x11*D.x12*D.x22*M.x2 + 4*D.x2*D.x11*D.x12*D.x22*M.x1 + 4*D.x1*D.x2*D.x11*D.x22*M.x12);

    status_xi = SolveP4(roots_xi, xi3N, xi2N, xi1N, xi0N);

    for(it = 0; it < status_xi ; it++)
    {
        adj_xi = 1/(roots_xi[it]*roots_xi[it] + D.x11*D.x22 - D.x12*D.x12);

        if(translate)
        {
            i_out->id[it] = -2*(adj_xi* (D.x22 * D.x1 + (-D.x12-roots_xi[it])*D.x2)) + xs1;
            i_out->iq[it] = -2*(adj_xi* ((roots_xi[it]-D.x12)* D.x1 + D.x11 * D.x2)) + xs2;
        }
        else
        {
            i_out->id[it] = -2*(adj_xi* (D.x22 * D.x1 + (-D.x12-roots_xi[it])*D.x2));
            i_out->iq[it] = -2*(adj_xi* ((roots_xi[it]-D.x12)* D.x1 + D.x11 * D.x2));
        }


        //Sorting

        Isdq_2[it] = i_out->id[it]*i_out->id[it] + 0.1 * i_out->iq[it]*i_out->iq[it];           //weighted

        if(Isdq_2[it] > 1.0e10)
        {

            i_out->sort[0] = 0;
            i_out->sort[1] = 1;
            i_out->sort[2] = 2;
            i_out->sort[3] = 3;
            return 0;               // No valid intersections found

        }

    }

    i_out->sort[0] = 0;
    i_out->sort[1] = 1;
    i_out->sort[2] = 2;
    i_out->sort[3] = 3;



    for(sort4_j=0;sort4_j<(status_xi);sort4_j++)
    {
        for(sort4_i=0;sort4_i<(status_xi - sort4_j - 1);sort4_i++)
        {
            if(Isdq_2[i_out->sort[sort4_i]] > Isdq_2[i_out->sort[sort4_i + 1]])
            {
                tmp_sort = i_out->sort[sort4_i + 1] ;
                i_out->sort[sort4_i + 1] = i_out->sort[sort4_i] ;
                i_out->sort[sort4_i]     = tmp_sort ;
            }
        }
    }





    return status_xi;
}

void FFIS_optimal_control_init(S_FFIS_Machine_Param *Param, S_FFIS_Machine_Param_Norm *Param_Norm, S_FFIS_Input *in)
{
    S_FFIS_Idq4s idq_init;
    int status_intersection = 0;
    float flux_d = 0;
    float flux_q = 0;


    FFIS_Normalize_Par(Param, Param_Norm, in);

    FFIS_matrixInit_Par(Param, Param_Norm);
    FFIS_matrixInit_M(Param, Param_Norm, in);
    status_intersection = FFIS_intersection(&FFIS_MTpC, &FFIS_MCurr, &idq_init);

    if(status_intersection >= 2)        //will be always 2 intersections id�iq;
    {
        flux_d = ((-Param->Psi_d) - idq_init.id[idq_init.sort[0]] * Param->Isfn*Param->Lsd) ;
        flux_q = idq_init.iq[idq_init.sort[0]] * Param->Isfn*Param->Lsq;

        Param_Norm->Flux_norm_sq = flux_d*flux_d + flux_q*flux_q;      //square of nominal flux linkage
        Param_Norm->Nom_flux_inv = 1/sqrt(Param_Norm->Flux_norm_sq);   //Inverted value of nominal flux linkage

#if(FFIS_CONST_PARAM)
        FFIS_MTPA_MCurr_d = idq_init.id[idq_init.sort[1]] * Param->Isfn;
        FFIS_MTPA_MCurr_q = fabs(idq_init.iq[idq_init.sort[1]]) * Param->Isfn;
#endif //(FFIS_CONST_PARAM)
    }

    FFIS_Normalize_M(Param, Param_Norm, in);

}





void FFIS_optimal_control(S_FFIS_Machine_Param *Param, S_FFIS_Machine_Param_Norm *Param_Norm, S_FFIS_Input *in, S_FFIS_Output *out)
{
    static float idm = 0;
    static float iqm = 0;
    static float Mmaxfeas = 0;
    static float Mintersect = 0;
    float Mintersect_pre = 0;
    float Mintersect_pre_second = 0;
    int mmsig_h;
    float id2 = 0;
    float iq2 = 0;
    float idq2_norm2 = 0;

#if(!FFIS_CONST_PARAM)
    S_FFIS_Idq4s idq_h_ac;
#endif //!FFIS_CONST_PARAM

    S_FFIS_Idq4s idq_h_vv;
    S_FFIS_Idq4s idq_h_av;
    S_FFIS_Idq4s idq_h_cv;
    S_FFIS_Idq4s idq_h;
    S_FFIS_Idq4s idq_h1;

    int16_t status_inter_at = 0;
    int16_t status_inter_vt = 0;

    float idm_ac = 0;
    float iqm_ac = 0;
    float idm_vv = 0;
    float iqm_vv = 0;
    float idm_av = 0;
    float iqm_av = 0;
    float idm_av2 = 0;
    float iqm_av2 = 0;
    float idm_cv = 0;
    float iqm_cv = 0;

    int16_t status_inter_vv = 0;
    int16_t status_inter_av = 0;
    int16_t status_inter_cv = 0;
    int16_t status_inter_ac = 0;

    float Flux_d_nom = 0;
    float Flux_q_nom = 0;

    float L_delta = Param->Lsd - Param->Lsq;


    // Re-normalize the (possibly time-varying) parameters and the current
    // operating point every cycle. This is essential: Normalize_Par is the
    // only place wk_N is derived from the commanded mechanical speed n_mech,
    // so without this call the controller would keep reusing the speed that
    // was set at init() time and never react to speed (or parameter) changes.
    // Updating coefficients online is exactly the contribution of the paper
    // (cf. "coefficients update" in its Table III).
    FFIS_Normalize_Par(Param, Param_Norm, in);

    FFIS_Normalize_M(Param, Param_Norm, in);

    FFIS_matrixInit_Par(Param, Param_Norm);
    FFIS_matrixInit_M(Param, Param_Norm, in);



    if(in->mmref > 0)       //polarity of requested torque
    {
        mmsig_h = 1;
    }
    else //if(in->mmref <= 0)
    {
        mmsig_h = -1;
    }
    //    else
    //    {
    //        mmsig_h = 0;
    //    }


#if(FFIS_CONST_PARAM)
    idm_ac = FFIS_MTPA_MCurr_d;
if(mmsig_h > 0)
{
    iqm_ac = FFIS_MTPA_MCurr_q;
}
else
{
    iqm_ac = -FFIS_MTPA_MCurr_q;
}

#else //FFIS_CONST_PARAM

status_inter_ac = FFIS_intersection(&FFIS_MTpC, &FFIS_MCurr, &idq_h_ac);

if(status_inter_ac >=2)
{
    idm_ac = idq_h_ac.id[idq_h_ac.sort[0]] * Param->Isfn;
    iqm_ac = idq_h_ac.iq[idq_h_ac.sort[0]] * Param->Isfn;

    if(( (iqm_ac < 0) ? -1 : (iqm_ac >= 0) ) == (-mmsig_h))     // the mmref and iq should have the same polarity
    {
        idm_ac = idq_h_ac.id[idq_h_ac.sort[1]] * Param->Isfn;
        iqm_ac = idq_h_ac.iq[idq_h_ac.sort[1]] * Param->Isfn;
    }
}
else
{
    idm_ac = 0;
    iqm_ac = 0;
}

#endif //FFIS_CONST_PARAM

#if (FFIS_PSI_Q == 1)
Flux_d_nom = ((-Param->Psi_d) - idm_ac * Param->Isfn*Param->Lsd);
Flux_q_nom = Param->Psi_q + iqm_ac * Param->Isfn*Param->Lsq;
#else //(FFIS_PSI_Q == 1)
Flux_d_nom = ((-Param->Psi_d) - idm_ac * Param->Isfn*Param->Lsd) ;
Flux_q_nom = iqm_ac * Param->Isfn*Param->Lsq;
#endif //(FFIS_PSI_Q == 1)

Param_Norm->Flux_norm_sq = Flux_d_nom*Flux_d_nom + Flux_q_nom*Flux_q_nom;
Param_Norm->Nom_flux_inv = 1 / sqrt(Param_Norm->Flux_norm_sq);

status_inter_vv = FFIS_intersection(&FFIS_MTpV,&FFIS_NVolt, &idq_h_vv);

if(status_inter_vv >=2)         //always >=2
        {
    idm_vv = idq_h_vv.id[idq_h_vv.sort[0]] * Param->Isfn;
    iqm_vv = idq_h_vv.iq[idq_h_vv.sort[0]] * Param->Isfn;

    if(  ( (iqm_vv < 0) ? -1 : (iqm_vv >= 0) ) == (-mmsig_h))       // the mmref and iq should have the same polarity
    {
        idm_vv = idq_h_vv.id[idq_h_vv.sort[1]] * Param->Isfn;
        iqm_vv = idq_h_vv.iq[idq_h_vv.sort[1]] * Param->Isfn;
    }

    if(  ( (iqm_vv < 0) ? -1 : (iqm_vv >= 0) ) == (-mmsig_h) )      // the mmref and iq should have the same polarity
    {
        idm_vv = idq_h_vv.id[idq_h_vv.sort[2]] * Param->Isfn;
        iqm_vv = idq_h_vv.iq[idq_h_vv.sort[2]] * Param->Isfn;
    }

    if(  ( (iqm_vv < 0) ? -1 : (iqm_vv >= 0) ) == (-mmsig_h))       // the mmref and iq should have the same polarity
    {
        idm_vv = idq_h_vv.id[idq_h_vv.sort[3]] * Param->Isfn;
        iqm_vv = idq_h_vv.iq[idq_h_vv.sort[3]] * Param->Isfn;
    }
        }
else
{
    idm_vv = 0;
    iqm_vv = 0;

    //TODO status - something wrong
}

status_inter_av = FFIS_intersection(&FFIS_MTpC,&FFIS_NVolt, &idq_h_av);

if(status_inter_av >=2)
{
    idm_av = idq_h_av.id[idq_h_av.sort[0]] * Param->Isfn;
    iqm_av = idq_h_av.iq[idq_h_av.sort[0]] * Param->Isfn;

    idm_av2 = idq_h_av.id[idq_h_av.sort[1]] * Param->Isfn;
    iqm_av2 = idq_h_av.iq[idq_h_av.sort[1]] * Param->Isfn;

#if (FFIS_PSI_Q == 1)
    Mintersect_pre =         1.5 * Param->pp * ((L_delta*idm_av + Param->Psi_d)*iqm_av + Param->Lm * (iqm_av*iqm_av - idm_av*idm_av) - Param->Psi_q*idm_av);
    Mintersect_pre_second =  1.5 * Param->pp * ((L_delta*idm_av2 + Param->Psi_d)*iqm_av2 + Param->Lm * ( iqm_av2 * iqm_av2 - idm_av2 * idm_av2) - Param->Psi_q * idm_av2);
#else //(FFIS_PSI_Q == 1)
    Mintersect_pre = 1.5 * Param->pp * ((L_delta*idm_av + Param->Psi_d)*iqm_av + Param->Lm * (iqm_av*iqm_av - idm_av*idm_av));
    Mintersect_pre_second =  1.5 * Param->pp * ((L_delta*idm_av2 + Param->Psi_d)*iqm_av2 + Param->Lm * (iqm_av2 * iqm_av2 - idm_av2 * idm_av2));
#endif //(FFIS_PSI_Q == 1)

    if(mmsig_h > 0)
    {
        if((iqm_av < -0.01) || ((fabs( Mintersect_pre_second ) >  fabs(Mintersect_pre) ) &&  ( (iqm_av2 > -0.01))))     //small numerical tolerance, use the setpoint that giving higher torque
        {
            idm_av = idm_av2;
            iqm_av = iqm_av2;
        }
    }
    else
    {
        if((iqm_av > 0.01) || ((fabs( Mintersect_pre_second ) >  fabs(Mintersect_pre) ) &&  ( (iqm_av2 < 0.01))))
        {
            idm_av = idm_av2;
            iqm_av = iqm_av2;
        }
    }

}
else
{
    idm_av = 0;
    iqm_av = 0;
}

status_inter_cv = FFIS_intersection(&FFIS_MCurr,&FFIS_NVolt, &idq_h_cv);

if(status_inter_cv >=2)
{
    idm_cv = idq_h_cv.id[idq_h_cv.sort[0]] * Param->Isfn;
    iqm_cv = idq_h_cv.iq[idq_h_cv.sort[0]] * Param->Isfn;

    if(( (iqm_cv < 0) ? -1 : (iqm_cv >= 0) ) == (-mmsig_h))
    {
        idm_cv = idq_h_cv.id[idq_h_cv.sort[1]] * Param->Isfn;
        iqm_cv = idq_h_cv.iq[idq_h_cv.sort[1]] * Param->Isfn;
    }

}
else
{

    if((in->n_mech * RPM_TO_RAD_S) > 0.97 * Param_Norm->w_mech_MCurr_NVolt_id0 && (in->n_mech * RPM_TO_RAD_S) < 1.03 * Param_Norm->w_mech_MCurr_NVolt_id0 )     //
    {
        status_inter_cv = 3;
        idm_cv = 0;
        if(mmsig_h == 1)
        {
            iqm_cv = Param->Isfn;
        }
        else
        {
            iqm_cv = -Param->Isfn;
        }
    }

    if(((in->n_mech * RPM_TO_RAD_S) < 0.97 * Param_Norm->w_mech_MCurr_NVolt_id0_neg && (in->n_mech * RPM_TO_RAD_S) > 1.03 * Param_Norm->w_mech_MCurr_NVolt_id0_neg ))
    {
        status_inter_cv = 3;
        idm_cv = 0;
        if(mmsig_h == -1)
        {
            iqm_cv = -Param->Isfn;
        }
        else
        {
            iqm_cv = Param->Isfn;
        }

    }

    if(status_inter_cv == 0)
    {
        idm_cv = 0;
        iqm_cv = 0;
    }
}

if(  ((idm_vv*idm_vv + iqm_vv*iqm_vv) > (Param->Isfn*Param->Isfn))  )        // if MTPV & Voltage ellipse intersection is inside the current circle -> MTPV
{
#if (FFIS_PSI_Q == 1)
    if(((( ((-Param->Psi_d) - idm_cv*Param->Lsd)*((-Param->Psi_d) - idm_cv*Param->Lsd)+ ((Param->Psi_q + iqm_cv*Param->Lsq)*(Param->Psi_q + iqm_cv*Param->Lsq))) > Param_Norm->Flux_norm_sq) || (status_inter_cv < 2))  )   //fluxnorm
    {
        idm = idm_ac;
        iqm = iqm_ac;
    }
    else
    {
        idm = idm_cv;
        iqm = iqm_cv;
    }
#else //(FFIS_PSI_Q == 1)
    if(((( ((-Param->Psi_d) - idm_cv*Param->Lsd)*((-Param->Psi_d) - idm_cv*Param->Lsd)+ (iqm_cv*Param->Lsq*iqm_cv*Param->Lsq)) > Param_Norm->Flux_norm_sq) || (status_inter_cv < 2))  )   //fluxnorm
    {
        idm = idm_ac;
        iqm = iqm_ac;
    }
    else
    {
        idm = idm_cv;
        iqm = iqm_cv;
    }
#endif //(FFIS_PSI_Q == 1)



}
else if(status_inter_vv >=2)
{
    idm = idm_vv;
    iqm = iqm_vv;

}
else
{
    //error state
    idm = 0;
    iqm = 0;
}




#if (FFIS_PSI_Q == 1)
    Mmaxfeas = 1.5 * Param->pp * ((L_delta*idm + Param->Psi_d)*iqm + Param->Lm * (iqm * iqm - idm * idm) - Param->Psi_q*idm);
#else //(FFIS_PSI_Q == 1)
    Mmaxfeas = 1.5 * Param->pp * ((L_delta*idm + Param->Psi_d)*iqm + Param->Lm * (iqm * iqm - idm * idm));
#endif //(FFIS_PSI_Q == 1)

if(status_inter_av >= 2)                                                //get Mintersect
{


#if (FFIS_PSI_Q == 1)
    Mintersect = 1.5 * Param->pp * ((L_delta*idm_av + Param->Psi_d)*iqm_av + Param->Lm * (iqm_av * iqm_av - idm_av * idm_av) - Param->Psi_q*idm_av);
#else //(FFIS_PSI_Q == 1)
    Mintersect = 1.5 * Param->pp * ((L_delta*idm_av + Param->Psi_d)*iqm_av + Param->Lm * (iqm_av * iqm_av - idm_av * idm_av));
#endif //(FFIS_PSI_Q == 1)

    if(fabs(Mintersect) > fabs(Mmaxfeas))
    {
        Mintersect = Mmaxfeas;
    }
}
else
{
    Mintersect = 0;

}


if(fabs(in->mmref) < fabs(Mintersect))
{
    status_inter_at = FFIS_intersection(&FFIS_NTorq, &FFIS_MTpC,&idq_h);

    if(status_inter_at >=2)
    {
        out->Isdw = idq_h.id[idq_h.sort[0]] * Param->Isfn;
        out->Isqw = idq_h.iq[idq_h.sort[0]] * Param->Isfn;

        id2 = idq_h.id[idq_h.sort[1]] * Param->Isfn;
        iq2 = idq_h.iq[idq_h.sort[1]] * Param->Isfn;

        idq2_norm2 = id2 * id2 + iq2 * iq2;

        if(mmsig_h > 0)
        {
            if((out->Isqw < -0.1) && (idq2_norm2 < (1.1*Param->Isfn*Param->Isfn)))
            {
                out->Isdw = id2;
                out->Isqw = iq2;
            }
        }
        else
        {
            if((out->Isqw > 0.1) && (idq2_norm2 < (1.1*Param->Isfn*Param->Isfn)))
            {
                out->Isdw = id2;
                out->Isqw = iq2;
            }
        }

    }
    else
    {
        //error state
        out->Isdw = 0;
        out->Isqw = 0;

    }


}
else if((fabs(in->mmref)*1.001) < fabs(Mmaxfeas))    // +0.1% tolerance
{
    status_inter_vt = FFIS_intersection(&FFIS_NTorq, &FFIS_NVolt, &idq_h1);


    if(status_inter_vt >=2)
    {
        out->Isdw = idq_h1.id[idq_h1.sort[0]] * Param->Isfn;
        out->Isqw = idq_h1.iq[idq_h1.sort[0]] * Param->Isfn;

        id2 = idq_h1.id[idq_h1.sort[1]] * Param->Isfn;
        iq2 = idq_h1.iq[idq_h1.sort[1]] * Param->Isfn;

        idq2_norm2 = id2 * id2 + iq2 * iq2;
        /*
                  if(( (out->Isqw < -0.05) ? -1 : (out->Isqw >= 0.05) ) == (-mmsig_h))
                  {
                      out->Isdw = idq_h1.id[idq_h1.sort[1]] * Param->Isfn;
                      out->Isqw = idq_h1.iq[idq_h1.sort[1]] * Param->Isfn;
                  }
         */
        if(mmsig_h > 0)
        {
            if((out->Isqw < -0.5) && (idq2_norm2 < (1.1*Param->Isfn*Param->Isfn)))
            {
                out->Isdw = id2;
                out->Isqw = iq2;
            }
        }
        else
        {
            if((out->Isqw > 0.5) && (idq2_norm2 < (1.1*Param->Isfn*Param->Isfn)))
            {
                out->Isdw = id2;
                out->Isqw = iq2;
            }
        }
    }
    else
    {
        out->Isdw = 0;
        out->Isqw = 0;
    }
}
else
{
    out->Isdw = idm;
    out->Isqw = iqm;

}


// Zero torque request & zero speed -> zero current reference
if(fabs(in->mmref) < 1e-5 && fabs((in->n_mech * RPM_TO_RAD_S)) < 1e-4)
{
    out->Isdw = 0;
    out->Isqw = 0;

}

// Safety fallback: never emit a non-finite setpoint. In single precision an
// extreme or degenerate operating point (e.g. near-singular inductances at
// very high speed) can overflow the quartic coefficients and yield NaN/Inf.
// A drive must never be commanded a non-finite current, so fall back to a
// zero reference. Within the rated operating envelope this never triggers;
// it is a guard for out-of-range / fuzzed inputs.
if(!isfinite(out->Isdw) || !isfinite(out->Isqw))
{
    out->Isdw = 0;
    out->Isqw = 0;
}


}


void FFIS_Normalize_Par(S_FFIS_Machine_Param *Param, S_FFIS_Machine_Param_Norm *Param_Norm, S_FFIS_Input *in)
{
    Param_Norm->wk_nom = Param->pp * RPM_TO_RAD_S * Param->rpm_mech_n;

    Param_Norm->Rs_N = Param->Rs / (Param->Usfn / Param->Isfn);
    Param_Norm->Lsd_N = Param_Norm->wk_nom * Param->Lsd / (Param->Usfn / Param->Isfn);
    Param_Norm->Lsq_N = Param_Norm->wk_nom * Param->Lsq / (Param->Usfn / Param->Isfn);
    Param_Norm->Lm_N = Param_Norm->wk_nom * Param->Lm / (Param->Usfn / Param->Isfn);

    Param_Norm->Psi_d_N = Param_Norm->wk_nom * Param->Psi_d / Param->Usfn;
    Param_Norm->Psi_q_N = Param_Norm->wk_nom * Param->Psi_q / Param->Usfn;

    Param_Norm->wk_N = Param->pp * RPM_TO_RAD_S * in->n_mech / Param_Norm->wk_nom;

    Param_Norm->Ism_N = Param->Isfn / Param->Isfn;

}

void FFIS_Normalize_M(S_FFIS_Machine_Param *Param, S_FFIS_Machine_Param_Norm *Param_Norm, S_FFIS_Input *in)
{

    if(FFIS_THIRD_HARMONIC_INJ)
    {
        Param_Norm->Usf_N = FFIS_Safety_V * in->U_dc * 0.57735 / Param->Usfn;        //2-level VSI  with 3rd harmonic injection
    }
    else
    {
        Param_Norm->Usf_N = FFIS_Safety_V * in->U_dc * 0.5 / Param->Usfn;        //2-level VSI  without 3rd harmonic  injection
    }



    // NOTE: the original embedded source had a stray ';' after this 'if',
    // which made the low-speed flux/voltage clamp run unconditionally. The
    // semicolon is removed here so the clamp is applied only below the
    // x-rated threshold speed, matching the method described in the paper
    // (Sec. IV-B, omega_min = 0.8 * omega_N).
    if(fabs((Param_Norm->wk_N * Param_Norm->wk_nom)/ (Param_Norm->Usf_N * Param->Usfn)) < (FFIS_X_RATED_NUMERIC_TOL * Param_Norm->Nom_flux_inv))
    {
        if(Param_Norm->wk_N < 0)
        {
            Param_Norm->wk_N = -FFIS_X_RATED_NUMERIC_TOL * Param_Norm->Nom_flux_inv * (Param_Norm->Usf_N * Param->Usfn) / Param_Norm->wk_nom;
        }
        else
        {
            Param_Norm->wk_N = FFIS_X_RATED_NUMERIC_TOL * Param_Norm->Nom_flux_inv * (Param_Norm->Usf_N * Param->Usfn) / Param_Norm->wk_nom;
        }


    }

}