#include "IQmathLib.h"
#include "math.h"
#include "my_filter.h"
#include "params.h"
#include "adc_tools.h"
#include "abc_to_dq.h"
#include "regul_power.h"
#include "regul_turns.h"
#include "teta_calc.h"
#include "pwm_vector_regul.h"
#include "pid_reg3.h"
#include "vector.h"
#include "params_motor.h"
#include "v_pwm24.h"
//#include "power_distribution.h"
#include "rotation_speed.h"
//#include "calc_3_power.h"

// #pragma DATA_SECTION(pidD,".fast_vars1");
// PIDREG3 pidD = PIDREG3_UNDEPENDENT_DEFAULTS;
PIDREG3 pidD = PIDREG3_DEFAULTS;
// #pragma DATA_SECTION(pidQ,".fast_vars1");
// PIDREG3 pidQ = PIDREG3_UNDEPENDENT_DEFAULTS;
PIDREG3 pidQ = PIDREG3_DEFAULTS;

// #pragma DATA_SECTION(pidD2,".fast_vars1");
// PIDREG3 pidD2 = PIDREG3_UNDEPENDENT_DEFAULTS;
PIDREG3 pidD2 = PIDREG3_DEFAULTS;
// #pragma DATA_SECTION(pidQ2,".fast_vars1");
// PIDREG3 pidQ2 = PIDREG3_UNDEPENDENT_DEFAULTS;
PIDREG3 pidQ2 = PIDREG3_DEFAULTS;

// #pragma DATA_SECTION(cos_fi,".fast_vars1");
COS_FI_STRUCT cos_fi = {0,0};

// #pragma DATA_SECTION(vect_control,".fast_vars1");
VECTOR_CONTROL vect_control = VECTOR_CONTROL_DEFAULTS;

// #pragma DATA_SECTION(koeff_Fs_filter,".fast_vars");
long koeff_Fs_filter = _IQ(0.01);

// _iq winding_displacement = FIXED_PIna6;

WINDING a;
FLAG f;


ROTOR_VALUE rotor = ROTOR_VALUE_DEFAULTS;

#define IQ_ONE  16777216

#define CONST_IQ_2PI  105414357 
#define CONST_IQ_2PI3  35138119  // 120 grad
#define CONST_IQ_PI2  26340229 

#define K_MODUL_MIN 167772	//83886 //0.5% - ограничение из-за перенапружений
#define K_MODUL_MAX 15435038LL	//13421772LL	//80%	16106127LL ~ 96%	//15435038LL ~ 0.92%
								//15770583LL ~ 0.94%
#define K_MODUL_MAX_PART	15099494LL	// 15099494LL ~ 0.9

#define I_ZERO_LEVEL_IQ 111848  //20A   //279620 ~ 50A //55924 ~ 10A
#define K_MODUL_DEAD_TIME 503316    //3%

//#define I_ZERO_LEVEL_IQ 27962 //5A //55924 ~ 10A

#define Id_MIN  1118481 //200A  //111848 ~ 20A //55924 ~ 10A

// #pragma DATA_SECTION(zadan_Id_min,".fast_vars");
_iq zadan_Id_min = Id_MIN;

#define START_TETTA_IQ 4392264  //15 grad

void analog_dq_calc(void);
void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2);
void calcUdUqCompensation(_iq Frot);

float kI_D = 0.2;
float kI_Q = 0.2;
float kP_D = 0.1;//.1;
float kP_Q = 0.3;//.3;

// float kI_D_rev = 0.02;
// float kI_Q_rev = 0.03;
// float kP_D_rev = 0.1;//.3;
// float kP_Q_rev = 0.15;//.3;

