#include "IQmathLib.h"
#include "alg_pll.h"
#include "params_pll.h"

#include "params_norma.h"

//#define NORMA_ACP 3000
//#define FREQ_PWM_VIPR 1975

//#define SIZE_PLL_AVG	50

//_iq w_in_avg[SIZE_PLL_AVG];
//_iq w_out_avg[SIZE_PLL_AVG];

#define DETECT_PLL_D (2000.0/NORMA_ACP) // ampl
#define DETECT_PLL_Q (500.0/NORMA_ACP)  // zero

_iq iqDetect_PLL_d=_IQ(DETECT_PLL_D);
_iq iqDetect_PLL_q=_IQ(DETECT_PLL_Q);

#define MAX_COUNT_ERR_PLL	5000 //20

#ifdef USE_SMOOTH_FOR_CALC_WC
SMOOTH mysmooth=SMOOTH_DEFAULTS;
#endif


//
///////////////////////////////////////////
//PLL_REC pll2 = PLL_REC_DEFAULT;
///////////////////////////////////////////


ABC_TO_ALPHABETA abc_to_alphabeta_u_input = ABC_TO_ALPHABETA_DEFAULTS;
ALPHABETA_TO_DQ	alphabeta_to_dq_u_input = ALPHABETA_TO_DQ_DEFAULTS; 
PIDREG3 pidUdq = PIDREG3_DEFAULTS;

//int count_wait_pll=0;
int count_err_pll = 0;


//int c_start=0;
//int c_stop=0;

#define MAX_TIME_WAIT_PLL 5000


//_iq iqUab=0, iqUbc=0, iqUca=0;


//_iq koef_Um_filter    =  _IQ(0.000125/0.09);


//_iq koef_AddIq_minus_1_filter = _IQ(0.00034/0.009);//9576244354248046875

#pragma CODE_SECTION(minus_plus_2_pi,".fast_run");
_iq minus_plus_2_pi(_iq a)
{

  while (a>=CONST_IQ_2PI)
  	a -= CONST_IQ_2PI;

  while (a<=-CONST_IQ_2PI)
  	a += CONST_IQ_2PI;

  return a;
}


#pragma CODE_SECTION(minus_plus_2_pi_v2,".fast_run");
_iq minus_plus_2_pi_v2(_iq a)
{

  while (a>=CONST_IQ_2PI)
  	a -= CONST_IQ_2PI;

  while (a<0)
  	a += CONST_IQ_2PI;

  return a;
}



#pragma CODE_SECTION(AB_BC_CA_To_ABC,".fast_run");
void AB_BC_CA_To_ABC(_iq U_AB, _iq U_BC, _iq U_CA, _iq *Ua, _iq *Ub, _iq *Uc)
{
//    static _iq c2 = _IQ(2.0);

//    static _iq c13_sqrt = _IQ(1.7320508075688772935274463415059 / 3.0);

	*Ua = U_AB;
	*Ub = U_BC;
	*Uc = U_CA;
/*

    
	*Ua = _IQmpy(c13_sqrt, (_IQmpy(c2,U_AB)+U_BC));//     2*U_AB/3+U_BC/3;
	*Ub = _IQmpy(c13_sqrt, (_IQmpy(c2,U_BC)+U_CA));//     2*U_BC/3+U_CA/3;
	*Uc = _IQmpy(c13_sqrt, (_IQmpy(c2,U_CA)+U_AB));//     2*U_CA/3+U_AB/3;
*/
}




