// #include "project.h"
#include "adc_tools.h"
// #include "xp_project.h"
#include "IQmathLib.h"
#include "math.h"
#include "my_filter.h"
#include "params.h"
// #include "params_protect.h"
#include "vector.h"
// #include "xp_adc.h"
// #include "TuneUpPlane.h" //Ìîðãàíèå ñâåòîäèîäîì
// #include "log_to_memory.h"
// #include "errors.h"

// #define COUNT_ARR_ADC_BUF 2
#define Shift_Filter 1  //2

#define BTR_ENABLED

#define R_ADC_DEFAULT     { 1180 ,1180 , 256,  256, 256, 256, 256, 1180, 1180, 256, 256, 256, 256, 256, 256, 256, 256, 256 }
#define  K_LEM_ADC_DEFAULT { 60000,60000,5000, 5000,5000,5000,5000,60000,60000,5000,5000,5000,5000,5000,5000,5000,5000,5000 }
//#define LOG_ACP_TO_BUF

int ADC_f[COUNT_ARR_ADC_BUF][16];

int ADC_sf[COUNT_ARR_ADC_BUF][16];


#define ZERO_ADC_DEFAULT {2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048}
_iq19 iq19_zero_ADC[18] = ZERO_ADC_DEFAULT;

int zero_ADC[18] = ZERO_ADC_DEFAULT;
_iq iq_k_norm_ADC[18];
_iq19 iq19_k_norm_ADC[18];

_iq iq_norm_ADC[18];

unsigned int const R_ADC[18] =  R_ADC_DEFAULT;
unsigned int const K_LEM_ADC[18] = K_LEM_ADC_DEFAULT;

//#pragma DATA_SECTION(analog,".fast_vars");
ANALOG_VALUE	analog;
//#pragma DATA_SECTION(filter,".fast_vars");
ANALOG_VALUE	filter;

ANALOG_RAW_DATA rawData = ANALOG_RAW_DATA_DEFAULT;

_iq koef_Ud_long_filter=0;
_iq koef_Ud_fast_filter=0;
_iq koef_Im_filter=0;

_iq koef_Iabc_filter=0;

_iq koef_Wlong=0;


void calc_norm_ADC(void);
void analog_values_calc(void);
_iq im_calc( _iq ia, _iq ib, _iq ic);

void read_adc(ANALOG_RAW_DATA *data)
{
	ADC_f[0][0] = data->U1_1;
	ADC_f[0][1] = data->U1_2;
	ADC_f[0][2] = data->Izpt1_1;
	ADC_f[0][3] = data->Izpt1_2;
	ADC_f[0][4] = data->Ia1;
	ADC_f[0][5] = data->Ib1;
	ADC_f[0][6] = data->Ic1;
	ADC_f[0][7] = 0;
	ADC_f[0][8] = data->U2_1;
	ADC_f[0][9] = data->U2_2;
	ADC_f[0][10] = data->Izpt2_1;
	ADC_f[0][11] = data->Izpt2_2;
	ADC_f[0][12] = data->Ia2;
	ADC_f[0][13] = data->Ib2;
	ADC_f[0][14] = data->Ic2;
	ADC_f[0][15] = 0;


	ADC_sf[0][0] += (ADC_f[0][0] - ADC_sf[0][0]) >> Shift_Filter;
	ADC_sf[0][1] += (ADC_f[0][1] - ADC_sf[0][1]) >> Shift_Filter;
	ADC_sf[0][2] += (ADC_f[0][2] - ADC_sf[0][2]) >> Shift_Filter;
	ADC_sf[0][3] += (ADC_f[0][3] - ADC_sf[0][3]) >> Shift_Filter;
	ADC_sf[0][4] += (ADC_f[0][4] - ADC_sf[0][4]) >> Shift_Filter;
	ADC_sf[0][5] += (ADC_f[0][5] - ADC_sf[0][5]) >> Shift_Filter;
	ADC_sf[0][6] += (ADC_f[0][6] - ADC_sf[0][6]) >> Shift_Filter;

	ADC_sf[0][7] += (ADC_f[0][7] - ADC_sf[0][7]) >> Shift_Filter;
	ADC_sf[0][8] += (ADC_f[0][8] - ADC_sf[0][8]) >> Shift_Filter;
	ADC_sf[0][9] += (ADC_f[0][9] - ADC_sf[0][9]) >> Shift_Filter;
	ADC_sf[0][10] += (ADC_f[0][10] - ADC_sf[0][10]) >> Shift_Filter;
	ADC_sf[0][11] += (ADC_f[0][11] - ADC_sf[0][11]) >> Shift_Filter;
	ADC_sf[0][12] += (ADC_f[0][12] - ADC_sf[0][12]) >> Shift_Filter;
	ADC_sf[0][13] += (ADC_f[0][13] - ADC_sf[0][13]) >> Shift_Filter;
	ADC_sf[0][14] += (ADC_f[0][14] - ADC_sf[0][14]) >> Shift_Filter;
	ADC_sf[0][15] += (ADC_f[0][15] - ADC_sf[0][15]) >> Shift_Filter;


	ADC_f[1][0] = 0;
	ADC_f[1][1] = 0;
	ADC_f[1][2] = 0;
	ADC_f[1][3] = 0;

	ADC_sf[1][0] += (ADC_f[1][0] - ADC_sf[1][0]) >> Shift_Filter;
	ADC_sf[1][1] += (ADC_f[1][1] - ADC_sf[1][1]) >> Shift_Filter;
	ADC_sf[1][2] += (ADC_f[1][2] - ADC_sf[1][2]) >> Shift_Filter;
	ADC_sf[1][3] += (ADC_f[1][3] - ADC_sf[1][3]) >> Shift_Filter;
}