void init_DQ_pid()
{
    unsigned int i = 0;
    int *pStr = (int*)&f;
    for (i = 0; i < sizeof(f) / sizeof(int); i++) {
        *(pStr + i) = 0;
    }

    *pStr = (int*)&a;
    for (i = 0; i < sizeof(a) / sizeof(int); i++) {
        *(pStr + i) = 0;
    }

    pidD.Ref = 0;
    pidD.Kp = _IQ(kP_D);//_IQ(0.2); //_IQ(0.1);
    pidD.Ki = _IQ(kI_D);	//_IQ(0.2);//	//
    pidD.Kc = _IQ(0.3);
	pidD.Kd = 0;
    pidD.OutMax = K_MODUL_MAX_PART;
    pidD.OutMin = -K_MODUL_MAX_PART;	//0;
	pidD.Up = 0;
	pidD.Ui = 0;
	pidD.Ud = 0;
	pidD.Out = 0;

    pidQ.Ref = 0;
    pidQ.Kp = _IQ(kP_Q);//_IQ(0.3);//_IQ(0.3);
    pidQ.Ki = _IQ(kI_Q);	//_IQ(0.2);	//
    pidQ.Kc = _IQ(0.3);
	pidQ.Kd = 0;
    pidQ.OutMax = K_MODUL_MAX_PART; //_IQ(0.9);
    pidQ.OutMin = -K_MODUL_MAX_PART;    //-_IQ(0.9);	//0;
	pidQ.Up = 0;
	pidQ.Ui = 0;
	pidQ.Ud = 0;
	pidQ.Out = 0;

	pidD2.Ref = 0;
	pidD2.Kp = _IQ(kP_D);//_IQ(0.1); //_IQ(0.1);
	pidD2.Ki = _IQ(kI_D);	//_IQ(0.2);//	//
	pidD2.Kc = _IQ(0.3);
	pidD2.Kd = 0;
	pidD2.OutMax = K_MODUL_MAX_PART;
	pidD2.OutMin = -K_MODUL_MAX_PART;	//0;
	pidD2.Up = 0;
	pidD2.Ui = 0;
	pidD2.Ud = 0;
	pidD2.Out = 0;

	pidQ2.Ref = 0;
	pidQ2.Kp = _IQ(kP_Q);// _IQ(0.3);//_IQ(0.3);
	pidQ2.Ki = _IQ(kI_Q);	//_IQ(0.2);	//
	pidQ2.Kc = _IQ(0.3);
	pidQ2.Kd = 0;
	pidQ2.OutMax = K_MODUL_MAX_PART;    //_IQ(0.9);
	pidQ2.OutMin = -K_MODUL_MAX_PART;   //-_IQ(0.9);	//0;
	pidQ2.Up = 0;
	pidQ2.Ui = 0;
	pidQ2.Ud = 0;
	pidQ2.Out = 0;

	init_tetta_pid();
	init_Fvect_pid();
	init_Pvect_pid();
	init_tetta_calc_struct();

    init_Adc_Variables();

	cos_fi.cos_fi_nom = _IQ(0.87);
	cos_fi.cos_fi_nom_squared = _IQ(0.87 * 0.87);
	// cos_fi.cos_fi_nom = _IQ(COS_FI);
	// cos_fi.cos_fi_nom_squared = _IQ(COS_FI * COS_FI);

	vect_control.koef_Ud_comp = _IQ((L_SIGMA_S + L_M * L_SIGMA_R / (L_M + L_SIGMA_R)) * 2 * 3.14 * NORMA_FROTOR);  //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r)
    vect_control.koef_Uq_comp = _IQ((L_M + L_SIGMA_S) * 2 * 3.14 * NORMA_FROTOR);
    vect_control.k_modul_max = K_MODUL_MAX;
    vect_control.k_modul_max_square = _IQmpy(K_MODUL_MAX, K_MODUL_MAX);
    vect_control.iq_Id_out_max = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP);

    vect_control.koeff_correct_Id = IQ_ONE;

    vect_control.equial_Iq_Proportional = _IQ(0.05);
    vect_control.flag_reverse = 0;
}

void reset_DQ_pid()
{
	pidD.Up = 0;
	pidD.Up1 = 0;
	pidD.Ui = 0;
	pidD.Ud = 0;
	pidD.Out = 0;
	pidQ.Up = 0;
	pidQ.Up1 = 0;
	pidQ.Ui = 0;
	pidQ.Ud = 0;
	pidQ.Out = 0;

	pidD2.Up = 0;
	pidD2.Up1 = 0;
	pidD2.Ui = 0;
	pidD2.Ud = 0;
	pidD2.Out = 0;
	pidQ2.Up = 0;
	pidQ2.Up1 = 0;
	pidQ2.Ui = 0;
	pidQ2.Ud = 0;
	pidQ2.Out = 0;
}

