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

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


#define IQ_OUT_SAT_POWER (2880.0 * COS_FI)
#define MAX_PID_CURRENT_P 2200.0	// 2200.0 ~ 1.6 Inom

#define IM_CURRENT_STOP_RMP 11184810	//2000

#define IQ_1500A    8388608
#define IQ_1800A    10066329

#define K_RMP_100A_PER_TIC 559240
#define K_RMP_400A_PER_SEC 2663

#define K_MODUL_MAX 15770583

// #define P_DELTA_ZAD_IZM 5592405 //3 MWt

#define LIMIT_Iq_ON_POWER_REVERSE

_iq Id_out_max_low_speed = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP);
_iq Id_out_max_full = _IQ(ID_OUT_SAT_POWER / NORMA_ACP);


void init_Pvect_pid()
{
    pidPvect.Ref = 0;
    pidPvect.Kp = _IQ(1.0);//_IQ(1.0);
    pidPvect.Ki = _IQ(0.1);//_IQ(0.05);//_IQ(0.06);
    pidPvect.Kc = _IQ(0.5);
    // pidPvect.Kp = _IQ(1); //_IQ(2.5);//_IQ(5.0);//
    // pidPvect.Ki = _IQ(0.1);//_IQ(0.05);//_IQ(0.06);
    // pidPvect.Kc = _IQ(0.5);
    pidPvect.OutMax = _IQ(MAX_PID_CURRENT_P / NORMA_ACP);
    pidPvect.OutMin = -_IQ(MAX_PID_CURRENT_P / NORMA_ACP);
}

void reset_Pvect_pid()
{
	pidPvect.Up = 0;
	pidPvect.Up1 = 0;
	pidPvect.Ui = 0;
	pidPvect.Ud = 0;
	pidPvect.Out = 0;
}

_iq koeff_Iq_filter = _IQ(0.5);

#define LEVEL_LIMIT_KM  14596177    //87%	//15099494 ~ 90%	//15435038 ~ 92%

#define POWER_7_MWt 13048945
#define POWER_4_MWt 7456540
#define POWER_200_kWt   372827

// #pragma CODE_SECTION(vector_power,".fast_run2");
void vector_power(_iq Pzad, _iq P_measured, _iq Iq_measured, int mode, _iq Frot, _iq* Iq_zad, _iq* Id_zad)
{
    static _iq Pzad_rmp = 0;
    static _iq koef_slow = _IQ(0.000075);  //_IQ(0.000065);15sec	//_IQ(0.000085); 13sec		// 	//_IQ(0.000065); normal rampa 13 seconds
    static _iq koef_slow_low_task = _IQ(0.000040);  //Ðàìïà äëÿ çàäàíèé ìîùíîñòè ìåíüøå ïîëîâèíû íîìèíàëà, ÷òîáû íåáûëî ïåðåðåãóëÿöèè
    static _iq koef_slow_2 = _IQ(0.000045);	//_IQ(0.000039);			// _IQ(0.000015);	//_IQ(0.000011) ~ 10 sec 0-10MWt	//_IQ(0.00002) ~ 7 ñåê 0-10ÌÂò
    static _iq koef_fast = _IQ(0.00013);	//_IQ(0.00008);			//_IQ(0.000025); ~3.1 sev 10-0MWt
//    static _iq prev_Pzad = 0;
//    static _iq cos_fi_nom_alg = _IQ(COS_FI), cos_fi_nom_squared_alg = _IQ(COS_FI * COS_FI);
//    static _iq cos_fi_nom = _IQ(COS_FI), cos_fi_nom_squared = _IQ(COS_FI * COS_FI);
    static _iq Iq_out_nom = _IQ(IQ_OUT_NOM / NORMA_ACP);	//, Id_out_nom = _IQ(ID_OUT_NOM / NORMA_ACP);
//    static _iq Iq_out_max = _IQ(IQ_OUT_SAT_POWER / NORMA_ACP);
    static _iq Id_out_max = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP);
   static _iq pid_Out_max = _IQ(MAX_PID_CURRENT_P / NORMA_ACP);
    _iq koef_rmp, koef_spad;		//koef_spad - äëþ çàùèòû îò ïðåâûøåíèþ âèíòîì ðàçðåø¸ííûõ îáîðîòîâ (170îá/ìèí)
    _iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat;