void acp_Handler(void)
{
	// read_adc();
	calc_norm_ADC();
	analog_values_calc();
	// Read_Fast_Errors();
}

#define COUNT_DETECT_ZERO 500

// void detect_zero_analog()
// {  
// 	int i, k;
// 	double tmp_buff[2][16] = {0};
// //	_iq koef_zero_ADC_filter = 1;
// 	for(i = 0; i < 16; tmp_buff[0][i++] = 0);
// 	for(i = 0; i < 16; tmp_buff[1][i++] = 0);
// 	i = 0;
// 	for (i = 0; i<COUNT_DETECT_ZERO; i++)
// 	{ 
// 		read_adc();

// 		for(k = 2; k < 15; k++)
// 		{ 
// 			if((k == 7) || (k == 8))
// 			{
// 				continue;
// 			}
// 			tmp_buff[0][k] += ADC_f[0][k];
// 		}
// 		for(k = 0;k < 4; k++)
// 		{ 
// 			tmp_buff[1][k] += ADC_f[1][k];
// 		}
// 		pause_1000(1000);
// 	}
// 	k = 2;
// 	for(i = 2; i < 15; i++)
// 	{
// 		if(i == 7)
// 		{
// 			continue;
// 		}
		
// 		if((i == 8) || (i == 9))
// 		{
// 			k++;
// 			continue;
// 		}
// 		zero_ADC[k] = (int)(tmp_buff[0][i] / COUNT_DETECT_ZERO + 0.5);
// 		k++;
// 	} 
// 	for(i = 0; i < 4; i++, k++)
// 	{
// 		zero_ADC[k] = (int)(tmp_buff[1][i] / COUNT_DETECT_ZERO + 0.5);
// 	}
// 	zero_ADC[0] = zero_ADC[2];
// 	zero_ADC[1] = zero_ADC[2];
// 	zero_ADC[7] = zero_ADC[9];
// 	zero_ADC[8] = zero_ADC[9];
	

// }

void init_Adc_Variables(void)
{
	unsigned int k;
	int *panalog, *pfilter;

	for (k=0;k<16;k++)
	{ 
		   
		ADC_f[0][k]=0;
		ADC_sf[0][k]=0;
		ADC_f[1][k]=0;
		ADC_sf[1][k]=0;
	}

	// detect_zero_analog();
	
	for (k=0;k<18;k++)
	{
		iq19_k_norm_ADC[k] = _IQ19(K_LEM_ADC[k]*250.0/R_ADC[k]/NORMA_ACP/4096.0);

		iq19_zero_ADC[k]=_IQ19(zero_ADC[k]);   //_IQ19(2030);//_IQ19(1770);

	}

	koef_Ud_long_filter = _IQ(0.05);
	koef_Ud_fast_filter = _IQ(0.2); // _IQ(0.001/0.00931); //_IQ(0.001/0.00131);

	koef_Im_filter = _IQ(0.015);

	koef_Iabc_filter = _IQ(0.0005); //_IQ(0.002/0.006);
	koef_Wlong = _IQ(0.001);
	
	panalog = (int*)&analog;
	pfilter = (int*)&filter;
	for (k=0;k < sizeof(ANALOG_VALUE) / sizeof(int) ;k++)
	{ 
		   
		*(panalog + k) = 0;
		*(pfilter + k) = 0;
	}

}