/////////////////////////////////////////////////
#pragma CODE_SECTION(PLLController,".fast_run");
/////////////////////////////////////////////////
void PLLController(PLL_REC *v)
{
	static unsigned int count_wait_pll=0;
	static _iq prev_Tetta_z=0;
	_iq Tetta_z_t=0;
	static int prev_flag_find_pll = 0;
	static int flag_reset_Tetta_p = 0;
//	static _iq prev_Tetta_v2 = 0;


	v->vars.Uab = v->input.Input_U_AB - v->vars.iqZeroUAB;
	v->vars.Ubc = v->input.Input_U_BC - v->vars.iqZeroUBC;
	v->vars.Uca = v->input.Input_U_CA - v->vars.iqZeroUCA;

	v->vars.sum_zeroU_AB_BC_CA = v->vars.Uab + v->vars.Ubc + v->vars.Uca;

	if (v->setup.rotation_u_cba)
	{
		AB_BC_CA_To_ABC(v->vars.Uab, v->vars.Uca, v->vars.Ubc, &v->vars.Ua, &v->vars.Ub, &v->vars.Uc);
	}
	else
	{
		AB_BC_CA_To_ABC(v->vars.Uab,v->vars.Ubc,v->vars.Uca, &v->vars.Ua, &v->vars.Ub, &v->vars.Uc);
	}
#ifdef ROTATION_U_CBA
	
#else
	
#endif

	v->vars.sum_zeroU_A_B_C = v->vars.Ua + v->vars.Ub + v->vars.Uc;

	abc_to_alphabeta_u_input.Ua = v->vars.Ua;
	abc_to_alphabeta_u_input.Ub = v->vars.Ub;
	abc_to_alphabeta_u_input.Uc = v->vars.Uc;

	abc_to_alphabeta_u_input.calc(&abc_to_alphabeta_u_input);


	v->vars.Ualpha = abc_to_alphabeta_u_input.Ualpha;
	v->vars.Ubeta  = abc_to_alphabeta_u_input.Ubeta;


	alphabeta_to_dq_u_input.Ualpha = v->vars.Ualpha;
	alphabeta_to_dq_u_input.Ubeta  = v->vars.Ubeta;
	alphabeta_to_dq_u_input.Tetta  = v->vars.Tetta;

	alphabeta_to_dq_u_input.calc(&alphabeta_to_dq_u_input);


	v->vars.pll_Ud = alphabeta_to_dq_u_input.Ud;
	v->vars.pll_Uq = alphabeta_to_dq_u_input.Uq;

 
//	pidUdq.Fdb = v->pll_Ud;//.pll_Uq;    //err = Ref - Fdb
//	pidUdq.Ref = 0;

	pidUdq.Ref = v->vars.pll_Uq;    //err = Ref - Fdb
	pidUdq.Fdb = 0;
	
	pidUdq.calc(&pidUdq);  	
	v->vars.wc = pidUdq.Out;


	 if (prev_flag_find_pll==0)
	 {
		flag_reset_Tetta_p = 0;
	 }

	 if (v->output.flag_find_pll)
	 {
#ifdef USE_SMOOTH_FOR_CALC_WC
       mysmooth.input = _IQtoIQ23(v->vars.wc);
	   mysmooth.add(&mysmooth);
	   mysmooth.calc(&mysmooth);
	   v->vars.w_shtrih = _IQ23toIQ(mysmooth.av);
#else
	  v->vars.w_shtrih = v->vars.wc;
#endif
	 }
	 else
	 {
	  v->vars.w_shtrih = v->vars.wc;
	 }

    	
	 v->vars.Tetta += v->vars.wc;
	 v->vars.Tetta = minus_plus_2_pi(v->vars.Tetta); // +- 2PI

	if (v->output.flag_find_pll)
	{
	 v->vars.dwc = v->vars.wc - v->vars.w_shtrih;

	 v->vars.Tetta_p += v->vars.dwc;
	 v->vars.Tetta_p = minus_plus_2_pi(v->vars.Tetta_p); // +- 2PI

	 v->vars.dTetta = v->vars.Tetta - v->vars.Tetta_p;// + iq_05Pi;

	 v->vars.dTetta = minus_plus_2_pi(v->vars.dTetta); // +- 2PI

	 v->vars.Tetta_z = v->vars.dTetta;

//   if (v->Tetta_z>=iq_05Pi && prev_Tetta_z<iq_05Pi)
//	     v->Tetta_p = 0;

     Tetta_z_t = minus_plus_2_pi_v2(v->vars.Tetta_z);

     if ( (Tetta_z_t>=0) && (Tetta_z_t<CONST_IQ_PI05) && ( (prev_Tetta_z>(CONST_IQ_2PI-CONST_IQ_PI05))    )  )
	 {
		if (flag_reset_Tetta_p==0)
		{
	       v->vars.Tetta_p = 0;
//		   flag_reset_Tetta_p   = 1;
		}
	 }
	
		
	 prev_Tetta_z = Tetta_z_t;

#ifdef USE_FILTER_TETTA
//use filter teta
	 v->vars.Tetta_v2 = v->vars.Tetta_z;//v->Tetta;
#else
//use mgnoven teta
	 v->vars.Tetta_v2 = v->vars.Tetta;
#endif
	 v->vars.delta_Tetta_c = v->vars.Tetta_z - v->vars.Tetta;

//	 prev_Tetta_v2 = v->Tetta_v2;

    }
	else
	{
	  v->vars.Tetta_v2 = v->vars.Tetta;
	  flag_reset_Tetta_p = 0;
	}


// PLL OK?
//count_wait_pll=0
//new alg find pll
	if (v->vars.w_shtrih >= v->vars.find_min_w_strih && v->vars.w_shtrih <= v->vars.find_max_w_strih)
	{
		if (v->vars.count_wait_pll_w_shtrih < v->vars.max_time_wait_pll_w_strih)
		   v->vars.count_wait_pll_w_shtrih++;
	}
	else
	{
		if (v->vars.count_wait_pll_w_shtrih>0)
		  v->vars.count_wait_pll_w_shtrih--;
	}

	if (v->vars.count_wait_pll_w_shtrih == v->vars.max_time_wait_pll_w_strih)
	  v->output.flag_find_pll = 1;

	if (v->vars.count_wait_pll_w_shtrih == 0)
	  v->output.flag_find_pll = 0;

//end new alg find pll



    if ( (_IQabs(v->vars.pll_Uq)<=_IQabs(iqDetect_PLL_q))   // zero
	     && (_IQabs(v->vars.pll_Ud)>=_IQabs(iqDetect_PLL_d) ) //ampl
	   )
    {
	    count_err_pll=0;
		if (count_wait_pll<MAX_TIME_WAIT_PLL)
		   count_wait_pll++;
		else		  
		// ok, pll finded 
		   v->output.flag_find_pll = 1;
	}
	else
	{
		if (count_err_pll>=MAX_COUNT_ERR_PLL)
		{
	  // fail find pll
		 count_wait_pll=0; 

	     v->output.flag_find_pll = 0;

		 if (v->output.flag_find_pll==1)
		 {
			v->vars.pll_Uq = 0;
			v->vars.pll_Ud = 0;
		 }

		}
		else
		{
		  count_err_pll++;
		  if ((v->output.flag_find_pll==0) && (count_wait_pll>0))
		   count_wait_pll--;
		}
	}

// end PLL Ok?


	v->vars.pi_teta_u_out = pidUdq.Out;
	v->vars.pi_teta_u_i = pidUdq.Ui;
	v->vars.pi_teta_u_p = pidUdq.Up;


	prev_flag_find_pll = v->output.flag_find_pll;
}