// #pragma CODE_SECTION(Idq_to_Udq,".fast_run2");
void Idq_to_Udq(_iq Id_zad, _iq Iq_zad, _iq Id_measured, _iq Iq_measured, _iq* Ud_zad, _iq* Uq_zad)
{
    pidD.Ref = Id_zad;
    pidD.Fdb = Id_measured;
    pidD.calc(&pidD);
    *Ud_zad = pidD.Out;

    pidQ.Ref = Iq_zad;
    pidQ.Fdb = Iq_measured;
    pidQ.calc(&pidQ);
    *Uq_zad = pidQ.Out;

}

// inline void set_pid_coeffs() {
// 	if (vect_control.flag_reverse == 0) {
// 		pidD.Kp = _IQ(kP_D);//_IQ(0.2); //_IQ(0.1);
//     	pidD.Ki = _IQ(kI_D);	//_IQ(0.2);//	//
//     	pidQ.Kp = _IQ(kP_Q);//_IQ(0.3);//_IQ(0.3);
//     	pidQ.Ki = _IQ(kI_Q);	//_IQ(0.2);	//
//     	pidD2.Kp = _IQ(kP_D);//_IQ(0.1); //_IQ(0.1);
// 		pidD2.Ki = _IQ(kI_D);	//_IQ(0.2);//	//
// 		pidQ2.Kp = _IQ(kP_Q);// _IQ(0.3);//_IQ(0.3);
// 		pidQ2.Ki = _IQ(kI_Q);	//_IQ(0.2);	//
// 	} else {
// 		pidD.Kp = _IQ(kP_D_rev);//_IQ(0.2); //_IQ(0.1);
//     	pidD.Ki = _IQ(kI_D_rev);	//_IQ(0.2);//	//
//     	pidQ.Kp = _IQ(kP_Q_rev);//_IQ(0.3);//_IQ(0.3);
//     	pidQ.Ki = _IQ(kI_Q_rev);	//_IQ(0.2);	//
//     	pidD2.Kp = _IQ(kP_D_rev);//_IQ(0.1); //_IQ(0.1);
// 		pidD2.Ki = _IQ(kI_D_rev);	//_IQ(0.2);//	//
// 		pidQ2.Kp = _IQ(kP_Q_rev);// _IQ(0.3);//_IQ(0.3);
// 		pidQ2.Ki = _IQ(kI_Q_rev);	//_IQ(0.2);	//
// 	}
// }

void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad, _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1
												, _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2)
{
	pidD.Ref = Id_zad;
	pidD.Fdb = Id_measured1;
	pidD.calc(&pidD);
	*Ud_zad1 = pidD.Out;

	pidQ.Ref = Iq_zad;
	pidQ.Fdb = Iq_measured1;
	pidQ.calc(&pidQ);
	*Uq_zad1 = pidQ.Out;

	pidD2.Ref = Id_zad;
	pidD2.Fdb = Id_measured2;
	pidD2.calc(&pidD2);
	*Ud_zad2 = pidD2.Out;

	pidQ2.Ref = Iq_zad;
	pidQ2.Fdb = Iq_measured2;
	pidQ2.calc(&pidQ2);
	*Uq_zad2 = pidQ2.Out;
}