inline _iq norma_adc(int plane, int chan, int n_norm)
{
	// return _IQ19toIQ(_IQ19mpy((iq19_zero_ADC[n_norm] - ((long)ADC_sf[plane][chan]<<19) ),iq19_k_norm_ADC[n_norm]));
	// return _IQ19toIQ(_IQ19mpy((((long)ADC_sf[plane][chan]<<19) - iq19_zero_ADC[n_norm]),iq19_k_norm_ADC[n_norm]));
	return _IQ19toIQ(_IQ19mpy((((long)ADC_sf[plane][chan]<<19)),iq19_k_norm_ADC[n_norm]));
}

void calc_norm_ADC(void)
{
    iq_norm_ADC[0] = norma_adc(0, 0, 0);
	iq_norm_ADC[1] = norma_adc(0, 1, 1);
	iq_norm_ADC[2] = norma_adc(0, 2, 2);
	iq_norm_ADC[3] = norma_adc(0, 3, 3);
	iq_norm_ADC[4] = norma_adc(0, 4, 4);
	iq_norm_ADC[5] = norma_adc(0, 5, 5);
	iq_norm_ADC[6] = norma_adc(0, 6, 6);
	iq_norm_ADC[7] = norma_adc(0, 8, 7);
	iq_norm_ADC[8] = norma_adc(0, 9, 8);
	iq_norm_ADC[9] = norma_adc(0, 10, 9);
	iq_norm_ADC[10] = norma_adc(0, 11, 10);
	iq_norm_ADC[11] = norma_adc(0, 12, 11);
	iq_norm_ADC[12] = norma_adc(0, 13, 12);
	iq_norm_ADC[13] = norma_adc(0, 14, 13);
	iq_norm_ADC[14] = norma_adc(1, 0, 14);
	iq_norm_ADC[15] = norma_adc(1, 1, 15);
	iq_norm_ADC[16] = norma_adc(1, 2, 16);
	iq_norm_ADC[17] = norma_adc(1, 3, 17);
}