//////////////////////////////////////////////////
//////////////////////////////////////////////////
//////////////////////////////////////////////////
/////////////////////////////////////////////////
//#pragma CODE_SECTION(pll_get_freq,".fast_run");
/////////////////////////////////////////////////
void pll_get_freq_float(PLL_REC *v)
{

	if (v->output.flag_find_pll)
	{
		v->output.int_freq_net = _IQtoF( v->vars.w_shtrih) * v->setup.freq_run_pll / PI * 50.00; // freq*100
	}
	else
	{
	  v->output.int_freq_net = 0;
	}

}

//////////////////////////////////////////////////
//////////////////////////////////////////////////
void pll_get_freq_iq(PLL_REC *v)
{

    if (v->output.flag_find_pll)
    {
        v->output.iq_freq_net = v->vars.w_shtrih;//_IQtoF( v->vars.w_shtrih) * v->setup.freq_run_pll / PI * 50.00; // freq*100
    }
    else
    {
      v->output.iq_freq_net = 0;
    }

}

//////////////////////////////////////////////////
//#pragma CODE_SECTION(detect_phase_count,".fast_run");
void detect_phase_count(PLL_REC *v)
{
	static _iq prev_Ua = 0;
	static int prev_flag_find_pll=0;



//	abc_to_dq.Ua

	if ((v->output.flag_find_pll != prev_flag_find_pll) && (v->output.flag_find_pll == 1))
	{
		prev_Ua = v->vars.Ua;
		v->vars.enable_detect_phase_count = 1;
		v->vars.error_phase_count = 0;
	}


	if (v->output.flag_find_pll==0)
	   v->vars.enable_detect_phase_count = 0;


	if (v->output.flag_find_pll)
	{
		if (v->vars.enable_detect_phase_count)
		{
			if ( (prev_Ua<0) && (v->vars.Ua>=0) )
			{

				if (v->vars.Uc > v->vars.Ub)
				  v->vars.enable_detect_phase_count = 0;

				if (v->vars.Ub > v->vars.Uc)
				{
				  v->vars.enable_detect_phase_count = 0;
				  v->vars.error_phase_count = 1;
				}
			}
		}
	}


	prev_flag_find_pll = v->output.flag_find_pll;
	prev_Ua = v->vars.Ua;

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


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

#pragma CODE_SECTION(Find_zero_Uabc,".fast_run");
void Find_zero_Uabc(PLL_REC *v)
{
	static int c_a=0;
//	static int c_b=0;
//	static int c_c=0;

	static _iq sum_a=0;
	static _iq sum_b=0;
	static _iq sum_c=0;

	_iq22 	sum_t,c_t;

	_iq22	sum_div;


//	AB_BC_CA_To_ABC(analog.iqUin_AB-iqZeroUABC, analog.iqUin_BC-iqZeroUABC, analog.iqUin_CA-iqZeroUABC);

	
    sum_a += v->input.Input_U_AB; //   analog.iqUin_AB;	
    sum_b += v->input.Input_U_BC; //   analog.iqUin_BC;	
    sum_c += v->input.Input_U_CA; //   analog.iqUin_CA;

	c_a++;

	if (c_a >= v->vars.count_sum_find_zero_uabc)
	{
	   sum_div = v->vars.sum_div_find_zero_uabc;

	   sum_t = _IQtoIQ22(sum_a);
	   c_t   = _IQ22div(sum_t,sum_div);
	   v->vars.iqZeroUAB = _IQ22toIQ(c_t);

	   sum_t = _IQtoIQ22(sum_b);
	   c_t   = _IQ22div(sum_t,sum_div);
	   v->vars.iqZeroUBC = _IQ22toIQ(c_t);

	   sum_t = _IQtoIQ22(sum_c);
	   c_t   = _IQ22div(sum_t,sum_div);
	   v->vars.iqZeroUCA = _IQ22toIQ(c_t);

	   sum_a = 0;
	   sum_b = 0;
	   sum_c = 0;
	   c_a = 0;
	}

}





void pll_init(PLL_REC *v)
{
   	v->output.status = STATUS_PLL_NOT_INITED;

	abc_to_alphabeta_u_input.Ua = 0;
	abc_to_alphabeta_u_input.Ub = 0;
	abc_to_alphabeta_u_input.Uc = 0;
	abc_to_alphabeta_u_input.Ualpha = 0;
	abc_to_alphabeta_u_input.Ubeta = 0;

	alphabeta_to_dq_u_input.Tetta = 0;
	alphabeta_to_dq_u_input.Ualpha = 0;
	alphabeta_to_dq_u_input.Ubeta = 0;
	alphabeta_to_dq_u_input.Ud = 0;
	alphabeta_to_dq_u_input.Uq = 0;

	v->vars.count_wait_pll_w_shtrih = 0;

	v->vars.pll_Ud = 0;
	v->vars.pll_Uq = 0;
	v->vars.Tetta  = 0;
	v->vars.Tetta_p = 0;

	v->vars.Ua = 0;
	v->vars.Ub = 0;
	v->vars.Uc = 0;

	v->vars.Ualpha = 0;
	v->vars.Ubeta = 0;

//    count_wait_pll = 0;
	count_err_pll  = 0;
	v->output.flag_find_pll = 0;

	pidUdq.Kp = _IQ(v->setup.pid_kp_pll);
	pidUdq.Ki = _IQ(v->setup.pid_ki_pll);

	pidUdq.Kc = _IQ(PID_KC_PLL);
	pidUdq.Kd = _IQ(0.0);

	pidUdq.OutMax = _IQ(K_PLL_MAX);
	pidUdq.OutMin = _IQ(K_PLL_MIN);	

	pidUdq.Err = 0;
	pidUdq.Out = 0;
	pidUdq.OutPreSat = 0;
	pidUdq.SatErr = 0;

	if (v->setup.freq_run_pll == 0)
	  v->setup.freq_run_pll = DEFAULT_FREQ_RUN_PLL;

    pidUdq.Ui = _IQ(2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll));

//	iqDetect_PLL_d = iqDetect_PLL_d;
//	iqDetect_PLL_q = iqDetect_PLL_q;

#ifdef USE_SMOOTH_FOR_CALC_WC
	mysmooth.init(&mysmooth);
#endif


	v->vars.count_sum_find_zero_uabc = v->setup.freq_run_pll/DEFAULT_FREQ_NET; //79*2 //1975/50*2 
	v->vars.sum_div_find_zero_uabc = _IQ22(v->vars.count_sum_find_zero_uabc);

    v->vars.find_max_w_strih = _IQ(FIND_MAX_W_STRIH*2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll));
    v->vars.find_min_w_strih = _IQ(FIND_MIN_W_STRIH*2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll));

    v->vars.max_time_wait_pll_w_strih = (v->vars.count_sum_find_zero_uabc * MAX_PERIOD_WAIT_PLL_W_SHTRIH);

   	v->output.status = STATUS_PLL_INITED;

}




#pragma CODE_SECTION(read_error_find_pll,".fast_run2");
int read_error_find_pll(PLL_REC *v)
{
//	static int enable_detect_pll_err=0;
//	static int prev_flag_find_pll=0;
	static int err_pll=0;


    err_pll = 0;
/*
	if ((v->output.flag_find_pll!=prev_flag_find_pll) 
	     && (v->output.flag_find_pll==1))
    {
		enable_detect_pll_err = 1;	
	}
	prev_flag_find_pll = v->output.flag_find_pll;

	if (v->input.enable_find_pll==0)
	    enable_detect_pll_err = 0;

	if ((enable_detect_pll_err) && (v->output.flag_find_pll==0) && (v->input.enable_find_pll==1))
	{
		err_pll = 1;
	}
*/
	return err_pll;
}




#pragma CODE_SECTION(pll_calc,".fast_run");
void pll_calc(PLL_REC *v)
{
	if (v->output.status >= STATUS_PLL_INITED)
	{
	 Find_zero_Uabc(v);
     PLLController(v);
//	 detect_phase_count(v);
	 v->output.status = STATUS_PLL_OK;
	}

}