// #pragma CODE_SECTION(pwm_vector_model_titov,".fast_run");
void pwm_vector_model_titov(_iq Pzad, _iq Fzad, int direction, _iq Frot, int mode,
                            int reset, int calcPWMtimes)
{
    int calc_channel = 1;
	_iq U_zad1 = 0, U_zad2 = 0;


	// if (f.flag_leading == 0 && f.read_task_from_optical_bus == 1 && f.sync_rotor_from_optical_bus == 1) {
	// 	Frot = rotor.iqFrotFromOptica;
	// }

	//Коррекция Id при привышении номинальных оборотов
    if (_IQabs(Frot) > IQ_F_ROTOR_NOM) {
        vect_control.koeff_correct_Id = _IQdiv(IQ_F_ROTOR_NOM, _IQabs(Frot));
    } else {
        vect_control.koeff_correct_Id = IQ_ONE;
    }


	if(direction < 0) { Frot = -Frot; }
//	prev_dir = direction;
	if(reset == 1)		//Сброс накопленых составл-ющих регул-тора
	{
		reset_DQ_pid();
		reset_tetta_pid();
		analog.iqIq1_filter = 0;
		analog.iqIq2_filter = 0;
		analog.iqUq2_filter = 0;
		U_zad1 = K_MODUL_MIN;
		U_zad2 = K_MODUL_MIN;
		vect_control.iq_Id_out_max = Id_out_max_low_speed;
		vect_control.flag_reverse = 0;
	}

	set_cos_tetta_calc_params();
	analog_dq_calc();


    vector_turns(Fzad, Frot, analog.iqIq1, mode, &vect_control.iqIq_zad, &vect_control.iqId_zad);
    vector_power(Pzad, analog.iqPvsi1 + analog.iqPvsi2, analog.iqIq1, mode, Frot, &vect_control.iqIq_zad, &vect_control.iqId_zad);

    vect_control.iqPzad = pidPvect.Ref;
    vect_control.iqPizm = pidPvect.Fdb;
    vect_control.iqFrot = Frot;

    // Задание минимального значения Id
    if (vect_control.iqId_zad < Id_MIN) { vect_control.iqId_zad = zadan_Id_min; }
    
    //Коррекция Id при привышении номинальных оборотов
    // if (_IQabs(Frot)  > IQ_F_ROTOR_NOM) {
    //     vect_control.iqId_zad = _IQmpy(vect_control.iqId_zad, vect_control.koeff_correct_Id);
    // }


// Frot =_IQ(15.0 / 6 / NORMA_FROTOR);
// vect_control.iqIq_zad = _IQ(IQ_OUT_NOM / NORMA_ACP);
// vect_control.iqId_zad = 0;// _IQ(789.0 / NORMA_ACP);

    // set_pid_coeffs();
    Idq_to_Udq_2_windings(vect_control.iqId_zad, vect_control.iqIq_zad, analog.iqId1, analog.iqIq1, &vect_control.iqUdKm1, &vect_control.iqUqKm1,
    		analog.iqId2, analog.iqIq2, &vect_control.iqUdKm2, &vect_control.iqUqKm2);


    calc_tetta_Id(Frot, vect_control.iqId_zad, vect_control.iqIq_zad, &analog.tetta, &analog.Fsl, &analog.iqFstator, reset);
    filter.Fsl = exp_regul_iq(koeff_Fs_filter, filter.Fsl, analog.Fsl);

    // analog.tetta = _IQ(vect_control.theta);
    analog.iqIq_zadan = vect_control.iqIq_zad;

	// calcUdUqCompensation(Frot);   //TODO: test on power after testing rotating
    vect_control.iqUdKm1Out = vect_control.iqUdKm1;
    vect_control.iqUqKm1Out = vect_control.iqUqKm1;
	vect_control.iqUdKm2Out = vect_control.iqUdKm2;
    vect_control.iqUqKm2Out = vect_control.iqUqKm2;

    if(vect_control.iqUdKm1Out > K_MODUL_MAX_PART) { vect_control.iqUdKm1Out = K_MODUL_MAX_PART;}
    if(vect_control.iqUdKm1Out < -K_MODUL_MAX_PART) { vect_control.iqUdKm1Out = -K_MODUL_MAX_PART;}
    if(vect_control.iqUqKm1Out > K_MODUL_MAX_PART) { vect_control.iqUqKm1Out = K_MODUL_MAX_PART;}
    if(vect_control.iqUqKm1Out < -K_MODUL_MAX_PART) { vect_control.iqUqKm1Out = -K_MODUL_MAX_PART;}

    if(vect_control.iqUdKm2Out > K_MODUL_MAX_PART) { vect_control.iqUdKm2Out = K_MODUL_MAX_PART;}
    if(vect_control.iqUdKm2Out < -K_MODUL_MAX_PART) { vect_control.iqUdKm2Out = -K_MODUL_MAX_PART;}
    if(vect_control.iqUqKm2Out > K_MODUL_MAX_PART) { vect_control.iqUqKm2Out = K_MODUL_MAX_PART;}
    if(vect_control.iqUqKm2Out < -K_MODUL_MAX_PART) { vect_control.iqUqKm2Out = -K_MODUL_MAX_PART;}

    U_zad1 = _IQsqrt(_IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out) + _IQmpy(vect_control.iqUqKm1Out, vect_control.iqUqKm1Out));
    U_zad2 = _IQsqrt(_IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out) + _IQmpy(vect_control.iqUqKm2Out, vect_control.iqUqKm2Out));
    vect_control.iqUzad1 = U_zad1;
    vect_control.iqUzad2 = U_zad2;

    if (U_zad1 > vect_control.k_modul_max) {
        if (vect_control.iqUqKm1Out > 0) {
            vect_control.iqUqKm1Out = _IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out));
        } else {
            vect_control.iqUqKm1Out = -_IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out));
        }
    }
    if (U_zad2 > vect_control.k_modul_max) {
        if (vect_control.iqUqKm2Out > 0) {
            vect_control.iqUqKm2Out = _IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out));
        } else {
            vect_control.iqUqKm2Out = -_IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out));
        }
    }

    // Компенсация колебания Iq между двумя обмотками
    // vect_control.equial_Iq_Delta = analog.iqIq1 - analog.iqIq2;
    // vect_control.equial_Iq_Out = _IQmpy(vect_control.equial_Iq_Delta,
    // 								vect_control.equial_Iq_Proportional);
    // vect_control.iqUqKm1Out -= vect_control.equial_Iq_Out;
    // vect_control.iqUqKm2Out += vect_control.equial_Iq_Out;

 //    if (U_zad1 < K_MODUL_DEAD_TIME) {
 //        vect_control.iqUdKm1 = 0;
 //        vect_control.iqUqKm1 = 0;
	// }
 //    if (U_zad2 < K_MODUL_DEAD_TIME) {
 //        vect_control.iqUdKm2 = 0;
 //        vect_control.iqUqKm2 = 0;
	// }
	
	vect_control.iqUzad1 = _IQsqrt(_IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out) + _IQmpy(vect_control.iqUqKm1Out, vect_control.iqUqKm1Out));
    vect_control.iqUzad2 = _IQsqrt(_IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out) + _IQmpy(vect_control.iqUqKm2Out, vect_control.iqUqKm2Out));

	analog_Udq_calc(vect_control.iqUdKm1, vect_control.iqUqKm1, vect_control.iqUdKm2, vect_control.iqUqKm2);


	if(mode == 0)	// Обнул-ем при первом входе при включении
	{
		U_zad1 = K_MODUL_MIN;
		U_zad2 = K_MODUL_MIN;
		vect_control.iqUdKm1Out = 0;
		vect_control.iqUqKm1Out = 0;
		vect_control.iqUdKm2Out = 0;
		vect_control.iqUqKm2Out = 0;
//		teta_st = analog.tetta;
	}

	a.iqk1 = vect_control.iqUzad1;