void analog_values_calc(void)
{

   analog.iqU_1 = iq_norm_ADC[0];
   analog.iqU_2 = iq_norm_ADC[1];
   analog.iqU_3 = iq_norm_ADC[7];
   analog.iqU_4 = iq_norm_ADC[8];

   analog.iqIin_1 = iq_norm_ADC[2];
   analog.iqIin_2 = -iq_norm_ADC[3];
   analog.iqIin_3 = iq_norm_ADC[9];
   analog.iqIin_4 = -iq_norm_ADC[10];


// #if PROJECT_ARKTIKA == 1
   analog.iqIa1_1 = iq_norm_ADC[4];
   analog.iqIb1_1 = iq_norm_ADC[5];
   analog.iqIc1_1 = iq_norm_ADC[6];

   analog.iqIa2_1 = iq_norm_ADC[11];
   analog.iqIb2_1 = iq_norm_ADC[12];
   analog.iqIc2_1 = iq_norm_ADC[13];
// #endif
#if PROJECT_SIBERIA == 1
   analog.iqIa1_1 = iq_norm_ADC[6];
   analog.iqIb1_1 = iq_norm_ADC[5];
   analog.iqIc1_1 = iq_norm_ADC[4];

   analog.iqIa2_1 = iq_norm_ADC[13];
   analog.iqIb2_1 = iq_norm_ADC[12];
   analog.iqIc2_1 = iq_norm_ADC[11];
#endif

   analog.iqIbtr1_1 = labs(iq_norm_ADC[14]);
   analog.iqIbtr1_2 = labs(iq_norm_ADC[16]);
   analog.iqIbtr2_1 = labs(iq_norm_ADC[15]);
   analog.iqIbtr2_2 = labs(iq_norm_ADC[17]);

	analog.iqIa1_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIa1_rms, _IQabs(analog.iqIa1_1));
	analog.iqIb1_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIb1_rms, _IQabs(analog.iqIb1_1));
	analog.iqIc1_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIc1_rms, _IQabs(analog.iqIc1_1));

	analog.iqIa2_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIa2_rms, _IQabs(analog.iqIa2_1));
	analog.iqIb2_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIb2_rms, _IQabs(analog.iqIb2_1));
	analog.iqIc2_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIc2_rms, _IQabs(analog.iqIc2_1));


   filter.iqU_1_long = exp_regul_iq(koef_Ud_long_filter, filter.iqU_1_long, analog.iqU_1);
   filter.iqU_2_long = exp_regul_iq(koef_Ud_long_filter, filter.iqU_2_long, analog.iqU_2);
   filter.iqU_3_long = exp_regul_iq(koef_Ud_long_filter, filter.iqU_3_long, analog.iqU_3);
   filter.iqU_4_long = exp_regul_iq(koef_Ud_long_filter, filter.iqU_4_long, analog.iqU_4);

   filter.iqIin_1 = exp_regul_iq(koef_Im_filter, filter.iqIin_1, analog.iqIin_1);
   filter.iqIin_2 = exp_regul_iq(koef_Im_filter, filter.iqIin_2, analog.iqIin_2);
   filter.iqIin_3 = exp_regul_iq(koef_Im_filter, filter.iqIin_3, analog.iqIin_3);
   filter.iqIin_4 = exp_regul_iq(koef_Im_filter, filter.iqIin_4, analog.iqIin_4);


   

   filter.iqU_1_fast = exp_regul_iq(koef_Ud_fast_filter, filter.iqU_1_fast, analog.iqU_1);
   filter.iqU_2_fast = exp_regul_iq(koef_Ud_fast_filter, filter.iqU_2_fast, analog.iqU_2);

   filter.iqU_3_fast = exp_regul_iq(koef_Ud_fast_filter, filter.iqU_3_fast, analog.iqU_3);
   filter.iqU_4_fast = exp_regul_iq(koef_Ud_fast_filter, filter.iqU_4_fast, analog.iqU_4);

   filter.iqIa1_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIa1_1, analog.iqIa1_1);
   filter.iqIb1_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIb1_1, analog.iqIb1_1);
   filter.iqIc1_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIc1_1, analog.iqIc1_1);

   filter.iqIa2_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIa2_1, analog.iqIa2_1);
   filter.iqIb2_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIb2_1, analog.iqIb2_1);
   filter.iqIc2_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIc2_1, analog.iqIc2_1);


   	
	analog.iqW1 = _IQmpy(filter.iqU_1_fast, analog.iqIin_1) + _IQmpy(filter.iqU_2_fast, analog.iqIin_2);
	analog.iqW2 = _IQmpy(filter.iqU_3_fast, analog.iqIin_3) + _IQmpy(filter.iqU_4_fast, analog.iqIin_4);
	analog.iqW = analog.iqW1 + analog.iqW2;



	analog.iqIm_1 = im_calc(filter.iqIa1_1, filter.iqIb1_1, filter.iqIc1_1);
	analog.iqIm_2 = im_calc(filter.iqIa2_1, filter.iqIb2_1, filter.iqIc2_1);


//   i_led1_on_off(0);
//   i_led1_on_off(1);

   filter.iqIm_1 = exp_regul_iq(koef_Im_filter, filter.iqIm_1, analog.iqIm_1);
   filter.iqIm_2 = exp_regul_iq(koef_Im_filter, filter.iqIm_2, analog.iqIm_2);
//	filter.iqIm = exp_regul_iq(koef_Iabc_filter, filter.iqIm, filter.iqIm_2);

   filter.iqW1 = exp_regul_iq(koef_Iabc_filter, filter.iqW1, analog.iqW1);
   filter.iqW2 = exp_regul_iq(koef_Iabc_filter, filter.iqW2, analog.iqW2);
   filter.iqW = exp_regul_iq(koef_Wlong, filter.iqW, filter.iqW1 + filter.iqW2);

//   filter.iqIin_1 = exp_regul_iq(koef_Im_filter, filter.iqIin_1, analog.iqIin_1);
//   filter.iqIin_2 = exp_regul_iq(koef_Im_filter, filter.iqIin_2, analog.iqIin_2);

}

#define CONST_IQ_11PI6 96629827  //11Pi/6
#define CONST_IQ_PI6  8784529 // Pi/6


