/*
 * uf_alg_ing.c
 *
 *  Created on: 10 февр. 2020 г.
 *      Author: yura
 */



#include "IQmathLib.h"
#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "DSP281x_Device.h"     // DSP281x Headerfile Include File



#include "uf_alg_ing.h"

#include <master_slave.h>
#include <params_alg.h>
#include <params_motor.h>
#include <params_norma.h>
#include <params_pwm24.h>
#include <v_pwm24_v2.h>

#include "math_pi.h"
#include "svgen_dq.h"
#include "svgen_mf.h"
#include "dq_to_alphabeta_cos.h"



//#pragma DATA_SECTION(svgen_mf1,".fast_vars");
SVGENMF svgen_mf1 = SVGENMF_DEFAULTS;

//#pragma DATA_SECTION(svgen_mf2,".fast_vars");
//SVGENMF svgen_mf2 = SVGENMF_DEFAULTS;


//#pragma DATA_SECTION(svgen_dq_1,".fast_vars");
SVGENDQ svgen_dq_1 = SVGENDQ_DEFAULTS;
//#pragma DATA_SECTION(svgen_dq_2,".fast_vars");
//SVGENDQ svgen_dq_2 = SVGENDQ_DEFAULTS;


UF_ALG_VALUE uf_alg = UF_ALG_VALUE_DEFAULT;

void InitVariablesSvgen_Ing(unsigned int freq)
{



    svgen_dq_1.Ualpha = 0;
    svgen_dq_1.Ubeta = 0;

 //   svgen_dq_2.Ualpha = 0;
 //   svgen_dq_2.Ubeta = 0;

    uf_alg.hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / freq ); // один раз за такт ШИМа
//    uf_alg.hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / freq /2.0 ); // два раза за такт ШИМа




// Initialize the SVGEN_MF module
//    svgen_mf1.FreqMax = _IQ(6*NORMA_FROTOR/freq);
//    svgen_mf2.FreqMax = _IQ(6*NORMA_FROTOR/freq);
//
//
//    svgen_mf2.Offset=_IQ(0);
//    svgen_mf1.Offset=_IQ(0);

    init_alpha_Ing(0);


}








void init_alpha_Ing(unsigned int master_slave)
{


    uf_alg.winding_displacement_bs1 = 0;
    uf_alg.winding_displacement_bs2 = 0;


//  power_ain1.init(&power_ain1);
//  power_ain2.init(&power_ain2);

//    svgen_mf1.NewEntry = 0;//_IQ(0.5);
//    svgen_mf2.NewEntry = 0;

//    svgen_mf1.SectorPointer = 0;
//    svgen_mf2.SectorPointer = 0;

//сдвиг по умолчанию 0 градусов
//    svgen_mf1.Alpha = _IQ(0);
//    svgen_mf2.Alpha = _IQ(0);

//
//
//

#if (SETUP_SDVIG_OBMOTKI == SDVIG_OBMOTKI_30_PLUS)
// 30 град. сдвиг
    // тут не радианы, а нормированные к 60 град=1.
//    svgen_mf1.Alpha = _IQ(0.5);
//    svgen_mf2.Alpha = _IQ(0);
//
//    svgen_mf1.Full_Alpha = svgen_mf1.Alpha;
//    svgen_mf2.Full_Alpha = svgen_mf2.Alpha;

    // тут радианы
    uf_alg.winding_displacement_bs1 = CONST_IQ_PI6; //_IQ(0.5);
    uf_alg.winding_displacement_bs2 = 0;

#else


#if (SETUP_SDVIG_OBMOTKI == SDVIG_OBMOTKI_30_MINUS)
// -30 град. сдвиг
    // тут не радианы, а нормированные к 60 град=1.
//    svgen_mf1.Alpha = _IQ(0);
//    svgen_mf2.Alpha = _IQ(0.5);
//    svgen_mf1.Full_Alpha = svgen_mf1.Alpha;
//    svgen_mf2.Full_Alpha = svgen_mf2.Alpha;
//
    // тут радианы
    uf_alg.winding_displacement_bs2 = CONST_IQ_PI6; // _IQ(0.5);
    uf_alg.winding_displacement_bs1 = 0;

#else

#if (SETUP_SDVIG_OBMOTKI ==  SDVIG_OBMOTKI_ZERO)
// 0 град. сдвиг
//    svgen_mf1.Alpha = _IQ(0);
//    svgen_mf2.Alpha = _IQ(0);
//    svgen_mf1.Full_Alpha = svgen_mf1.Alpha;
//    svgen_mf2.Full_Alpha = svgen_mf2.Alpha;

    uf_alg.winding_displacement_bs1 = 0;
    uf_alg.winding_displacement_bs2 = 0;

#else
   #error "!!!ОШИБКА!!! Не определен SETUP_SDVIG_OBMOTKI в params_motor.h!!!"


#endif

#endif

#endif




}