//	analog.iqUdKm = Ud_zad;
//	analog.iqUqKm = Uq_zad;
//	analog.iqId_zad = vect_control.iqId_zad;

	
    if(mode && f.Go && calcPWMtimes)
	{
    	test_calc_dq_pwm24(vect_control.iqUdKm1Out, vect_control.iqUqKm1Out, vect_control.iqUdKm2Out, vect_control.iqUqKm2Out, analog.tetta,K_MODUL_MAX);
    }

	
}



// #pragma DATA_SECTION(koeff_Iq_filter_slow,".fast_vars");
long koeff_Iq_filter_slow = _IQ(0.5);   //_IQ(0.3);
// #pragma DATA_SECTION(koeff_Idq_filter,".fast_vars");
long koeff_Idq_filter = _IQ(0.15);
// #pragma DATA_SECTION(koeff_Uq_filter,".fast_vars");
long koeff_Uq_filter = 500;
// #pragma DATA_SECTION(koeff_Ud_filter,".fast_vars");
long koeff_Ud_filter = _IQ(0.5);


// #pragma CODE_SECTION(analog_dq_calc,".fast_run");
void analog_dq_calc()
{
    ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS;

    abc_dq_converter.Ia = analog.iqIa1_1;
    abc_dq_converter.Ib = analog.iqIb1_1;
    abc_dq_converter.Ic = analog.iqIc1_1;
    abc_dq_converter.Tetta = analog.tetta;
	abc_dq_converter.calc(&abc_dq_converter);
    analog.iqId1 = abc_dq_converter.Id;
    filter.iqId1 = exp_regul_iq(koeff_Idq_filter, filter.iqId1, analog.iqId1);
    analog.iqIq1 = abc_dq_converter.Iq;
    filter.iqIq1 = exp_regul_iq(koeff_Idq_filter, filter.iqIq1, analog.iqIq1);
//	analog.iqIq1_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow,
//											analog.iqIq1_filter, analog.iqIq1);
	analog.iqIq1_filter = exp_regul_iq(koeff_Iq_filter_slow, analog.iqIq1_filter, analog.iqIq1);

    abc_dq_converter.Ia = analog.iqIa2_1;
    abc_dq_converter.Ib = analog.iqIb2_1;
    abc_dq_converter.Ic = analog.iqIc2_1;
    abc_dq_converter.Tetta = analog.tetta - winding_displacement;
    abc_dq_converter.calc(&abc_dq_converter);
    analog.iqId2 = abc_dq_converter.Id;
    filter.iqId2 = exp_regul_iq(koeff_Idq_filter, filter.iqId2, analog.iqId2);
    analog.iqIq2 = abc_dq_converter.Iq;
    filter.iqIq2 = exp_regul_iq(koeff_Idq_filter, filter.iqIq2, analog.iqIq2);
//	analog.iqIq2_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow,
//											analog.iqIq2_filter, analog.iqIq2);
	analog.iqIq2_filter = exp_regul_iq(koeff_Iq_filter_slow, analog.iqIq2_filter, analog.iqIq2);

	if (_IQabs(analog.iqId1) < I_ZERO_LEVEL_IQ) { analog.iqId1 = 0; }
	if (_IQabs(analog.iqIq1) < I_ZERO_LEVEL_IQ) { analog.iqIq1 = 0; }
	if (_IQabs(analog.iqId2) < I_ZERO_LEVEL_IQ) { analog.iqId2 = 0; }
	if (_IQabs(analog.iqIq2) < I_ZERO_LEVEL_IQ) { analog.iqIq2 = 0; }

//    analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, analog.iqUq1), 25165824L);
//    analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, analog.iqUq2), 25165824L);

    analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, _IQabs(analog.iqUq1)), 25165824L);
    analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, _IQabs(analog.iqUq2)), 25165824L);