//    _iq Iq_out_limited = 0;
	static _iq Iq_out_filter = 0;
	static _iq mzz_zad_int=0;
	static int counterStopRampa = 0;
#ifdef LIMIT_Iq_ON_POWER_REVERSE
	static int flag_reverse = 0;
#endif  //LIMIT_Iq_ON_POWER_REVERSE

	// if (f.DownTemperature) {
	// 	mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, _IQmpy(f.iq_mzz_zad, temperature_limit_koeff));
	// } else 
//     if(f.DownToNominal && (f.iq_mzz_zad > Iq_out_nom)) {
// 		mzz_zad_int = zad_intensiv_q(29480, 29480, mzz_zad_int, Iq_out_nom);
// 	} else if (a.iqk1 > LEVEL_LIMIT_KM || a.iqk2 > LEVEL_LIMIT_KM) {
// 	    Pzad = 0;
// 	    Pzad_rmp = zad_intensiv_q((koef_fast << 3), (koef_fast << 3), Pzad_rmp, Pzad);
// //	    f.DownToNominalVoltage = 1;
// 	}
// 	else
	{
		if (f.iq_mzz_zad != 0) {
            mzz_zad_int = zad_intensiv_q(28480, 28480, mzz_zad_int, f.iq_mzz_zad);
        } else {
            mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, pid_Out_max);
        }
//		f.DownToNominalVoltage = 0;
	}

    if((mode != 2) || (!f.Go)
//    || (f.flag_leading == 0 && f.read_task_from_optical_bus == 1 && f.sync_Iq_from_optical_bus == 1)
    )
    {
        Pzad_rmp = P_measured;
//		prev_Pzad = P_measured;
		pidPvect.Ui = Iq_measured;
		Id_out_max = Id_out_max_low_speed;
		if(!f.Go)
		{
			*Iq_zad = *Id_zad = 0;
		}
        #ifdef LIMIT_Iq_ON_POWER_REVERSE
		flag_reverse = 0;
        #endif
        return;
    }

#ifdef LIMIT_Iq_ON_POWER_REVERSE
    // Ïðè òîðîæåíèè îãðàíè÷èâàåì Iq íóë¸ì ñî ñòîðîíû íå òîðìîæåíèÿ
    // ò.ê. ïðè òîðìîæåíèè èç-çà êîëåáàíèé çàäàíèå íà òîê ìîæåò ïîìåíÿòü çíàê
    if ((Frot > 0 && Pzad_rmp < 0 && pidPvect.Out < 0) ||
            (Frot < 0 && Pzad_rmp > 0 && pidPvect.Out > 0)) {
        flag_reverse = 1;
        vect_control.flag_reverse = 1;
    } else
        if ((Frot > 0 && Pzad_rmp > 0) || (Frot < 0 && Pzad_rmp < 0))
    {
        flag_reverse = 0;
        vect_control.flag_reverse = 0;
    }
    if (flag_reverse == 0) {
        pidPvect.OutMax = mzz_zad_int;
        pidPvect.OutMin = -mzz_zad_int;
    } else {
        if (Pzad_rmp < 0) {
            pidPvect.OutMax = 0;
            pidPvect.OutMin = -mzz_zad_int;
        } else {
            pidPvect.OutMax = mzz_zad_int;
            pidPvect.OutMin = 0;
        }
    }
#else
    pidPvect.OutMax = mzz_zad_int;
    pidPvect.OutMin = -mzz_zad_int;
#endif  //LIMIT_Iq_ON_POWER_REVERSE

//    if (f.setCosTerminal) {
//    	cos_fi.cos_fi_nom = f.cosinusTerminal;
//    	cos_fi.cos_fi_nom_squared = f.cosinusTerminalSquared;
//    }


    //if(Pzad != prev_Pzad)
    //{
        if((_IQabs(Pzad_rmp) <= _IQabs(Pzad)) &&
            (((Pzad >= 0) && (Pzad_rmp >= 0)) || ((Pzad < 0) && (Pzad_rmp < 0))))
        {
            // if (_IQabs(Pzad) < POWER_4_MWt) {
            //     koef_rmp = koef_slow_low_task;
            // } else if (_IQabs(Pzad_rmp) < POWER_7_MWt) {  // 7MWt
            //     koef_rmp = koef_slow;
            // } else {
            //     koef_rmp = koef_slow_2;
            // }
            koef_rmp = koef_slow;
            //Ïðè íàáðîñå ìîùíîñòè ýëåêòðè÷åñêàÿ ìîùíîñòü ìîæåò ïðåâûñèòü çàäàííóþ
            //ïîýòîìó åñëè ïðèáëèçèëèñü ê çàäàíèþ, òî çàìåäëÿåì ðàìïó âåêòîðîé ìîùíîñòè
            // if (_IQabs(f.iq_p_zad_electric) - _IQabs(analog.iqW) < POWER_200_kWt) {
            //     koef_rmp = koef_rmp / 4;
            // }
        }
        else
        {
            koef_rmp = koef_fast;
        }
    //}