////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
#pragma CODE_SECTION(uf_disbalance_uzpt,".fast_run");
void uf_disbalance_uzpt(_iq Uzad_uf1, unsigned int disable_alg_u_disbalance, _iq kplus_u_disbalance, _iq k_u_disbalance,
                        _iq U_1, _iq U_2,
                        _iq Uzad_max,
                        _iq *Kplus_out)
{
    _iq pwm_t,delta_U,Uplus,Uminus;
    volatile _iq Kplus;
    static _iq u1=0,u2=0;

    volatile _iq KplusMax;


//    change_pwm_freq(Fzad_uf);


    Uplus  = U_2;//filter.iqU_2_fast; // тут Uplus берет от U2 т.к. тут знак "-" скорее всего не учитываетЯ где-то
    Uminus = U_1;//filter.iqU_1_fast;

    delta_U = Uplus - Uminus;

    if (disable_alg_u_disbalance==0)
    {
        if (kplus_u_disbalance!=0) // если мы задали извне, то его и используем, это для теста.
        {
            Kplus  = kplus_u_disbalance;
        }
        else
        {
         if (delta_U != 0)
         {
            Kplus  =  _IQmpy(k_u_disbalance, _IQmpy(Uzad_uf1, (_IQdiv( (Uplus-Uminus), (Uplus+Uminus) ))  )  );//CONST_1 + _IQmpy(k_u_disbalance,    _IQmpy(Uzad_uf1,  ( _IQdiv(_IQmpy(CONST_2,Uplus),(Uplus+Uminus))  - CONST_1 )  )   );
         }
         else
         {
            Kplus  = 0;
         }
        }
    }
    else
    {
//      Uplus  = 0;
//      Uminus = 0;
//      delta_U = 0;
        Kplus  = 0;
    }

    KplusMax = _IQmpy(Uzad_uf1,CONST_IQ_05);

    if (Kplus>=KplusMax)
    {
      Kplus  = KplusMax;
    }

    if (Kplus<=-KplusMax)
    {
      Kplus  = -KplusMax;
    }


    *Kplus_out =  Kplus;


}
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////




