#ifndef __ALG_PLL_H__
#define __ALG_PLL_H__

#include "IQmathLib.h"
#include "math_pi.h"
#include "pid_reg3.h"
#include "abc_to_alphabeta.h"
#include "alphabeta_to_dq.h"
#include "smooth.h"
#include "smooth.h"


#define DEFAULT_FREQ_NET	50.00 // Hz

#define DEFAULT_FREQ_RUN_PLL	4000 // Hz

#define DEFAULT_PID_KP_PLL 0.0375
#define DEFAULT_PID_KI_PLL 0.0128

#define STATUS_PLL_OK			10

#define STATUS_PLL_ERROR		2
#define STATUS_PLL_INITED		1
#define STATUS_PLL_NOT_INITED	0

#define FIND_MAX_W_STRIH	1.5 //0.12	//75Hz	
#define FIND_MIN_W_STRIH	0.5 //0.045	//33Hz

#define MAX_PERIOD_WAIT_PLL_W_SHTRIH 	3


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

typedef struct { 
				 float pid_kp_pll;		// êîýô. ðåãóëßòîðà Kp äëß ïîèñêà ñåòè
				 float pid_ki_pll;		// êîýô. ðåãóëßòîðà Ki äëß ïîèñêà ñåòè
				 int freq_run_pll; 		// ÷àñòîòà çàïóñêà ðàñ÷åòà, Ãö.
				 int rotation_u_cba; 	// ÷åðåäîâàíèå ôàç: 0 - ïðàâèëüíîå A-B-C, 1 - íåïðàâèëüíîå A-C-B
				 } PLL_SETUP;

#define PLL_SETUP_DEFAULT	{DEFAULT_PID_KP_PLL, DEFAULT_PID_KI_PLL, DEFAULT_FREQ_RUN_PLL,0}

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

typedef struct { _iq Input_U_AB;  // âõîäíîå íàïðßæåíèå Uab
				 _iq Input_U_BC;  // âõîäíîå íàïðßæåíèå Ubc
				 _iq Input_U_CA;  // âõîäíîå íàïðßæåíèå Uca
				 } PLL_INPUT;

#define PLL_INPUT_DEFAULT	{0, 0, 0}

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

typedef struct { int flag_find_pll; 
				 int int_freq_net;
				 _iq iq_freq_net;
				 int status;
				 } PLL_OUTPUT;

#define PLL_OUTPUT_DEFAULT	{0, 0, 0, STATUS_PLL_NOT_INITED}

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

typedef struct {
				 int enable_detect_phase_count;	
				 int error_phase_count;

				 _iq pll_Ud;
				 _iq pll_Uq;
				 _iq Tetta;
				 _iq Tetta_v2;
				 
				 _iq wc;
				 _iq dwc;
				 _iq w_shtrih;

				 _iq Tetta_z;
				 _iq Tetta_p;
				 _iq dTetta;

				 _iq zeroPLL;
				 _iq pi_teta_u_out;
				 _iq pi_teta_u_p;
				 _iq pi_teta_u_i;
				 _iq add_teta;
				 _iq add_teta2;

				 _iq Ua;
				 _iq Ub;
				 _iq Uc;


				 _iq Uab;
				 _iq Ubc;
				 _iq Uca;

				 _iq Ualpha;
				 _iq Ubeta;

				 _iq iqZeroUAB;
				 _iq iqZeroUBC;
				 _iq iqZeroUCA;

				 _iq sum_zeroU_AB_BC_CA;
				 _iq sum_zeroU_A_B_C;
				
				 _iq delta_Tetta_c;
				 _iq22	sum_div_find_zero_uabc;
				 int count_sum_find_zero_uabc;

				 _iq find_max_w_strih;
				 _iq find_min_w_strih;

				 int count_wait_pll_w_shtrih;
				 int max_time_wait_pll_w_strih;//MAX_TIME_WAIT_PLL_W_SHTRIH

				 int enable_find_pll;


				 }PLL_VARS;//39

#define PLL_VARS_DEFAULT    {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}


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

typedef struct {  PLL_INPUT input;
				  PLL_OUTPUT output;
				  PLL_SETUP setup;
				  PLL_VARS vars;
				  void (*init)(); // Pointer to calculation function
				  void (*calc_pll)(); // Pointer to calculation function
				  void (*get_freq_float)(); // Pointer to calculation function
				  void (*get_freq_iq)();
				 }PLL_REC;

typedef PLL_REC *PLL_REC_handle;

#define PLL_REC_DEFAULT      {\
								PLL_INPUT_DEFAULT,\
								PLL_OUTPUT_DEFAULT,\
								PLL_SETUP_DEFAULT,\
								PLL_VARS_DEFAULT,\
								(void (*)(unsigned long))pll_init,\
								(void (*)(unsigned long))pll_calc,\
								(void (*)(unsigned long))pll_get_freq_float,\
								(void (*)(unsigned long))pll_get_freq_iq \
                                }

void pll_init(PLL_REC_handle);
void pll_calc(PLL_REC_handle);
void pll_get_freq_float(PLL_REC_handle);
void pll_get_freq_iq(PLL_REC_handle);

void Find_zero_Uabc(PLL_REC_handle);
void PLLController(PLL_REC *v);
void AB_BC_CA_To_ABC(_iq U_AB, _iq U_BC, _iq U_CA, _iq *Ua, _iq *Ub, _iq *Uc);
void detect_phase_count(PLL_REC *v);
int read_error_find_pll(PLL_REC *v);



_iq minus_plus_2_pi(_iq a);
_iq minus_plus_2_pi_v2(_iq a);







#endif