//    EfficiencyCompensation(&Pzad, &Iq_measured);

 //    if (analog.iqIm_1 > IM_CURRENT_STOP_RMP || analog.iqIm_2 > IM_CURRENT_STOP_RMP) {
 //    	counterStopRampa = 400;
 //    } else {
 //    	if (counterStopRampa > 0) {
	// 		counterStopRampa -= 1;
	// 	}
 //    }
 //    if (counterStopRampa <= 0) {
	// 	Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Pzad_rmp, Pzad);
	// 	counterStopRampa = 0;
	// } else {
	// 	Pzad_rmp = zad_intensiv_q(koef_slow_2, koef_slow_2, Pzad_rmp, Pzad);
	// }

    Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Pzad_rmp, Pzad);
    
#ifdef P_DELTA_ZAD_IZM
    //Åñëè èçìåðåííàÿ ìîùíîñòü íà ìíîãî ìåíüøå çàäàííîé ðàìïû,
    //òî ðàìïà îãðàíè÷èâàåòñÿ çíà÷åíèåì èçìåðåííîé ïëþñ íåêîòîðàÿ äåëüòà
    if ((Pzad_rmp > 0 && P_measured > 0) &&
            ((Pzad_rmp - P_measured) > P_DELTA_ZAD_IZM)) {
        Pzad_rmp = P_measured + P_DELTA_ZAD_IZM;
    }
    if ((Pzad_rmp < 0 && P_measured < 0) &&
        ((Pzad_rmp - P_measured) < -P_DELTA_ZAD_IZM)) {
        Pzad_rmp = P_measured - P_DELTA_ZAD_IZM;
    }
#endif

    f.iq_p_rampa = Pzad_rmp;


    //������ �� ���������� ������ ����������� ��������
	if(_IQabs(Frot) > IQ_170_RPM)
	{
		koef_spad = _IQdiv((IQ_170_RPM + IQ_10_RPM - _IQabs(Frot)),  IQ_10_RPM);
		if(koef_spad < 0) {
		    koef_spad = 0;
		}
		pidPvect.OutMax = _IQmpy(pidPvect.OutMax, koef_spad);
		pidPvect.OutMin = _IQmpy(pidPvect.OutMin, koef_spad);
		Pzad_rmp = _IQmpy(Pzad_rmp, koef_spad);
	}
	pidPvect.Ref = Pzad_rmp;
    pidPvect.Fdb = P_measured;


    pidPvect.calc(&pidPvect);

    Iq_out_unsat = pidPvect.Out;

	Iq_out_filter = Iq_out_unsat;   // exp_regul_iq(koeff_Iq_filter, Iq_out_filter, Iq_out_unsat);

    // ìîäåëè çäåñü íå áûëî îãðàíè÷åíèé
	Iq_out_sat = _IQsat(Iq_out_filter, mzz_zad_int, -mzz_zad_int);	//Test
	
    if(Iq_out_filter < 0)
    {
        Id_out_unsat = _IQdiv(_IQmpy((-Iq_out_filter), _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
    } else {
        Id_out_unsat = _IQdiv(_IQmpy(Iq_out_filter, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
    }

    if (_IQabs(Frot) < IQ_50_RPM) {
    	Id_out_max = zad_intensiv_q(5592, 5592, Id_out_max, Id_out_max_low_speed);  //????
    } else {
    	Id_out_max = zad_intensiv_q(5592, 5592, Id_out_max, Id_out_max_full);
    }
    Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0);
    
//    prev_Iq_zad = Iq_out_sat;

    *Iq_zad = Iq_out_sat;
    *Id_zad = Id_out_sat;

}