// _iq err_level_adc_on_go = ERR_LEVEL_ADC_PLUS_6_ON_GO_IQ;

// _iq err_level_adc = ERR_LEVEL_ADC_PLUS_6_IQ;


// unsigned int detect_protect_ACP_plus()
// {
//   unsigned int mask = 0, result = 0;
//   static unsigned int prev_mask = 0xFCFC;

// 	if(f.Go)
// 	{
// 		mask |= filter.iqU_1_long > err_level_adc_on_go ? 1 : 0;
// 		mask |= filter.iqU_2_long > err_level_adc_on_go ? 1 << 1: 0;
// 	}
// 	else
// 	{
// 		mask |= filter.iqU_1_long > err_level_adc ? 1 : 0;
// 		mask |= filter.iqU_2_long > err_level_adc ? 1 << 1: 0;
// 	}
	
	
// #ifdef NEW_I_ZPT_REZISTORS
// 	mask |= ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 2: 0;
// 	mask |= ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 3: 0;
// #else
// 	mask |= ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS? 1 << 2: 0;
// 	mask |= ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS? 1 << 3: 0;
// #endif
	
// 	mask |= ADC_sf[0][4] < ERR_LEVEL_I_FAZA_PLUS? 1 << 4: 0;
// 	mask |= ADC_sf[0][5] < ERR_LEVEL_I_FAZA_PLUS? 1 << 5: 0;
// 	mask |= ADC_sf[0][6] < ERR_LEVEL_I_FAZA_PLUS? 1 << 6: 0;

// 	if(f.Go)
// 	{
// 		mask |= filter.iqU_3_long > err_level_adc_on_go ? 1 << 8: 0;
// 		mask |= filter.iqU_4_long > err_level_adc_on_go ? 1 << 9: 0;
// 	}
// 	else
// 	{
// 		mask |= (filter.iqU_3_long > err_level_adc) ? 1 << 8: 0;
// 		mask |= (filter.iqU_4_long > err_level_adc) ? 1 << 9: 0;
// 	}
	
	
// #ifdef NEW_I_ZPT_REZISTORS
// 	mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 10: 0;
// 	mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 11: 0;
// #else
// 	mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS? 1 << 10: 0;
// 	mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS? 1 << 11: 0;
// #endif
	
// 	mask |= ADC_sf[0][12] < ERR_LEVEL_I_FAZA_PLUS? 1 << 12: 0;
// 	mask |= ADC_sf[0][13] < ERR_LEVEL_I_FAZA_PLUS? 1 << 13: 0;
// 	mask |= ADC_sf[0][14] < ERR_LEVEL_I_FAZA_PLUS? 1 << 14: 0;

// //Calculate values for BTR
// #ifdef BTR_ENABLED
// 	mask |= ADC_sf[1][0] < ERR_LEVEL_BREAK_REZ? 1 << 7: 0; //BTR1pos
// 	mask |= ADC_sf[1][2] < ERR_LEVEL_BREAK_REZ? 1 << 15: 0; //BTR1neg
// #endif //BTR_ENABLED
// /*	*/

// 	result = mask & prev_mask;
// 	prev_mask |= mask; //
// 						//
// 	if(mask == 0) { prev_mask = 0xFFFF;}
// 	return result;

// }


// unsigned int detect_protect_ACP_minus()
// {
// 	unsigned int mask = 0, result = 0;
// 	static unsigned int prev_mask = 0xFCFC; //

// /*	   */
// 	if(f.Ready2)
// 	{
// //		mask |= (ADC_sf[0] > ERR_LEVEL_ADC_MINUS_6) ? 1: 0;
// //		mask |= (ADC_sf[1] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 1: 0;
// 	}
// #ifdef NEW_I_ZPT_REZISTORS
// 	mask |= (ADC_sf[0][2] > ERR_LEVEL_I_ZPT_MINUS_REZ_249)? 1 << 2: 0;
// 	mask |= (ADC_sf[0][3] > ERR_LEVEL_I_ZPT_MINUS_REZ_249)? 1 << 3: 0;
// #else
// 	mask |= (ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS)? 1 << 2: 0;
// 	mask |= (ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS)? 1 << 3: 0;
// #endif
// 	mask |= ADC_sf[0][4] > ERR_LEVEL_I_FAZA_MINUS? 1 << 4: 0;
// 	mask |= ADC_sf[0][5] > ERR_LEVEL_I_FAZA_MINUS? 1 << 5: 0;
// 	mask |= ADC_sf[0][6] > ERR_LEVEL_I_FAZA_MINUS? 1 << 6: 0;
// 	/*	   */
// /*	if(f.Ready2)
// 	{
// 		mask |= (ADC_sf[0][7] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 8: 0;
// 		mask |= (ADC_sf[0][8] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 9: 0;
// 	} */
// #ifdef NEW_I_ZPT_REZISTORS
// 	mask |= ADC_sf[0][10] > ERR_LEVEL_I_ZPT_MINUS_REZ_249? 1 << 10: 0;
// 	mask |= ADC_sf[0][11] > ERR_LEVEL_I_ZPT_MINUS_REZ_249? 1 << 11: 0;
// #else
// 	mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS? 1 << 10: 0;
// 	mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS? 1 << 11: 0;
// #endif
		
