#include "IQmathLib.h"
#include "math.h"
#include "params.h"
#include "adc_tools.h"
#include "regul_turns.h"
#include "pid_reg3.h"
#include "vector.h"
#include "teta_calc.h"
#include "pwm_vector_regul.h"
#include "my_filter.h"

// #pragma DATA_SECTION(pidFvect,".fast_vars1");
PIDREG3 pidFvect = PIDREG3_DEFAULTS;

#define MAX_PID_CURRENT IQ_OUT_NOM

#define IQ_165_RPM 2306867		//165об/мин
#define IQ_170_RPM 2376772		//170об/мин
#define IQ_5_RPM 69905			//5об/мин

#define TIME_RMP_FAST 10.0	//sec
#define TIME_RMP_SLOW 30.0	//sec
#define F_DEST 3.0	//Hz

void init_Fvect_pid()
{
    pidFvect.Ref = 0;
    pidFvect.Kp = _IQ(10.0);//_IQ(30.0);//	//
    pidFvect.Ki = _IQ(0.03);	//_IQ(0.015);
    pidFvect.Kc = _IQ(0.5);
    pidFvect.OutMax = _IQ(MAX_PID_CURRENT / NORMA_ACP);
    pidFvect.OutMin = -_IQ(MAX_PID_CURRENT / NORMA_ACP);
}

void reset_F_pid()
{
	pidFvect.Up = 0;
	pidFvect.Up1 = 0;
	pidFvect.Ui = 0;
	pidFvect.Ud = 0;
	pidFvect.Out = 0;
}

void vector_turns(_iq Fzad, _iq Frot, _iq Iq, int mode, _iq* Iq_zad, _iq* Id_zad)
{
    static _iq Fzad_rmp = 0, koef_fast = _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 2.5);
    static _iq koef_slow = _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 5.0);
//    static _iq cos_fi_nom = _IQ(COS_FI), cos_fi_nom_squared = _IQ(COS_FI * COS_FI);	//prev_Fzad = 0,
    static _iq Iq_out_max = _IQ(IQ_OUT_NOM / NORMA_ACP);
    static _iq Id_out_max = _IQ(500.0 / NORMA_ACP);	//_IQ(ID_OUT_NOM / NORMA_ACP);
	static _iq mzz_zad_int=0;
	static int Iq_rmp_to_optica = 0;
//	static int flag_Iq_synced_with_optica = 0;
//	static _iq step_Iq_rmp = _IQ(20.0 / NORMA_ACP);
    _iq koef_rmp;	//, koef_spad;
    _iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat;
	_iq deltaVar;

// 	if (f.DownTemperature) {
// //		mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, _IQmpy(Iq_out_max, temperature_limit_koeff));
// 		mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, _IQmpy(f.iq_mzz_zad, temperature_limit_koeff));
// 	} else 
    {
		mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, f.iq_mzz_zad);
//		mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, pid_Out_max);
	}

	pidFvect.OutMax = Iq_out_max;
    pidFvect.OutMin = -Iq_out_max;
    
    if (Fzad >= 0 && Frot > 0) {
    	pidFvect.OutMin = 0;
    }

    if (Fzad <= 0 && Frot < 0) {
    	pidFvect.OutMax = 0;
    }



    if((mode != 1) || (!f.Go))  //Заносим текущее состояние в регул-тор, что бы при смене режима не было скачков
    {                       //
        Fzad_rmp = Frot;
//		prev_Fzad = Frot;
		reset_F_pid(); //Было ниже, может быть что-то было не так
		pidFvect.Ui = Iq;
		pidFvect.Out = Iq;
		*Iq_zad = Iq;
		Iq_rmp_to_optica = Iq;
//		*Id_zad = _IQdiv(_IQmpy(Iq, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
		if(!f.Go)
		{
			*Iq_zad = *Id_zad = 0;
		}
//		flag_Iq_synced_with_optica = 0;
        return; 
    }

    //Синхронизация парных ПЧ
    //Задание по Iq передаётся по оптической шине
