#include "teta_calc.h"
#include "params.h"
#include "pid_reg3.h"
#include "IQmathLib.h"
#include "vector.h"
#include "my_filter.h"

#define CONST_IQ_2PI  105414357 
#define PI 3.1415926535897932384626433832795

PIDREG3 pidTetta = PIDREG3_DEFAULTS;

#define MAX_Ud_Pid_Out	(167772 * 100)	//419430 ~ 0.5	//251658 ~ 0.3 //209715 ~ 0.25	//167772   ~ 0.2Hz	//100663	//0.12Hz
// 184549 ~ 2.2

#define MAX_Ud_Pid_Out_Id  176160   //0.2 ~ 167772 //0.21 ~ 176160
									//0.15 ~ 125829
#define BPSI_START 0.17	//0.15

void init_tetta_pid()
{
    pidTetta.Ref = 0;
	pidTetta.Fdb = 0;
    pidTetta.Kp = _IQ(0.1);	//_IQ(0.15);
    pidTetta.Ki = _IQ(0.0003);	//_IQ(0.00003)
	pidTetta.Kc = _IQ(0.5);
    pidTetta.OutMax = MAX_Ud_Pid_Out;
    pidTetta.OutMin = -MAX_Ud_Pid_Out;
	pidTetta.Up = 0;
	pidTetta.Ui = 0;
}

void reset_tetta_pid()
{
	pidTetta.Up = 0;
	pidTetta.Up1 = 0;
	pidTetta.Ui = 0;
	pidTetta.Out = 0;
}


//#pragma CODE_SECTION(calc_tetta,".fast_run");
void calc_tetta(_iq Frot, int direction, _iq Ud, int direct_zadan, _iq *tetta_out, _iq *Fsl_out, _iq *Fstator_out, int mode, int reset) //
{
    static _iq tetta = 0, Fsl_start = 0, bpsi_start = _IQ(BPSI_START / NORMA_FROTOR);// , bpsi_start_on_go = _IQ(0.1 / NORMA_FROTOR);
    static _iq hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM / 2.0);
	static int flag_start = 0;	
	static int koeff_zad_int = 30; //1000;
	static long stop_frot = 27962;	//2 rpm	// 10000;
//	static _iq prev_Fsl = 0;
	static int flag_reverse = 0;
//	static int flag_out_of_reverse = 0;
	static int prev_direction = 0;
	static _iq koeffKpRmp = _IQ(0.0003750);
	static _iq koeffKiRmp = _IQ(0.00005);
	static _iq lowSpeedKp = _IQ(0.1);
	static _iq lowSpeedKi = _IQ(0.0003);
	static _iq highSpeedKp = _IQ(0.01);
	static _iq highSpeedKi = _IQ(0.011);	//_IQ(0.02);

    _iq Fstat, Fsl;
    int reverse_Ud = 1;

	if(reset == 1)
	{
		flag_start = 1;  //Won`t work, if machine will stop without changing mode
		Fsl_start = 0;
		flag_reverse = 0;
//		flag_out_of_reverse = 0;
	}
	if (_IQabs(Frot) < 768955) {	//55 rpm
		pidTetta.Kp = zad_intensiv_q(koeffKpRmp, koeffKpRmp, pidTetta.Kp, lowSpeedKp);
		pidTetta.Ki = zad_intensiv_q(koeffKiRmp, koeffKiRmp, pidTetta.Ki, lowSpeedKi);
	}
	else if ((_IQabs(Frot) < 1118481)) {	//80 rpm
		pidTetta.Kp = zad_intensiv_q(koeffKpRmp, koeffKpRmp, pidTetta.Kp, highSpeedKp);
		pidTetta.Ki = zad_intensiv_q(koeffKiRmp, koeffKiRmp, pidTetta.Ki, highSpeedKi);
	}
	//прюмое направление direction == 1
	//direction == 0 - двигатель стоит
//    if(((Frot >= 0) && (direct_zadan == 1) && (direction >= 0)) ||
//    	((direct_zadan == -1) && (direction >= 0) && (Frot >= stop_frot)))
	//27962 ~ 2 rpm