////////////////////////////////////////////////////////////////////////////
// kplus_u_disbalance должен быть равен = 0, если не ноль, то это тест
// enable_alg_u_disbalance - разрешить работу алгоритма разбаланса
// k_u_disbalance - коэф. силы алгоритма разбаланса.
////////////////////////////////////////////////////////////////////////////
//#pragma CODE_SECTION(uf_const_f_24_Ing,".fast_run");
void uf_const_f_24_Ing(_iq Fzad_uf1,_iq Fzad_uf2,_iq Uzad_uf1, _iq Uzad_uf2,
                   unsigned int enable_alg_u_disbalance, _iq kplus_u_disbalance, _iq k_u_disbalance,
                   _iq U_1, _iq U_2, unsigned int flag_km_0,
                   _iq Uzad_max,
                   _iq *Kplus_out)
{

    _iq pwm_t,delta_U,Uplus,Uminus;
    _iq Kplus;
    static _iq u1=0,u2=0;

    volatile _iq KplusMax;


    uf_disbalance_uzpt(Uzad_uf1, enable_alg_u_disbalance,  kplus_u_disbalance,  k_u_disbalance, U_1, U_2, Uzad_max, &Kplus);
//    change_pwm_freq(Fzad_uf);

    *Kplus_out =  Kplus;


/////////////////////////////////////////
    svgen_mf1.Gain = _IQsat(Uzad_uf1,Uzad_max,0);              // Pass inputs to svgen_mf1
    svgen_mf1.Freq = Fzad_uf1;              // Pass inputs to svgen_mf1

//    svgen_mf2.Gain =  _IQsat(Uzad_uf2,Uzad_max,0);;              // Pass inputs to svgen_mf1
//    svgen_mf2.Freq = Fzad_uf2;              // Pass inputs to svgen_mf1

    svgen_mf1.calc(&svgen_mf1);    // Call compute function for svgen_mf1
 //   svgen_mf2.calc(&svgen_mf2);    // Call compute function for svgen_mf2
    /////////////////////////////////////////








  /*
    logpar.log1 = (int16)(_IQtoIQ15(Uzad_uf1));
    logpar.log2 = (int16)(_IQtoIQ15(Fzad_uf1));
    logpar.log3 = (int16)((svgen_pwm24_1.Ta_0));
    logpar.log4 = (int16)((svgen_pwm24_1.Ta_1));
    logpar.log5 = (int16)(_IQtoIQ15(svgen_mf1.Ta));
    logpar.log6 = (int16)(_IQtoIQ15(_IQdiv(Kplus,CONST_IQ_10)));
    logpar.log7 = (int16)(_IQtoIQ15(_IQdiv(Kminus,CONST_IQ_10)));
    logpar.log8 = (int16)(_IQtoIQ15(Uplus));
    logpar.log9 = (int16)(_IQtoIQ15(Uminus));

*/
// 1
// a
    pwm_t = correct_balance_uzpt_pwm24 (svgen_mf1.Ta, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_1, &svgen_pwm24_1.Ta_0, &svgen_pwm24_1.Ta_1, &svgen_pwm24_1.Ta_imp, pwm_t);
// b
    pwm_t = correct_balance_uzpt_pwm24 (svgen_mf1.Tb, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_1, &svgen_pwm24_1.Tb_0, &svgen_pwm24_1.Tb_1, &svgen_pwm24_1.Tb_imp,pwm_t);
// c
    pwm_t = correct_balance_uzpt_pwm24 (svgen_mf1.Tc, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_1, &svgen_pwm24_1.Tc_0, &svgen_pwm24_1.Tc_1, &svgen_pwm24_1.Tc_imp,pwm_t);

// 2
    svgen_pwm24_2.Ta_0 = svgen_pwm24_1.Ta_0;
    svgen_pwm24_2.Ta_1 = svgen_pwm24_1.Ta_1;
    svgen_pwm24_2.Tb_0 = svgen_pwm24_1.Tb_0;
    svgen_pwm24_2.Tb_1 = svgen_pwm24_1.Tb_1;
    svgen_pwm24_2.Tc_0 = svgen_pwm24_1.Tc_0;
    svgen_pwm24_2.Tc_1 = svgen_pwm24_1.Tc_1;

// a
//    pwm_t = correct_balance_uzpt_pwm24 (svgen_mf1.Ta, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_2, &svgen_pwm24_2.Ta_0, &svgen_pwm24_2.Ta_1, &svgen_pwm24_2.Ta_imp,pwm_t);
//// b
//    pwm_t = correct_balance_uzpt_pwm24 (svgen_mf1.Tb, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_2, &svgen_pwm24_2.Tb_0, &svgen_pwm24_2.Tb_1, &svgen_pwm24_2.Tb_imp,pwm_t);
//// c2
//    pwm_t = correct_balance_uzpt_pwm24 (svgen_mf1.Tc, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_2, &svgen_pwm24_2.Tc_0, &svgen_pwm24_2.Tc_1, &svgen_pwm24_2.Tc_imp,pwm_t);
//

////
    if (flag_km_0)
    {
     svgen_pwm24_1.prev_level = V_PWM24_PREV_PWM_WORK_KM0;
     svgen_pwm24_2.prev_level = V_PWM24_PREV_PWM_WORK_KM0;
    }
    else
    {
     svgen_pwm24_1.prev_level = V_PWM24_PREV_PWM_WORK;
     svgen_pwm24_2.prev_level = V_PWM24_PREV_PWM_WORK;
    }



/////////
/////////

//   logpar.log10 = (int16)((svgen_pwm24_1.Ta_0));
//    logpar.log11 = (int16)((svgen_pwm24_1.Ta_1));



}