//	analog.iqM_1 = _IQdiv(analog.iqPvsi1, _IQmpy(rotor.iqW, CONST_IQ_2PI));
//	analog.iqM_2 = _IQdiv(analog.iqPvsi2, _IQmpy(rotor.iqW, CONST_IQ_2PI));
//    logpar.log11 = (int16)_IQtoIQ15(analog.iqIq1_filter);
}

// #pragma CODE_SECTION(analog_Udq_calc,".fast_run");
void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2)
{
	analog.iqUd1 = _IQmpy(Ud1, _IQmpy((filter.iqU_1_long + filter.iqU_2_long), 8388608L));	// 8388608 = _IQ(0.5)
    analog.iqUq1 = _IQmpy(Uq1, _IQmpy((filter.iqU_1_long + filter.iqU_2_long), 8388608L));
    analog.iqUd2 = _IQmpy(Ud2, _IQmpy((filter.iqU_3_long + filter.iqU_4_long), 8388608L));
    analog.iqUq2 = _IQmpy(Uq2, _IQmpy((filter.iqU_3_long + filter.iqU_4_long), 8388608L));

	filter.iqUd1 = exp_regul_iq(koeff_Ud_filter, filter.iqUd1, analog.iqUd1);

}


//#pragma CODE_SECTION(analog_dq_calc_const,".fast_run");
void analog_dq_calc_const()
{
    ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS;

    abc_dq_converter.Ia = analog.iqIa1_1;
    abc_dq_converter.Ib = analog.iqIb1_1;
    abc_dq_converter.Ic = analog.iqIc1_1;
    abc_dq_converter.Tetta = analog.tetta;	// svgen_pwm24_1.Alpha;
	abc_dq_converter.calc(&abc_dq_converter);
    analog.iqId1 = abc_dq_converter.Id;
    analog.iqIq1 = abc_dq_converter.Iq;
	analog.iqIq1_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow,
											analog.iqIq1_filter, analog.iqIq1);

    abc_dq_converter.Ia = analog.iqIa2_1;
    abc_dq_converter.Ib = analog.iqIb2_1;
    abc_dq_converter.Ic = analog.iqIc2_1;
    abc_dq_converter.Tetta = analog.tetta;	// svgen_pwm24_2.Alpha;
    abc_dq_converter.calc(&abc_dq_converter);
    analog.iqId2 = abc_dq_converter.Id;
    analog.iqIq2 = abc_dq_converter.Iq;
	analog.iqIq2_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow,
											analog.iqIq2_filter, analog.iqIq2);

    analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, analog.iqUq1), 25165824L);
    analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, analog.iqUq2), 25165824L);