// 	mask |= ADC_sf[0][12] > ERR_LEVEL_I_FAZA_MINUS? 1 << 12: 0;
// 	mask |= ADC_sf[0][13] > ERR_LEVEL_I_FAZA_MINUS? 1 << 13: 0;
// 	mask |= ADC_sf[0][14] > ERR_LEVEL_I_FAZA_MINUS? 1 << 14: 0;

// //Calculate values for BTR
// #ifdef BTR_ENABLED
// 	mask |= ADC_sf[1][1] < ERR_LEVEL_BREAK_REZ? 1 << 7: 0;	//BTR2pos
// 	mask |= ADC_sf[1][3] < ERR_LEVEL_BREAK_REZ? 1 << 15: 0;	//BTR2neg
// #endif //BTR_ENABLED
// /*	*/

	
// 	result = mask & prev_mask;
// 	prev_mask |= mask; //
// 						//
// 	if(mask == 0) { prev_mask = 0xFFFF;}
// 	return result;

// }

// #pragma CODE_SECTION(fillADClogs,".fast_run");
// void fillADClogs(void)
// {
// 	logpar.log1 = (int16)_IQtoIQ15(analog.iqU_1);
// 	logpar.log2 = (int16)_IQtoIQ15(analog.iqU_2);
// 	logpar.log3 = (int16)_IQtoIQ15(analog.iqIin_1);
// 	logpar.log4 = (int16)_IQtoIQ15(analog.iqIin_2);
// 	logpar.log5 = (int16)_IQtoIQ15(analog.iqIa1_1);
// 	logpar.log6 = (int16)_IQtoIQ15(analog.iqIb1_1);
// 	logpar.log7 = (int16)_IQtoIQ15(analog.iqIc1_1);
// 	logpar.log8 = (int16)_IQtoIQ15(analog.iqU_3);
// 	logpar.log9 = (int16)_IQtoIQ15(analog.iqU_4);
// 	logpar.log10 = (int16)_IQtoIQ15(analog.iqIin_3);
// 	logpar.log11 = (int16)_IQtoIQ15(analog.iqIin_4);
// 	logpar.log12 = (int16)_IQtoIQ15(analog.iqIa2_1);
// 	logpar.log13 = (int16)_IQtoIQ15(analog.iqIb2_1);
// 	logpar.log14 = (int16)_IQtoIQ15(analog.iqIc2_1);
// //	logpar.log15 = (int16)_IQtoIQ15(filter.iqU_1_fast);
// //	logpar.log16 = (int16)_IQtoIQ15(filter.iqU_1_long);
// }

/********************************************************************/    
/* Расчет модулy тока из показаний трех фаз */                                
/********************************************************************/
_iq SQRT_32 = _IQ(0.8660254037844);
_iq CONST_23 = _IQ(2.0/3.0);
_iq CONST_15 = _IQ(1.5);

_iq im_calc( _iq ia, _iq ib, _iq ic)
{
  _iq isa,isb, t;
  

  isa  = _IQmpy(CONST_15,ia);
  isb  =  _IQmpy(SQRT_32,ib-ic );
  
//  ( _IQ19mpy(SQRT_32, _IQ15toIQ19(ib)) -  _IQ15mpy(SQRT_32, _IQ15toIQ19(ic)) );

  // t = _IQmag(isa,isb);
  t = _IQsqrt(_IQmpy(isa, isa) + _IQmpy(isb, isb));
  t = _IQmpy(CONST_23,t);

  return  (t);

}