////////////////////////////////////////////////////////////
//#pragma CODE_SECTION(test_calc_simple_dq_pwm24_Ing,".v_24pwm_run");
void test_calc_simple_dq_pwm24_Ing(_iq Fzad_uf1,_iq Uzad_uf1,
                                   unsigned int disable_alg_u_disbalance, _iq kplus_u_disbalance, _iq k_u_disbalance,
                                   _iq U_1, _iq U_2, unsigned int flag_km_0,
                                   _iq Uzad_max,
                                   _iq master_tetta,
                                   _iq master_Uzad_uf1,
                                   unsigned int master,
                                   unsigned int n_bs,
                                   _iq *Kplus_out,
                                   _iq *tetta_out,
                                   _iq *Uzad_out
)
{
//    static _iq hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM / 2.0);
//  static _iq tetta = 0;
    _iq pwm_t;
    _iq Kplus;
    _iq Ud = 0;
    _iq Uq = 0;
    _iq add_tetta = 0;

    DQ_TO_ALPHABETA dq_to_ab = DQ_TO_ALPHABETA_DEFAULTS;

    ////////////////////////////////////////

    uf_disbalance_uzpt(Uzad_uf1, disable_alg_u_disbalance,  kplus_u_disbalance,  k_u_disbalance,
                       U_1, U_2,
                       Uzad_max,
                       &Kplus);


    ////////////////////////////////////////


    ////////////////////////////////////////
    ////////////////////////////////////////
    if (master==MODE_MASTER)
    {
      // мы master
      add_tetta = _IQmpy(Fzad_uf1, uf_alg.hz_to_angle);
      uf_alg.tetta += add_tetta;
      Ud = 0;
      Uq = _IQsat(Uzad_uf1,Uzad_max,0);
    }
    else
    if (master==MODE_SLAVE)
    {
        // мы slave
        add_tetta = 0;
        uf_alg.tetta = master_tetta;
        Ud = 0;
        Uq = _IQsat(master_Uzad_uf1,Uzad_max,0);
    }
    else
    {
        // мы непонятно кто!
        Ud = 0;
        Uq = 0;
        add_tetta = 0;
        uf_alg.tetta = 0;
    }
    ////////////////////////////////////////
    ////////////////////////////////////////



    uf_alg.Ud = Ud;
    uf_alg.Uq = Uq;

    if (uf_alg.tetta > CONST_IQ_2PI)
    {
        uf_alg.tetta -= CONST_IQ_2PI;
    }

    if (uf_alg.tetta < 0)
    {
        uf_alg.tetta += CONST_IQ_2PI;
    }

    if (n_bs==0)
        uf_alg.tetta_bs = uf_alg.tetta + uf_alg.winding_displacement_bs1;
    else
        uf_alg.tetta_bs = uf_alg.tetta + uf_alg.winding_displacement_bs2;

    dq_to_ab.Tetta = uf_alg.tetta_bs;
    ////////////////////////////////////////
    ////////////////////////////////////////

    dq_to_ab.Ud = Ud;
    dq_to_ab.Uq = Uq;
    dq_to_ab.calc2(&dq_to_ab);

    svgen_dq_1.Ualpha = dq_to_ab.Ualpha;
    svgen_dq_1.Ubeta = dq_to_ab.Ubeta;
    ////////////////////////////////////////

    uf_alg.Ualpha = dq_to_ab.Ualpha;
    uf_alg.Ubeta = dq_to_ab.Ubeta;
    ////////////////////////////////////////


//    svgen_dq_1.Ualpha = 0;
//    svgen_dq_1.Ubeta = 0;

    svgen_dq_1.calc(&svgen_dq_1);

    uf_alg.svgen_dq_Ta = svgen_dq_1.Ta;
    uf_alg.svgen_dq_Tb = svgen_dq_1.Tb;
    uf_alg.svgen_dq_Tc = svgen_dq_1.Tc;
    ////////////////////////////////////////


//    dq_to_ab.Tetta = uf_alg.tetta;
//    dq_to_ab.Ud = Ud;
//    dq_to_ab.Uq = Uq;
//    dq_to_ab.calc2(&dq_to_ab);
//
//    svgen_dq_2.Ualpha = dq_to_ab.Ualpha;
//    svgen_dq_2.Ubeta = dq_to_ab.Ubeta;
//
//    svgen_dq_2.calc(&svgen_dq_2);

// 1
// a
    pwm_t = correct_balance_uzpt_pwm24 (svgen_dq_1.Ta, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_1, &svgen_pwm24_1.Ta_0, &svgen_pwm24_1.Ta_1, &svgen_pwm24_1.Ta_imp, pwm_t);
// b
    pwm_t = correct_balance_uzpt_pwm24 (svgen_dq_1.Tb, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_1, &svgen_pwm24_1.Tb_0, &svgen_pwm24_1.Tb_1, &svgen_pwm24_1.Tb_imp,pwm_t);
// c
    pwm_t = correct_balance_uzpt_pwm24 (svgen_dq_1.Tc, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_1, &svgen_pwm24_1.Tc_0, &svgen_pwm24_1.Tc_1, &svgen_pwm24_1.Tc_imp,pwm_t);

// 2 аналогично 1 т.к. тут парал. работа в Ingeteam
    svgen_pwm24_2.Ta_0 = svgen_pwm24_1.Ta_0;
    svgen_pwm24_2.Ta_1 = svgen_pwm24_1.Ta_1;
    svgen_pwm24_2.Tb_0 = svgen_pwm24_1.Tb_0;
    svgen_pwm24_2.Tb_1 = svgen_pwm24_1.Tb_1;
    svgen_pwm24_2.Tc_0 = svgen_pwm24_1.Tc_0;
    svgen_pwm24_2.Tc_1 = svgen_pwm24_1.Tc_1;

//
//// a
//    pwm_t = correct_balance_uzpt_pwm24 (svgen_dq_1.Ta, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_2, &svgen_pwm24_2.Ta_0, &svgen_pwm24_2.Ta_1, &svgen_pwm24_2.Ta_imp,pwm_t);
//// b
//    pwm_t = correct_balance_uzpt_pwm24 (svgen_dq_1.Tb, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_2, &svgen_pwm24_2.Tb_0, &svgen_pwm24_2.Tb_1, &svgen_pwm24_2.Tb_imp,pwm_t);
//// c2
//    pwm_t = correct_balance_uzpt_pwm24 (svgen_dq_1.Tc, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24 (&svgen_pwm24_2, &svgen_pwm24_2.Tc_0, &svgen_pwm24_2.Tc_1, &svgen_pwm24_2.Tc_imp,pwm_t);

    ////////////////////////////////////////
    ////////////////////////////////////////

////
    if (flag_km_0)
    {
     svgen_pwm24_1.prev_level = V_PWM24_PREV_PWM_WORK_KM0;
     svgen_pwm24_2.prev_level = V_PWM24_PREV_PWM_WORK_KM0;
    }
    else
    {
     svgen_pwm24_1.prev_level = V_PWM24_PREV_PWM_WORK;
     svgen_pwm24_2.prev_level = V_PWM24_PREV_PWM_WORK;
    }

    ////////////////////////////////////////
    ////////////////////////////////////////


//  logpar.log1 = (int16)(_IQtoIQ15(uz1));
//  logpar.log2 = (int16)(_IQtoIQ15(fz1));
//  logpar.log3 = (int16)(_IQtoIQ15(Ud));
//  logpar.log4 = (int16)(_IQtoIQ15(Uq));
//  logpar.log5 = (int16)(_IQtoIQ15(svgen_dq_1.Ualpha));
//  logpar.log6 = (int16)(_IQtoIQ15(svgen_dq_1.Ubeta));
//  logpar.log7 = (int16)(_IQtoIQ15(svgen_dq_1.Ta));
//  logpar.log8 = (int16)(_IQtoIQ15(svgen_dq_1.Tb));
//  logpar.log9 = (int16)(_IQtoIQ15(svgen_dq_1.Tc));
//  logpar.log10 = (int16)(_IQtoIQ12(analog.tetta));
//  logpar.log11 = (int16)(svgen_pwm24_1.Ta_0.Ti);
//  logpar.log12 = (int16)((svgen_pwm24_1.Ta_1.Ti));
//  logpar.log13 = (int16)(svgen_pwm24_1.Tb_0.Ti);
//  logpar.log14 = (int16)((svgen_pwm24_1.Tb_1.Ti));
//  logpar.log15 = (int16)(svgen_pwm24_1.Tc_0.Ti);
//  logpar.log16 = (int16)((svgen_pwm24_1.Tc_1.Ti));


//    svgen_pwm24_1.calc(&svgen_pwm24_1);
//    svgen_pwm24_2.calc(&svgen_pwm24_2);

    // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

    // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

//  set_predel_dshim24_simple0(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//  set_predel_dshim24_simple1(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//
//  set_predel_dshim24_simple0(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//  set_predel_dshim24_simple1(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//
//  set_predel_dshim24_simple0(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//  set_predel_dshim24_simple1(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//
//  set_predel_dshim24_simple0(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//  set_predel_dshim24_simple1(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//
//  set_predel_dshim24_simple0(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//  set_predel_dshim24_simple1(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//
//  set_predel_dshim24_simple0(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
//  set_predel_dshim24_simple1(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);


    *Uzad_out   = Uq;

    if (master==MODE_MASTER)
       *tetta_out  = uf_alg.tetta + add_tetta; // еще сдвигаемся, т.к. в slave придет с задержкой на один такт шима или на два?
    else
        *tetta_out  = uf_alg.tetta;

    *Kplus_out  = Kplus;

}