//	analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1, analog.iqUq1), 25165824L);
//	analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2, analog.iqUq2), 25165824L);

//	analog.iqM_1 = _IQdiv(analog.iqPvsi1, _IQmpy(rotor.iqW, CONST_IQ_2PI));
//	analog.iqM_2 = _IQdiv(analog.iqPvsi2, _IQmpy(rotor.iqW, CONST_IQ_2PI));
}

void calcUdUqCompensation(_iq Frot) {
    _iq Uzpt = (filter.iqU_1_long + filter.iqU_2_long) >> 1;
    _iq UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), analog.iqIq1);
    _iq UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), analog.iqId1);
    if (Uzpt != 0) {
	    vect_control.iqUdCompensation1 = -_IQdiv(UdVolt, Uzpt);
	    vect_control.iqUqCompensation1 = _IQdiv(UqVolt, Uzpt);
    } else {
    	vect_control.iqUdCompensation1 = 0;
	    vect_control.iqUqCompensation1 = 0;
    }

    vect_control.iqUdKm1Out = vect_control.iqUdKm1 + vect_control.iqUdCompensation1;
    vect_control.iqUqKm1Out = vect_control.iqUqKm1 + vect_control.iqUqCompensation1;

    Uzpt = (filter.iqU_3_long + filter.iqU_4_long) >> 1;
    UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), analog.iqIq2);
    UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), analog.iqId2);
    if (Uzpt != 0) {
	    vect_control.iqUdCompensation2 = -_IQdiv(UdVolt, Uzpt);
	    vect_control.iqUqCompensation2 = _IQdiv(UqVolt, Uzpt);
    } else {
    	vect_control.iqUdCompensation2 = 0;
	    vect_control.iqUqCompensation2 = 0;
    }


    vect_control.iqUdKm2Out = vect_control.iqUdKm2 + vect_control.iqUdCompensation2;
    vect_control.iqUqKm2Out = vect_control.iqUqKm2 + vect_control.iqUqCompensation2;
}

#define IQ_150_RPM 2097152      //150об/мин
#define IQ_140_RPM 1957341      //140об/мин
//#define IQ_135_RPM 1887436      //135об/мин
#define IQ_125_RPM 1747626      //125об/мин
#define IQ_120_RPM 1677721      //120об/мин
#define IQ_115_RPM 1607816      //115об/мин
#define IQ_110_RPM 1537911      //110об/мин

#define K_MODUL_SWITCH_COS_FI 13925089  //83%
#define K_MODUL_SWITCH_COS_FI_2 14260633  //85%
#define K_MODUL_SWITCH_OFF  10905190    //65% //11744051 ~ 70% //12582912 ~ 75%

#define U200V 1118481