//     if ((f.flag_leading == 0) && (f.read_task_from_optical_bus == 1) && (f.sync_Iq_from_optical_bus == 1)) {
// //        if (flag_Iq_synced_with_optica == 0) {
// //            if (_IQabs(analog.iqIq_zad_from_optica - Iq_rmp_to_optica) > 1118481) { //200A
// //                Iq_rmp_to_optica = zad_intensiv_q(step_Iq_rmp, step_Iq_rmp, Iq_rmp_to_optica, analog.iqIq_zad_from_optica);
// //            } else {
// //                Iq_rmp_to_optica = analog.iqIq_zad_from_optica;
// //                flag_Iq_synced_with_optica = 1;
// //            }
// //        } else {
// //            Iq_rmp_to_optica = analog.iqIq_zad_from_optica;
// //        }
//         Iq_rmp_to_optica = analog.iqIq_zad_from_optica;
//         Iq_out_unsat = Iq_rmp_to_optica;
//         Iq_out_sat = _IQsat(Iq_out_unsat, mzz_zad_int, -mzz_zad_int);
//         Id_out_unsat = _IQdiv(_IQmpy(_IQabs(Iq_out_sat), _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
//         Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0);
//         *Iq_zad = Iq_out_sat;
//         *Id_zad = Id_out_sat;

//         reset_F_pid();
//         pidFvect.Ui = Iq_out_sat;
//         pidFvect.Out = Iq_out_sat;
//         Fzad_rmp = Frot;
//         return;
//     }
	
    //if(Fzad != prev_Fzad) //Рампа зависит от режима. Если просто повышение, то медленно.
    //{						//Если понижение или реверс, то быстро.
        if(_IQabs(Fzad_rmp) <= _IQabs(Fzad) &&
        (((Fzad >= 0) && (Fzad_rmp >= 0)) || ((Fzad < 0 ) && (Fzad_rmp < 0))))
        {
            koef_rmp = koef_slow;
        }
        else
        {
            koef_rmp = koef_fast;
        }
//        prev_Fzad = Fzad;
    //}

 //   Fzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Fzad_rmp, Fzad);
//        logpar.log2 = (int16)(_IQtoIQ15(Fzad));
//        logpar.log19 = (int16)(_IQtoIQ15(Fzad_rmp));
    
    //В какой-то момент перестала корректно работать функция zad_intensiv_q
    // поэтому код из неё вставил сюда.
    deltaVar = Fzad-Fzad_rmp;

    if ((deltaVar>koef_rmp) || (deltaVar<-koef_rmp))
    {
      if (deltaVar>0) Fzad_rmp += koef_rmp;
      else Fzad_rmp -= koef_rmp;
    }
    else
      Fzad_rmp = Fzad;

	pidFvect.Ref = Fzad_rmp;
    pidFvect.Fdb = Frot;

    pidFvect.calc(&pidFvect);
    Iq_out_unsat = pidFvect.Out;

    //TODO: ATTENTION!!! worked better on stend
    if (_IQabs(Iq_out_unsat) < 27962) //5A
    {
        pidTetta.Ui = 0;
        pidTetta.SatErr = 0;
    }
    Iq_out_sat = _IQsat(Iq_out_unsat, Iq_out_max, -Iq_out_max);
//	if(f.DownToNominal) //In this mode max I equial Inom
//	{
//		Iq_out_sat = _IQsat(Iq_out_unsat, I_OUT_NOMINAL_IQ, -I_OUT_NOMINAL_IQ);
//	}
	// Iq_out_sat = _IQsat(Iq_out_unsat, mzz_zad_int, -mzz_zad_int);	//Test
	//Расчёт d составл-ющей тока
    Iq_out_unsat = _IQabs(Iq_out_unsat);
    Id_out_unsat = _IQdiv(_IQmpy(Iq_out_unsat, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
    Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0);



    *Iq_zad = Iq_out_sat;
    *Id_zad = Id_out_sat;
    
}