#pragma CODE_SECTION(test_calc_vect_dq_pwm24_Ing,".v_24pwm_run");
void test_calc_vect_dq_pwm24_Ing(_iq Theta_zad,_iq Ud_zad, _iq Uq_zad,
                                   unsigned int disable_alg_u_disbalance, _iq kplus_u_disbalance, _iq k_u_disbalance,
                                   _iq U_1, _iq U_2, unsigned int flag_km_0,
                                   _iq Uzad_max,
                                   unsigned int master,
                                   unsigned int n_bs,
                                   _iq *Kplus_out,
                                   _iq *Uzad_out )
{
    _iq Kplus;
    _iq Ud = 0;
    _iq Uq = 0;
    _iq Umod = 0;
    _iq pwm_t;

    DQ_TO_ALPHABETA dq_to_ab = DQ_TO_ALPHABETA_DEFAULTS;

    static _iq max_Km = _IQ(MAX_ZADANIE_K_M);
    static _iq max_Km_square = _IQ(MAX_ZADANIE_K_M * MAX_ZADANIE_K_M);

    ////////////////////////////////////////
    Umod = _IQsqrt(_IQmpy(Ud_zad, Ud_zad) + _IQmpy(Uq_zad, Uq_zad));
    if (Umod > max_Km) {
        Uq = _IQsqrt(max_Km_square - _IQmpy(Ud_zad, Ud_zad));
    }
    uf_disbalance_uzpt(Umod, disable_alg_u_disbalance, kplus_u_disbalance,
                       k_u_disbalance, U_1, U_2, Uzad_max, &Kplus);
    *Kplus_out = Kplus;
    *Uzad_out = Umod;
    ////////////////////////////////////////
    ////////////////////////////////////////
    if (master == MODE_MASTER)
    {
        // мы master
        uf_alg.tetta = Theta_zad;
        Ud = Ud_zad;
        Uq = Uq_zad;  //_IQsat(Uzad_uf1,Uzad_max,0);
    }
    else if (master == MODE_SLAVE)
    {
        // мы slave
        uf_alg.tetta = Theta_zad;
        Ud = Ud_zad;
        Uq = Uq_zad;   //_IQsat(master_Uzad_uf1,Uzad_max,0);
    }
    else
    {
        // мы непонятно кто!
        Ud = 0;
        Uq = 0;
        uf_alg.tetta = 0;
    }
    ////////////////////////////////////////
    ////////////////////////////////////////

    uf_alg.Ud = Ud;
    uf_alg.Uq = Uq;

    if (uf_alg.tetta > CONST_IQ_2PI)
    {
        uf_alg.tetta -= CONST_IQ_2PI;
    }

    if (uf_alg.tetta < 0)
    {
        uf_alg.tetta += CONST_IQ_2PI;
    }

    if (n_bs == 0)
        uf_alg.tetta_bs = uf_alg.tetta + uf_alg.winding_displacement_bs1;
    else
        uf_alg.tetta_bs = uf_alg.tetta + uf_alg.winding_displacement_bs2;

    dq_to_ab.Tetta = uf_alg.tetta_bs;
    dq_to_ab.Ud = Ud;
    dq_to_ab.Uq = Uq;
    dq_to_ab.calc2(&dq_to_ab);

    svgen_dq_1.Ualpha = dq_to_ab.Ualpha;
    svgen_dq_1.Ubeta = dq_to_ab.Ubeta;
    ////////////////////////////////////////

    uf_alg.Ualpha = dq_to_ab.Ualpha;
    uf_alg.Ubeta = dq_to_ab.Ubeta;

    svgen_dq_1.calc(&svgen_dq_1);

    uf_alg.svgen_dq_Ta = svgen_dq_1.Ta;
    uf_alg.svgen_dq_Tb = svgen_dq_1.Tb;
    uf_alg.svgen_dq_Tc = svgen_dq_1.Tc;

// 1
// a
    pwm_t = correct_balance_uzpt_pwm24(svgen_dq_1.Ta, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24(&svgen_pwm24_1, &svgen_pwm24_1.Ta_0,
                                           &svgen_pwm24_1.Ta_1,
                                           &svgen_pwm24_1.Ta_imp, pwm_t);
// b
    pwm_t = correct_balance_uzpt_pwm24(svgen_dq_1.Tb, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24(&svgen_pwm24_1, &svgen_pwm24_1.Tb_0,
                                           &svgen_pwm24_1.Tb_1,
                                           &svgen_pwm24_1.Tb_imp, pwm_t);
// c
    pwm_t = correct_balance_uzpt_pwm24(svgen_dq_1.Tc, Kplus);
    recalc_time_pwm_minimal_2_xilinx_pwm24(&svgen_pwm24_1, &svgen_pwm24_1.Tc_0,
                                           &svgen_pwm24_1.Tc_1,
                                           &svgen_pwm24_1.Tc_imp, pwm_t);

// 2 аналогично 1 т.к. тут парал. работа в Ingeteam

    svgen_pwm24_2.Ta_0 = svgen_pwm24_1.Ta_0;
    svgen_pwm24_2.Ta_1 = svgen_pwm24_1.Ta_1;
    svgen_pwm24_2.Tb_0 = svgen_pwm24_1.Tb_0;
    svgen_pwm24_2.Tb_1 = svgen_pwm24_1.Tb_1;
    svgen_pwm24_2.Tc_0 = svgen_pwm24_1.Tc_0;
    svgen_pwm24_2.Tc_1 = svgen_pwm24_1.Tc_1;



//// a
//    pwm_t = correct_balance_uzpt_pwm24(svgen_dq_1.Ta, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24(&svgen_pwm24_2, &svgen_pwm24_2.Ta_0,
//                                           &svgen_pwm24_2.Ta_1,
//                                           &svgen_pwm24_2.Ta_imp, pwm_t);
//// b
//    pwm_t = correct_balance_uzpt_pwm24(svgen_dq_1.Tb, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24(&svgen_pwm24_2, &svgen_pwm24_2.Tb_0,
//                                           &svgen_pwm24_2.Tb_1,
//                                           &svgen_pwm24_2.Tb_imp, pwm_t);
//// c2
//    pwm_t = correct_balance_uzpt_pwm24(svgen_dq_1.Tc, Kplus);
//    recalc_time_pwm_minimal_2_xilinx_pwm24(&svgen_pwm24_2, &svgen_pwm24_2.Tc_0,
//                                           &svgen_pwm24_2.Tc_1,
//                                           &svgen_pwm24_2.Tc_imp, pwm_t);

    ////////////////////////////////////////
    ////////////////////////////////////////


}