// #pragma CODE_SECTION(set_cos_tetta_calc_params,".fast_run1");
void set_cos_tetta_calc_params() {

    _iq currentCos = _IQ(COS_FI);
    _iq currentCosSq = _IQ(COS_FI * COS_FI);
    static _iq cosFi_low_speed = _IQ(0.85);
    static _iq cosFi_Sq_low_speed = _IQ(0.85 * 0.85);
    static _iq cosFi_mid_speed = _IQ(COS_FI);
    static _iq cosFi_Sq_mid_speed = _IQ(COS_FI * COS_FI);
    static _iq cosFi_high_speed = _IQ(0.89);
    static _iq cosFi_Sq_high_speed = _IQ(0.89 * 0.89);
    static _iq stepChangeCos = _IQ(0.001);
    static _iq kt_low_speed = _IQ(0.0045);
    static _iq kt_over_140rpm = _IQ(0.0048);
    static _iq kt_over_150rpm = _IQ(0.0049);
    static _iq kt_single_inv = _IQ(0.0039);

    _iq current_kt = _IQ(0.0045);
    static _iq stepChangeKt =_IQ(0.00001);

    static _iq addCompensateUd = _IQ(0.0004);

    static unsigned int flag_high_Km = 0;

    if (a.iqk1 < K_MODUL_SWITCH_OFF) {
        flag_high_Km = 0;
    }

//    if (_IQabs(rotor.iqFout) > IQ_150_RPM) {
//        currentCos = _IQ(0.9);
//        currentCosSq = _IQ(0.9 * 0.9);
////            tetta_calc.k_t = _IQ(0.0049);
//        current_kt = kt_over_150rpm;    //_IQ(0.0049);
//    } else
    if (_IQabs(rotor.iqFout) > IQ_140_RPM || (a.iqk1 > K_MODUL_SWITCH_COS_FI_2)
            || flag_high_Km == 1) {
        if (a.iqk1 > K_MODUL_SWITCH_COS_FI_2) {
            flag_high_Km = 1;
        }
        currentCos = cosFi_high_speed;
        currentCosSq = cosFi_Sq_high_speed;
//            tetta_calc.k_t = _IQ(0.0048);
        current_kt = kt_over_140rpm;    //_IQ(0.0048);
    } else if ((_IQabs(rotor.iqFout) > IQ_115_RPM && cos_fi.cos_fi_nom < _IQ(0.889))
            || (_IQabs(rotor.iqFout) < IQ_135_RPM && cos_fi.cos_fi_nom > _IQ(0.889))
            || (a.iqk1 > K_MODUL_SWITCH_COS_FI)) {
        currentCos = cosFi_mid_speed;
        currentCosSq = cosFi_Sq_mid_speed;
//            tetta_calc.k_t = _IQ(0.0045);
        current_kt = kt_low_speed;  //_IQ(0.0045);
    } else if (_IQabs(rotor.iqFout) < IQ_110_RPM) {
        currentCos = cosFi_low_speed;
        currentCosSq = cosFi_Sq_low_speed;
        //(f.secondPChState != 4)
        if(((analog.iqIm_1 > I_OUT_NOMINAL_IQ) || (analog.iqIm_2 > I_OUT_NOMINAL_IQ))
                && (f.secondPChState != 4)) {
            current_kt = kt_single_inv; //_IQ(0.0039);
        } else if ((analog.iqIm_1 < 9507089) || (analog.iqIm_2 < 9507089)) {
            current_kt = kt_low_speed;  //_IQ(0.0045);
        }
    }

    if (analog.iqUd1 < -U200V || (a.iqk1 > K_MODUL_SWITCH_COS_FI_2)) {
        current_kt += addCompensateUd;  // _IQ(0.0004);
    }
    cos_fi.cos_fi_nom = zad_intensiv_q(stepChangeCos, stepChangeCos, cos_fi.cos_fi_nom, currentCos);
    cos_fi.cos_fi_nom_squared = zad_intensiv_q(stepChangeCos, stepChangeCos, cos_fi.cos_fi_nom_squared, currentCosSq);

    tetta_calc.k_t = zad_intensiv_q(stepChangeKt, stepChangeKt, tetta_calc.k_t, current_kt);
}


void limit_mzz_zad_power(_iq Frot)
{
    Frot = labs(Frot);

    if (vect_control.flag_reverse) {
//        f.iq_mzz_zad = _IQ(1300.0/NORMA_MZZ);
        f.iq_mzz_zad = _IQ(1100.0/NORMA_MZZ);
    } else 
    if (Frot < 279620) //20 ob/min
    {
//        f.iq_mzz_zad = _IQ(1200.0/NORMA_MZZ);
        f.iq_mzz_zad = _IQ(1400.0/NORMA_MZZ);
    }
    else if (Frot < 419430)  //30 ob/min
    {
        f.iq_mzz_zad = _IQ(1400.0/NORMA_MZZ);
    }
//    else if(rotor.iqFout < 699050) //50 ob/min
    else if (Frot < 838860)  //60 ob/min
//    else if (rotor.iqFout < 978670)  //70 ob/min
    {
//        f.iq_mzz_zad = _IQ(1800.0/NORMA_MZZ);
        f.iq_mzz_zad = _IQ(2000.0/NORMA_MZZ);
    }
    else
    {
        f.iq_mzz_zad = _IQ(2000.0/NORMA_MZZ);
//              f.iq_mzz_zad = _IQ(1500.0/NORMA_MZZ);
    }
}