//	if(((direct_zadan == 1) && (direction == 0)) || (direction > 0))
//	{
//        Ud = -Ud;
//    }


	if(Frot >= 27962) {
		reverse_Ud = -1;
		flag_reverse = (Frot >= 27962 && direct_zadan == -1) ? 1 : 0;
		prev_direction = 1;
	}
	else if (Frot <= -27962) {
		reverse_Ud = 1;
		flag_reverse = (Frot <= -27962 && direct_zadan == 1) ? 1 : 0;
		prev_direction = -1;
	}
	if(_IQabs(Frot) < 27962) {
		if (flag_reverse == 1) {
			if (prev_direction == 1) {
				reverse_Ud = -1;
			} else {
				reverse_Ud = 1;
			}
		} else
//			if (flag_start)
		{
			if (direct_zadan == 1) {
				reverse_Ud = -1;
			} else {
				reverse_Ud = 1;
			}
		}
	}
	Ud = Ud * reverse_Ud;

	pidTetta.Ref = Ud * 100;		//Ref -заданое
    pidTetta.calc(&pidTetta);
   	Fsl = pidTetta.Out / 100;

	if(flag_start)	//
    {
        if(_IQabs(Frot) < stop_frot)
        {
			Fsl_start = bpsi_start * direct_zadan; //Если двигатель стоит, прибавл-ем скольжение.
        }
        else
        {
        	if(Fsl_start == 0)
        	{
				Fsl_start = bpsi_start * direct_zadan;	//direction;
        	}
        	flag_start = 0;
        }

    }
	else
	{
		//Когда переходим через ноль, нужно добавить скольжение, т.к. регуляторы сами не поддерживают правильное состояние.
		if(_IQabs(Frot) < (stop_frot))
		{
//			if((direct_zadan == 1) && (Fsl + Fsl_start < bpsi_start))
//			{
//				Fsl_start += (koeff_zad_int << 2);
////				Fsl_start += koeff_zad_int;
//			}
//			if((direct_zadan == -1) && (Fsl + Fsl_start > -bpsi_start))
//			{
//				Fsl_start -= (koeff_zad_int << 2);
////				Fsl_start -= koeff_zad_int;
//			}
		}
		else
		{
			if(_IQabs(Fsl_start) > koeff_zad_int)
			{
				if(Fsl_start > 0)
				{
					Fsl_start -= koeff_zad_int;
				}
				if(Fsl_start < 0)
				{
					Fsl_start += koeff_zad_int;
				}
			}
			else
			{
				Fsl_start = 0;
			}
		}

	}
	Fsl += Fsl_start;
    Fstat = Frot * POLUS + Fsl;
	tetta += _IQmpy(Fstat, hz_to_angle);


	if (tetta > CONST_IQ_2PI)
	{
		tetta -= CONST_IQ_2PI;
	}
	
	if (tetta < 0)
	{
		tetta += CONST_IQ_2PI;
	}
	*tetta_out = tetta;
	*Fsl_out = Fsl;
//	prev_Fsl = Fsl;
	*Fstator_out = Fstat;


}

TETTA_CALC tetta_calc = TETTA_CALC_DEF;

void init_tetta_calc_struct()
{
    tetta_calc.Imds = 0;
    tetta_calc.tetta = 0;
    tetta_calc.k_r = _IQ(0.015);
    tetta_calc.k_t = _IQ(0.0045);	//_IQ(0.0045);
    tetta_calc.hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM / 2.);
}

void calc_tetta_Id(_iq Frot, _iq Id, _iq Iq, _iq *tetta_out, _iq *Fsl_out, _iq *Fstator_out, int reset) {

    _iq Fsl, Fst;
    if (reset) {
        tetta_calc.Imds = 0;
    }

    tetta_calc.Imds = tetta_calc.Imds + _IQmpy(tetta_calc.k_r, (Id - tetta_calc.Imds));
    Fsl = _IQmpy(tetta_calc.k_t, Iq);
    if (tetta_calc.Imds != 0) {
        Fsl = _IQdiv(Fsl, tetta_calc.Imds);
    } else {
        Fsl = 0;
    }
    // Fsl = _IQ(0.2 / NORMA_FROTOR);
    if (Fsl > MAX_Ud_Pid_Out_Id) { Fsl = MAX_Ud_Pid_Out_Id;}
    if (Fsl < -MAX_Ud_Pid_Out_Id) { Fsl = -MAX_Ud_Pid_Out_Id;}

    Fst = Frot * POLUS + Fsl;
    tetta_calc.tetta += _IQmpy(Fst, tetta_calc.hz_to_angle);

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

    if (tetta_calc.tetta < 0)
    {
        tetta_calc.tetta += CONST_IQ_2PI;
    }
    *Fsl_out = Fsl;
    *tetta_out = tetta_calc.tetta;
    *Fstator_out = Fst;


}

#define LEVEL_REDUCE_UD	838860	//150V

void correct_tetta_calc_Kt(void) {
	static _iq coeff_add_Kt = _IQ(0.00005);
	static _iq max_Kt = _IQ(0.0069);
	static _iq min_Kt = _IQ(0.0029);

	if (tetta_calc.Imds  > LEVEL_REDUCE_UD) {
		tetta_calc.k_t -= coeff_add_Kt;
	}
	if (tetta_calc.Imds < -LEVEL_REDUCE_UD) {
		tetta_calc.k_t += coeff_add_Kt;
	}
	if (tetta_calc.k_t > max_Kt) {tetta_calc.k_t = max_Kt; }
	if (tetta_calc.k_t < min_Kt) {tetta_calc.k_t = min_Kt; }
}