/*
 * Diff_1.c
 *
 *  Created on: 01-03-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

#include "tdefs.h"
#include "misc.h"
#include "helper.h"



#include "Diff.h"
#include "Diff_1_bis.h"

#define c1r_nowe 2 //ilosc powtorzen zadzialania bezwarunkowego
#define c1p_nowe 5 //ilosc powtorzen odpadu bezwarunkowego

void ustaw_kompensacje_bis(struct diff_1_bis_logic *log, enum typ_kompensacji typ, u16 nr_pradu)
{
	struct we_an_diff *a0,*a1,*a2;

	switch(nr_pradu)
	{
		case 0:
			if((u32)log->I1_l1==(u32)&log_manager.nets_data[0])
				a0=&log->nast_.wejscie_zero;
			else
				a0 = &log->an_struct_i1_l1;


			if((u32)log->I1_l2==(u32)&log_manager.nets_data[0])
				a1=&log->nast_.wejscie_zero;
			else
				a1 = &log->an_struct_i1_l2;

			if((u32)log->I1_l3==(u32)&log_manager.nets_data[0])
				a2=&log->nast_.wejscie_zero;
			else
				a2 = &log->an_struct_i1_l3;
			break;
		case 1:
			if((u32)log->I2_l1==(u32)&log_manager.nets_data[0])
				a0=&log->nast_.wejscie_zero;
			else
				a0 = &log->an_struct_i2_l1;


			if((u32)log->I2_l2==(u32)&log_manager.nets_data[0])
				a1=&log->nast_.wejscie_zero;
			else
				a1 = &log->an_struct_i2_l2;

			if((u32)log->I2_l3==(u32)&log_manager.nets_data[0])
				a2=&log->nast_.wejscie_zero;
			else
				a2 = &log->an_struct_i2_l3;
			break;
		case 2:
			if((u32)log->I3_l1==(u32)&log_manager.nets_data[0])
				a0=&log->nast_.wejscie_zero;
			else
				a0 = &log->an_struct_i3_l1;


			if((u32)log->I3_l2==(u32)&log_manager.nets_data[0])
				a1=&log->nast_.wejscie_zero;
			else
				a1 = &log->an_struct_i3_l2;

			if((u32)log->I3_l3==(u32)&log_manager.nets_data[0])
				a2=&log->nast_.wejscie_zero;
			else
				a2 = &log->an_struct_i3_l3;
			break;
		case 3:
			
			a0=&log->nast_.wejscie_zero;
			a1=&log->nast_.wejscie_zero;
			a2=&log->nast_.wejscie_zero;

			break;
		default:
			if((u32)log->I1_l1==(u32)&log_manager.nets_data[0])
				a0=&log->nast_.wejscie_zero;
			else
				a0 = &log->an_struct_i1_l1;


			if((u32)log->I1_l2==(u32)&log_manager.nets_data[0])
				a1=&log->nast_.wejscie_zero;
			else
				a1 = &log->an_struct_i1_l2;

			if((u32)log->I1_l3==(u32)&log_manager.nets_data[0])
				a2=&log->nast_.wejscie_zero;
			else
				a2 = &log->an_struct_i1_l3;
			break;
	}

	switch (typ)
	{
	case zero:
		log->nast_.L[0].Ip[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[1].Ip[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[2].Ip[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.a[nr_pradu] = 0;
		break;
	case A:
		log->nast_.L[0].Ip[nr_pradu] = a0;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[1].Ip[nr_pradu] = a1;
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[2].Ip[nr_pradu] = a2;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.a[nr_pradu] = 1;
		break;
	case B:
		log->nast_.L[0].Ip[nr_pradu] = a0;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[1].Ip[nr_pradu] = a1;//
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[2].Ip[nr_pradu] = a2;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.a[nr_pradu] = -1;
		break;
	case C:
		log->nast_.L[0].Ip[nr_pradu] = a0;
		log->nast_.L[0].Im[nr_pradu] = a1;
		log->nast_.L[1].Ip[nr_pradu] = a1;
		log->nast_.L[1].Im[nr_pradu] = a2;
		log->nast_.L[2].Ip[nr_pradu] = a2;
		log->nast_.L[2].Im[nr_pradu] = a0;
		log->nast_.a[nr_pradu] = 0.57735f;
		break;
	case D:
		log->nast_.L[0].Ip[nr_pradu] = a0;
		log->nast_.L[0].Im[nr_pradu] = a1;
		log->nast_.L[1].Ip[nr_pradu] = a1;
		log->nast_.L[1].Im[nr_pradu] = a2;
		log->nast_.L[2].Ip[nr_pradu] = a2;
		log->nast_.L[2].Im[nr_pradu] = a0;
		log->nast_.a[nr_pradu] = -0.57735f;
		break;
	case E:
		log->nast_.L[0].Ip[nr_pradu] = a0;
		log->nast_.L[0].Im[nr_pradu] = a2;
		log->nast_.L[1].Ip[nr_pradu] = a1;
		log->nast_.L[1].Im[nr_pradu] = a0;
		log->nast_.L[2].Ip[nr_pradu] = a2;
		log->nast_.L[2].Im[nr_pradu] = a1;
		log->nast_.a[nr_pradu] = 0.57735f;
		break;
	case F:
		log->nast_.L[0].Ip[nr_pradu] = a0;
		log->nast_.L[0].Im[nr_pradu] = a2;
		log->nast_.L[1].Ip[nr_pradu] = a1;
		log->nast_.L[1].Im[nr_pradu] = a0;
		log->nast_.L[2].Ip[nr_pradu] = a2;
		log->nast_.L[2].Im[nr_pradu] = a1;
		log->nast_.a[nr_pradu] = -0.57735f;
		break;
	case N:
		log->nast_.L[0].Ip[nr_pradu] = a1;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[1].Ip[nr_pradu] = a2;
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[2].Ip[nr_pradu] = a0;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.a[nr_pradu] = 1;
		break;
	case O:
		log->nast_.L[0].Ip[nr_pradu] = a1;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[1].Ip[nr_pradu] = a2;
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[2].Ip[nr_pradu] = a0;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.a[nr_pradu] = -1;
		break;

	case U:
		log->nast_.L[0].Ip[nr_pradu] = a2;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[1].Ip[nr_pradu] = a0;
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[2].Ip[nr_pradu] = a1;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.a[nr_pradu] = -1;
		break;

	case V:
		log->nast_.L[0].Ip[nr_pradu] = a2;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[1].Ip[nr_pradu] = a0;
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.L[2].Ip[nr_pradu] = a1;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero;
		log->nast_.a[nr_pradu] = 1;
		break;


	case W: //Pawel
			log->nast_.L[0].Ip[nr_pradu] = a2;
			log->nast_.L[0].Im[nr_pradu] = a1;
			log->nast_.L[1].Ip[nr_pradu] = a0;
			log->nast_.L[1].Im[nr_pradu] = a2;
			log->nast_.L[2].Ip[nr_pradu] = a1;
			log->nast_.L[2].Im[nr_pradu] = a0;
			log->nast_.a[nr_pradu] = 0.57735f;
			break;
	case X: //Pawel
			log->nast_.L[0].Ip[nr_pradu] = a1;
			log->nast_.L[0].Im[nr_pradu] = a2;
			log->nast_.L[1].Ip[nr_pradu] = a2;
			log->nast_.L[1].Im[nr_pradu] = a0;
			log->nast_.L[2].Ip[nr_pradu] = a0;
			log->nast_.L[2].Im[nr_pradu] = a1;
			log->nast_.a[nr_pradu] = 0.57735f;
			break;
	case Y: //Pawel
			log->nast_.L[0].Ip[nr_pradu] = a1;
			log->nast_.L[0].Im[nr_pradu] = a0;
			log->nast_.L[1].Ip[nr_pradu] = a2;
			log->nast_.L[1].Im[nr_pradu] = a1;
			log->nast_.L[2].Ip[nr_pradu] = a0;
			log->nast_.L[2].Im[nr_pradu] = a2;
			log->nast_.a[nr_pradu] = 0.57735f;
			break;
	case Z: //Pawel
			log->nast_.L[0].Ip[nr_pradu] = a2;
			log->nast_.L[0].Im[nr_pradu] = a0;
			log->nast_.L[1].Ip[nr_pradu] = a0;
			log->nast_.L[1].Im[nr_pradu] = a1;
			log->nast_.L[2].Ip[nr_pradu] = a1;
			log->nast_.L[2].Im[nr_pradu] = a2;
			log->nast_.a[nr_pradu] = 0.57735f;
			break;
	}
}


void przelicz_nastawy_bis(struct diff_1_bis_logic *log,struct nast_trafo n)
{
	int k;

	memset(&log->dw, 0x00, sizeof(log->dw)); // zerowanie przekaznika
	memset(&log->l, 0x00, sizeof(log->l));
	memset(&log->P_, 0x00, sizeof(log->P_));

	//status_rej = oczekiwanie;

#ifdef IZAZ600
	//sprawdzenie adresu danej analogowej dla rejestratora 3kHz
	if ((nast_.adr.Ir_L1_ortb < ilosc_kanalow_rej3kHz)
		& (nast_.adr.Ir_L2_ortb < ilosc_kanalow_rej3kHz)
		& (nast_.adr.Ir_L3_ortb < ilosc_kanalow_rej3kHz)
		& (nast_.adr.Ih_L1_ortb < ilosc_kanalow_rej3kHz)
		& (nast_.adr.Ih_L2_ortb < ilosc_kanalow_rej3kHz)
		& (nast_.adr.Ih_L3_ortb < ilosc_kanalow_rej3kHz)
		)
		nast_.Rej3kHz_adr_wej_OK = 1; else nast_.Rej3kHz_adr_wej_OK = 0;
#endif


	//wyznaczenie adresow wejsc do rownan kompensacji
	//OFF,y0,y1,y5,y6,y7,y11,d0,d1,d5,d7,d6,d11
	memset(&log->nast_.wejscie_zero, 0x00, sizeof(log->nast_.wejscie_zero));
	for (k = 0; k < SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2; k++)
		log->nast_.buf_zero[k] = 32767;

	log->nast_.wejscie_zero.samples_buf = log->nast_.buf_zero;
	log->nast_.wejscie_zero.params = &log->nast_.n_zero;
	//log->nast_.wejscie_zero.nr_probki = &bufnr_probki;
	log->nast_.n_zero.multiplier = 0;
	log->nast_.n_zero.znam_pierw = 1;
	log->nast_.n_zero.offset = 0;
	memset(&log->nast_.a, 0x00, 4 * sizeof(float));


	if (n.GrP1) // jesli gorne uzwojenie trafo D
	{
		if (!n.GrP3) //jesli dwuuzwojeniowy trafo
		{
			log->jaki_uklad = tylko_T2w_D;
		}
		else
		{ //jesli trojuzwojeniowy trafo
			log->jaki_uklad = tylko_T3w_D;
		}
	}
	else
	{ //jesli gorne uzwojenie trafo Y
		if (!n.GrP3) //jesli dwuuzwojeniowy trafo
		{
			log->jaki_uklad = tylko_T2w_Y;
		}
		else
		{ //jesli trojuzwojeniowy trafo
			log->jaki_uklad = tylko_T3w_Y;
		}
	}

	//-----------------------------------------

	if(n.GrP2>12)
		log->nast_.p2d=1;
	else
		log->nast_.p2d=0;

	if(n.GrP3>12)
		log->nast_.p3d=1;
	else
		log->nast_.p3d=0;

	//ustawienie skladnikow kompensacji pradow
	switch (log->jaki_uklad)
	{
	case tylko_T2w_Y:
		ustaw_kompensacje_bis(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Ig, 0);
		ustaw_kompensacje_bis(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Id, 1);
		ustaw_kompensacje_bis(log,zero, 2);
		ustaw_kompensacje_bis(log,zero, 3);

		if(!pY_ZPrAE[n.GrP2])
			set_struct(&log->nieobs);

		log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw
			/ log->nast_.L[0].Ip[0]->params->znam_pierw
			/ n.PT12
			;
		break;
	case tylko_T2w_D:
		ustaw_kompensacje_bis(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0);
		ustaw_kompensacje_bis(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1);
		ustaw_kompensacje_bis(log,zero, 2);
		ustaw_kompensacje_bis(log,zero, 3);

		if(!pD_ZPrAE[n.GrP2])
			set_struct(&log->nieobs);

		log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw
			/ log->nast_.L[0].Ip[0]->params->znam_pierw
			/ n.PT12
			;

		break;
	case tylko_T3w_Y:
		ustaw_kompensacje_bis(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Ig, 0);
		ustaw_kompensacje_bis(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Id, 1);
		ustaw_kompensacje_bis(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Io, 2);
		ustaw_kompensacje_bis(log,zero, 3);

		if(!pY_ZPrAE[n.GrP2]||!pY_ZPrAE[n.GrP3])
			set_struct(&log->nieobs);

		log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw
			/ log->nast_.L[0].Ip[0]->params->znam_pierw
			/ n.PT12
			;
		log->nast_.a[2] *= log->nast_.L[0].Ip[2]->params->znam_pierw
			/ log->nast_.L[0].Ip[0]->params->znam_pierw
			/ n.PT13
			;
		break;
	case tylko_T3w_D:
		ustaw_kompensacje_bis(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0);
		ustaw_kompensacje_bis(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1);
		ustaw_kompensacje_bis(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP3]].Io, 2);
		ustaw_kompensacje_bis(log,zero, 3);

		if(!pD_ZPrAE[n.GrP2]||!pD_ZPrAE[n.GrP3])
			set_struct(&log->nieobs);

		log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw
			/ log->nast_.L[0].Ip[0]->params->znam_pierw
			/ n.PT12
			;
		log->nast_.a[2] *= log->nast_.L[0].Ip[2]->params->znam_pierw
			/ log->nast_.L[0].Ip[0]->params->znam_pierw
			/ n.PT13
			;
		break;
	}
	//-----------------------------------------

	//przeliczenie wspolczynnikow kompensacji o korekcje i ofset
	uint16_t i;

	for (i = 0; i < 3; i++)
	{

		log->nast_.L[i].a_p[0][0] = log->nast_.a[0] * log->nast_.L[i].Ip[0]->params->nast.mnoznik;
		log->nast_.L[i].a_p[0][1] = log->nast_.L[i].Ip[0]->params->nast.offset + 32767;
		log->nast_.L[i].a_m[0][0] = log->nast_.a[0] * log->nast_.L[i].Im[0]->params->nast.mnoznik;
		log->nast_.L[i].a_m[0][1] = log->nast_.L[i].Im[0]->params->nast.offset + 32767;
		log->nast_.L[i].a_p[1][0] = log->nast_.a[1] * log->nast_.L[i].Ip[1]->params->nast.mnoznik;
		log->nast_.L[i].a_p[1][1] = log->nast_.L[i].Ip[1]->params->nast.offset + 32767;
		log->nast_.L[i].a_m[1][0] = log->nast_.a[1] * log->nast_.L[i].Im[1]->params->nast.mnoznik;
		log->nast_.L[i].a_m[1][1] = log->nast_.L[i].Im[1]->params->nast.offset + 32767;
		log->nast_.L[i].a_p[2][0] = log->nast_.a[2] * log->nast_.L[i].Ip[2]->params->nast.mnoznik;
		log->nast_.L[i].a_p[2][1] = log->nast_.L[i].Ip[2]->params->nast.offset + 32767;
		log->nast_.L[i].a_m[2][0] = log->nast_.a[2] * log->nast_.L[i].Im[2]->params->nast.mnoznik;
		log->nast_.L[i].a_m[2][1] = log->nast_.L[i].Im[2]->params->nast.offset + 32767;
		log->nast_.L[i].a_p[3][0] = log->nast_.a[3] * log->nast_.L[i].Ip[3]->params->nast.mnoznik;
		log->nast_.L[i].a_p[3][1] = log->nast_.L[i].Ip[3]->params->nast.offset + 32767;
		log->nast_.L[i].a_m[3][0] = log->nast_.a[3] * log->nast_.L[i].Im[3]->params->nast.mnoznik;
		log->nast_.L[i].a_m[3][1] = log->nast_.L[i].Im[3]->params->nast.offset + 32767;

	}

	// wyznaczenie wspolczynnikow filtru


	for (i = 0; i < SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS; i++)
	{
		log->nast_.wsp_filtr_sin_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2);
		log->nast_.wsp_filtr_cos_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2);
		log->nast_.wsp_filtr_sin2h_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2);
		log->nast_.wsp_filtr_cos2h_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2);
		log->nast_.wsp_filtr_sin5h_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/5)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2);
		log->nast_.wsp_filtr_cos5h_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/5)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2);
	}

	//----------------------------------------------------------------------------------

	memset(&log->w, 0x00, sizeof(log->w));
}

int diff_1_bis_initlog(void *arguments, void *logic)
{
	struct diff_1_bis_args *args = (struct diff_1_bis_args *)arguments;
	struct diff_1_bis_logic *log = (struct diff_1_bis_logic *)logic;

	if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl))
		return -1;
	if(set_pointer_in_ptr(args->io.ia_l1_ptr_in,(u32 *)&log->I1_l1))
		return -1;
	if(set_pointer_in_ptr(args->io.ia_l1_params_ptr_in,(u32 *)&log->I1_l1_params))
		return -1;
	if(set_pointer_in_ptr(args->io.ia_l2_ptr_in,(u32 *)&log->I1_l2))
		return -1;
	if(set_pointer_in_ptr(args->io.ia_l2_params_ptr_in,(u32 *)&log->I1_l2_params))
		return -1;
	if(set_pointer_in_ptr(args->io.ia_l3_ptr_in,(u32 *)&log->I1_l3))
		return -1;
	if(set_pointer_in_ptr(args->io.ia_l3_params_ptr_in,(u32 *)&log->I1_l3_params))
		return -1;

	if(set_pointer_in_ptr(args->io.ib_l1_ptr_in,(u32 *)&log->I2_l1))
		return -1;
	if(set_pointer_in_ptr(args->io.ib_l1_params_ptr_in,(u32 *)&log->I2_l1_params))
		return -1;
	if(set_pointer_in_ptr(args->io.ib_l2_ptr_in,(u32 *)&log->I2_l2))
		return -1;
	if(set_pointer_in_ptr(args->io.ib_l2_params_ptr_in,(u32 *)&log->I2_l2_params))
		return -1;
	if(set_pointer_in_ptr(args->io.ib_l3_ptr_in,(u32 *)&log->I2_l3))
		return -1;
	if(set_pointer_in_ptr(args->io.ib_l3_params_ptr_in,(u32 *)&log->I2_l3_params))
		return -1;

	if(set_pointer_in_ptr(args->io.ic_l1_ptr_in,(u32 *)&log->I3_l1))
		return -1;
	if(set_pointer_in_ptr(args->io.ic_l1_params_ptr_in,(u32 *)&log->I3_l1_params))
		return -1;
	if(set_pointer_in_ptr(args->io.ic_l2_ptr_in,(u32 *)&log->I3_l2))
		return -1;
	if(set_pointer_in_ptr(args->io.ic_l2_params_ptr_in,(u32 *)&log->I3_l2_params))
		return -1;
	if(set_pointer_in_ptr(args->io.ic_l3_ptr_in,(u32 *)&log->I3_l3))
		return -1;
	if(set_pointer_in_ptr(args->io.ic_l3_params_ptr_in,(u32 *)&log->I3_l3_params))
		return -1;
/*
	if(set_pointer_in_ptr(args->io.id_l1_ptr_in,(u32 *)&log->I4_l1))
		return -1;
	if(set_pointer_in_ptr(args->io.id_l1_params_ptr_in,(u32 *)&log->I4_l1_params))
		return -1;
	if(set_pointer_in_ptr(args->io.id_l2_ptr_in,(u32 *)&log->I4_l2))
		return -1;
	if(set_pointer_in_ptr(args->io.id_l2_params_ptr_in,(u32 *)&log->I4_l2_params))
		return -1;
	if(set_pointer_in_ptr(args->io.id_l3_ptr_in,(u32 *)&log->I4_l3))
		return -1;
	if(set_pointer_in_ptr(args->io.id_l3_params_ptr_in,(u32 *)&log->I4_l3_params))
		return -1;
*/
	if(set_bit_ptr_struct(args->io.test_in,&log->test))
		return -1;

	if(set_bit_ptr_struct(args->io.w_out,&log->W))
		return -1;

	if(set_bit_ptr_struct(args->io.z_out,&log->Z))
		return -1;

	if(set_bit_ptr_struct(args->io.p_out,&log->P))
		return -1;

	if(set_bit_ptr_struct(args->io.pl1_out,&log->Pf1))
		return -1;

	if(set_bit_ptr_struct(args->io.pl2_out,&log->Pf2))
		return -1;

	if(set_bit_ptr_struct(args->io.pl3_out,&log->Pf3))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_2h_L1_out,&log->BL2h1))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_2h_L2_out,&log->BL2h2))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_2h_L3_out,&log->BL2h3))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_5h_L1_out,&log->BL5h1))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_5h_L2_out,&log->BL5h2))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_5h_L3_out,&log->BL5h3))
		return -1;

	if(set_bit_ptr_struct(args->io.zzew1_out,&log->Zw1))
		return -1;

	if(set_bit_ptr_struct(args->io.zzew2_out,&log->Zw2))
		return -1;

	if(set_bit_ptr_struct(args->io.zzew3_out,&log->Zw3))
		return -1;

	if(set_bit_ptr_struct(args->io.nieobs_out,&log->nieobs))
		return -1;
		
	if(set_bit_ptr_struct(args->io.pob_bezwar_out,&log->pob_bezwar))
		return -1;	

	if(set_float_ptr(args->io.di_l1_float_out,&log->Ir1))
		return -1;
	if(set_float_ptr(args->io.di_l2_float_out,&log->Ir2))
		return -1;
	if(set_float_ptr(args->io.di_l3_float_out,&log->Ir3))
		return -1;

	if(set_float_ptr(args->io.ih_l1_float_out,&log->Ih1))
		return -1;
	if(set_float_ptr(args->io.ih_l2_float_out,&log->Ih2))
		return -1;
	if(set_float_ptr(args->io.ih_l3_float_out,&log->Ih3))
		return -1;
	if(set_float_ptr(args->io.i2h_l1_float_out,&log->I2h1))
		return -1;
	if(set_float_ptr(args->io.i2h_l2_float_out,&log->I2h2))
		return -1;
	if(set_float_ptr(args->io.i2h_l3_float_out,&log->I2h3))
		return -1;
	if(set_float_ptr(args->io.i5h_l1_float_out,&log->I5h1))
		return -1;
	if(set_float_ptr(args->io.i5h_l2_float_out,&log->I5h2))
		return -1;
	if(set_float_ptr(args->io.i5h_l3_float_out,&log->I5h3))
		return -1;

	if(set_float_ptr(args->io.ir_l1_ch_float_out,&log->Ir_L1_ortb))
		return -1;
	if(set_float_ptr(args->io.ir_l2_ch_float_out,&log->Ir_L2_ortb))
		return -1;
	if(set_float_ptr(args->io.ir_l3_ch_float_out,&log->Ir_L3_ortb))
		return -1;

	log->an_struct_i1_l1.samples_buf=log->I1_l1;
	log->an_struct_i1_l1.params=log->I1_l1_params;
	log->an_struct_i1_l2.samples_buf=log->I1_l2;
	log->an_struct_i1_l2.params=log->I1_l2_params;
	log->an_struct_i1_l3.samples_buf=log->I1_l3;
	log->an_struct_i1_l3.params=log->I1_l3_params;

	log->an_struct_i2_l1.samples_buf=log->I2_l1;
	log->an_struct_i2_l1.params=log->I2_l1_params;
	log->an_struct_i2_l2.samples_buf=log->I2_l2;
	log->an_struct_i2_l2.params=log->I2_l2_params;
	log->an_struct_i2_l3.samples_buf=log->I2_l3;
	log->an_struct_i2_l3.params=log->I2_l3_params;

	log->an_struct_i3_l1.samples_buf=log->I3_l1;
	log->an_struct_i3_l1.params=log->I3_l1_params;
	log->an_struct_i3_l2.samples_buf=log->I3_l2;
	log->an_struct_i3_l2.params=log->I3_l2_params;
	log->an_struct_i3_l3.samples_buf=log->I3_l3;
	log->an_struct_i3_l3.params=log->I3_l3_params;

    log->nast_.on =  (args->params.bity & 0x0001)?1:0;
    log->nast_.w =  (args->params.bity & 0x0002)?1:0;
    log->nast_.bl =  (args->params.bity & 0x0004)?1:0;
    log->nast_.bl2h =  (args->params.bity & 0x0008)?1:0;
    log->nast_.bl5h =  (args->params.bity & 0x0010)?1:0;
    log->nast_.sbc =  (args->params.bity & 0x0020)?1:0;
    log->nast_.h =  (args->params.bity & 0x0040)?1:0;
    log->nast_.r =  (args->params.bity & 0x0080)?1:0;
    log->nast_.cross_block =  (args->params.bity & 0x0100)?1:0;
    log->algorytm = (args->params.bity & 0x0200)?1:0;

	//ustawienie wartosci rozruchowych
	log->nast_.khr_2h = args->params.k_2h * args->params.k_2h;
	log->nast_.khp_2h = log->nast_.khr_2h * 0.98;//args->params.kp *args->params.kp;
	log->nast_.khr_5h = args->params.k_5h * args->params.k_5h;
	log->nast_.khp_5h = log->nast_.khr_5h * 0.98;//args->params.kp *args->params.kp;
	log->nast_.Irr = args->params.Ir * 1.3577f;
	log->Irr2 = args->params.Ir * args->params.Ir;
	log->Irr2_p = log->Irr2 * args->params.kp;
	log->nast_.Ipr = args->params.Ir * args->params.kp;
	log->nast_.Ihdr = args->params.Ihd;
	log->nast_.Irdr = args->params.rd;
	log->nast_.kh1r = args->params.kh1;
	log->nast_.kh1p = args->params.kh1 * args->params.kp;
	log->nast_.kh2r = args->params.kh2;
	log->nast_.kh2p = args->params.kh2 * args->params.kp;
	log->nast_.Ir3c = args->params.Ihp * (args->params.kh1 - args->params.kh2);
	log->nast_.Ip3c = log->nast_.Ir3c * args->params.kp;
	log->nast_.Iror = args->params.Iro;
	log->nast_.Irop = args->params.Iro * args->params.kp;
	log->nast_.Ir4 = args->params.Ihg * args->params.kh2 + args->params.Ihp * (args->params.kh1 - args->params.kh2);
	log->nast_.Ip4 = log->nast_.Ir4 * args->params.kp;
	log->nast_.t_sbc = args->params.tbld * 1000;
	log->nast_.Ih1 = args->params.Iro / args->params.kh1;
	log->nast_.Ihp = args->params.Ihp;
	log->nast_.Ihg = args->params.Ihg;

	log->nast_.tz = args->params.t * 1000;

//	log->nast_.ih_siemens = (args->params.Ih_typ == IH_SUM_IP) ? 1 : 0;
//--------

	struct nast_trafo n1;
	n1.GrP1 = args->params.GrP1;
	n1.GrP2 = args->params.GrP2;
	n1.GrP3 = args->params.GrP3;
	n1.PT12 = args->params.PT12;
	n1.PT13 = args->params.PT13;
//	n1.Tr_zas = args->params.Tr_zas;

	przelicz_nastawy_bis(log,n1);

	return 0;
}



/**
Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji
\param Ir wartoscs pradu roznicowego
\param Ih wartosc pradu stabilizacji
*/
void kryterium2_nowe(struct diff_1_bis_logic *log,float Ir, float Ih, u8 *pob)
{

	u16 n = 1;

	if (Ih > log->nast_.Ih1) n = 2;
	if (Ih > log->nast_.Ihp) n = 3;
	if ((Ih > log->nast_.Ihg) && log->nast_.h) n = 4;
	*pob = 0;
	*(pob + 1) = 0;
	*(pob + 2) = 1;
	switch (n)
	{
	case 1:
		*pob = Ir > log->nast_.Iror;
		*(pob + 1) = Ir < log->nast_.Irop;
		break;
	case 2:
		*pob = Ir > Ih * log->nast_.kh1r;
		*(pob + 1) = Ir < Ih * log->nast_.kh1p;
		break;
	case 3:
		*pob = Ir > (Ih * log->nast_.kh2r + log->nast_.Ir3c);
		*(pob + 1) = Ir < (Ih * log->nast_.kh2p + log->nast_.Ip3c);
		break;
	case 4:
		*pob = Ir > log->nast_.Ir4;
		*(pob + 1) = Ir < log->nast_.Ip4;
		//*(pob + 2) = *(pob + 1); zakomentowane 11.04.20222 po bledach z blokada czasowa zgloszonymi przez NC

		break;
	}
}

/**
Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji
\param Ir wartosc pradu roznicowego
\param Ih wartosc pradu stabilizacji
*/
void kryterium3_nowe(struct diff_1_bis_logic *log,float Ir, float Ih, u8 *pob)
{

	*pob = 0;
	*(pob + 1) = 0;
	*(pob + 2) = 1;

	if ((Ih > log->nast_.Ihg) && log->nast_.h)
	{
		*pob = Ir > log->nast_.Ir4;
		*(pob + 1) = Ir < log->nast_.Ip4;
		*(pob + 2) = *(pob + 1);
	}
	else {
		*pob = Ir > (log->nast_.Iror + log->nast_.kh1r2 * Ih);
		*(pob + 1) = Ir < (log->nast_.Irop + log->nast_.kh1p2 * Ih);
	}
}

void przelicz_1h_ip_nowe(struct diff_1_bis_logic *log, uint16_t Lnr)
{
	int16_t i, i1,j;
	float orta;
	float ortb;
	float pr;

	log->w.Ir[Lnr]=0;

	for(j=0;j<3;j++)
	{
		orta=0;
		ortb=0;
		i1 = log->w.nr_probki;

		for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++)
		{
			i1--;
			if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2;
			pr = log->w.buf_ip[i1][((Lnr-3)*3)+j];
			orta += pr * log->nast_.wsp_filtr_sin_[i];
			ortb += pr * log->nast_.wsp_filtr_cos_[i];
		}

		pr = orta * orta + ortb * ortb;
		log->w.Ir[Lnr] += sqrtf(pr / 2);
	}

	log->w.Ir[Lnr]/=2;
	log->w.Ir[Lnr]*=log->w.Ir[Lnr];
}

/**
Wylicza wartosci chwilowe odfiltrowanego przebiegu (ortogonalna a(sinus) i b(cosinus))
Wpisuje wartoci ortogonalnych a do stan_.orta
Wpisuje wartoci ortogonalnych b do stan_.ortb_
Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl
\param: Lnr - numer fazy
*/
void przelicz_1h_nowe(struct diff_1_bis_logic *log, uint16_t Lnr)
{
	int16_t i, i1;
	float orta = 0;
	float ortb = 0;
	float pr;
	i1 = log->w.nr_probki;
	for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++)
	{
		i1--;
		if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2;
		pr = log->w.buf_Lr[i1][Lnr];
		orta += pr * log->nast_.wsp_filtr_sin_[i];
		ortb += pr * log->nast_.wsp_filtr_cos_[i];
	}

	log->w.orta[Lnr] = orta;
	log->w.ortb[Lnr] = ortb;

	pr = orta * orta + ortb * ortb;
	log->w.Ir[Lnr] = pr / 2;
}


/**
Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus))
Wpisuje wartoci ortogonalnych a do stan_.orta
Wpisuje wartoci ortogonalnych b do stan_.ortb_
Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl
\param: Lnr - numer fazy
*/
void przelicz_2h_nowe(struct diff_1_bis_logic *log,uint16_t Lnr)
{

	short i, i1;
	float orta = 0;
	float ortb = 0;
	float pr;
	i1 = log->w.nr_probki;
	for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++)
	{
		i1--;
		if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2;
		pr = log->w.buf_Lr[i1][Lnr];
		orta += pr * log->nast_.wsp_filtr_sin2h_[i];
		ortb += pr * log->nast_.wsp_filtr_cos2h_[i];
	}

	log->w.orta2h[Lnr] = orta;
	log->w.ortb2h[Lnr] = ortb;

	pr = orta * orta + ortb * ortb;
	log->w.I2h[Lnr] = pr / 2;

}


/**
Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus))
Wpisuje wartoci ortogonalnych a do stan_.orta
Wpisuje wartoci ortogonalnych b do stan_.ortb_
Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl
\param: Lnr - numer fazy
*/
void przelicz_5h_nowe(struct diff_1_bis_logic *log,uint16_t Lnr)
{
	short i, i1;
	float orta = 0;
	float ortb = 0;
	float pr;
	i1 = log->w.nr_probki;
	for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++)
	{
		i1--;
		if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2;
		pr = log->w.buf_Lr[i1][Lnr];
		orta += pr * log->nast_.wsp_filtr_sin5h_[i];
		ortb += pr * log->nast_.wsp_filtr_cos5h_[i];
	}

	log->w.orta5h[Lnr] = orta;
	log->w.ortb5h[Lnr] = ortb;

	pr = orta * orta + ortb * ortb;
	log->w.I5h[Lnr] = pr / 2;
}



void przelicz_1200Hz_1_nowe(struct diff_1_bis_logic *log)
{
	uint16_t i;
	float local1, local2,b,c;
	float i0[3];
	// wyliczenie pradu ronicowego i stabilizacji
	float ich[3];

	while (log->w.nr_probki != bus_an_cur_sample_num)
	{
		log->w.nr_probki++;
		if (log->w.nr_probki > ((SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1)) log->w.nr_probki = 0;

		i0[0]=log->nast_.L[0].a_p[0][0] * ((float)log->nast_.L[0].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[0][1])
		+  log->nast_.L[1].a_p[0][0] * ((float)log->nast_.L[1].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[0][1])
		+  log->nast_.L[2].a_p[0][0] * ((float)log->nast_.L[2].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[0][1]);

		i0[1]=log->nast_.L[0].a_p[1][0] * ((float)log->nast_.L[0].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[1][1])
		+  log->nast_.L[1].a_p[1][0] * ((float)log->nast_.L[1].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[1][1])
		+  log->nast_.L[2].a_p[1][0] * ((float)log->nast_.L[2].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[1][1]);

		i0[2]=log->nast_.L[0].a_p[2][0] * ((float)log->nast_.L[0].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[2][1])
		+  log->nast_.L[1].a_p[2][0] * ((float)log->nast_.L[1].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[2][1])
		+  log->nast_.L[2].a_p[2][0] * ((float)log->nast_.L[2].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[2][1]);

		i0[0]/=3;
		i0[1]/=3;
		i0[2]/=3;

		for (i = 0; i < 3; i++)
		{


			local1 = log->nast_.L[i].a_p[0][0] * ((float)log->nast_.L[i].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[0][1])
				- log->nast_.L[i].a_m[0][0] * ((float)log->nast_.L[i].Im[0]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[0][1]);

			b = log->nast_.L[i].a_p[1][0] * ((float)log->nast_.L[i].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[1][1])
			  - log->nast_.L[i].a_m[1][0] * ((float)log->nast_.L[i].Im[1]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[1][1]);

			c = log->nast_.L[i].a_p[2][0] * ((float)log->nast_.L[i].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[2][1])
			  - log->nast_.L[i].a_m[2][0] * ((float)log->nast_.L[i].Im[2]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[2][1]);

			if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y)
			{
				local1-=i0[0];
				if(!log->nast_.p2d)
					b-=i0[1];
				if(!log->nast_.p3d)
					c-=i0[2];
			}

			local2 = b + c
				+ log->nast_.L[i].a_p[3][0] * ((float)log->nast_.L[i].Ip[3]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[3][1])
				- log->nast_.L[i].a_m[3][0] * ((float)log->nast_.L[i].Im[3]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[3][1]);

			//log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2;
			log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2; //zmienino 09.11.2020
			log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) / 2;
			ich[i] = fabs(log->w.buf_Lr[log->w.nr_probki][0 + i]);

			log->w.buf_ip[log->w.nr_probki][i*3 + 0] = local1;
			log->w.buf_ip[log->w.nr_probki][i*3 + 1] = b;
			log->w.buf_ip[log->w.nr_probki][i*3 + 2] = c;
		}


		if (log->nast_.r) // jesli wlaczone wylaczenie bezwarunkowe
		{
			if (log->algorytm == 0)
			{
				log->P_.Irr1 = kryterium1(&log->l.Ir1[0],
					(ich[0] > log->nast_.Irr),
					(ich[0]<log->nast_.Ipr),
					log->P_.Irr1,
					c1r, c1p);
				log->P_.Irr2 = kryterium1(&log->l.Ir2[0],
					(ich[1]>log->nast_.Irr),
					(ich[1]<log->nast_.Ipr),
					log->P_.Irr2,
					c1r, c1p);
				log->P_.Irr3 = kryterium1(&log->l.Ir3[0],
					(ich[2]>log->nast_.Irr),
					(ich[2] < log->nast_.Ipr),
					log->P_.Irr3,
					c1r, c1p);
			}
			else
			{
				log->P_.Irr1 = kryterium1(&log->l.Ir1[0],
					(log->w.Ir[0] >log->Irr2),
					(log->w.Ir[0]<log->Irr2_p),
					log->P_.Irr1,
					c1r_nowe, c1p_nowe);
				log->P_.Irr2 = kryterium1(&log->l.Ir2[0],
					(log->w.Ir[1]>log->Irr2),
					(log->w.Ir[1]<log->Irr2_p),
					log->P_.Irr2,
					c1r_nowe, c1p_nowe);
				log->P_.Irr3 = kryterium1(&log->l.Ir3[0],
					(log->w.Ir[2]>log->Irr2),
					(log->w.Ir[2]<log->Irr2_p),
					log->P_.Irr3,
					c1r_nowe, c1p_nowe);
			}	
		}
		else log->P_.Irr1 = log->P_.Irr2 = log->P_.Irr3 = 0;

		check_and_set_struct((log->P_.Irr1 ||log->P_.Irr2 || log->P_.Irr3),&log->pob_bezwar);


	}

	//----------------------------------------------------------------------------------------

	//filtracja przebiegow


	// 1h pradu rozruchowego


	przelicz_1h_nowe(log,0);
	przelicz_1h_nowe(log,1);
	przelicz_1h_nowe(log,2);

	przelicz_1h_ip_nowe(log,3);
	przelicz_1h_ip_nowe(log,4);
	przelicz_1h_ip_nowe(log,5);

	//-----------------------------------------------------------------------------------------

	u8 war1 = log->w.Ir[0] > 0.0016;
	u8 war2 = log->w.Ir[1] > 0.0016;
	u8 war3 = log->w.Ir[2] > 0.0016;



	log->Ir1_ = sqrtf(log->w.Ir[0]);
	log->Ir2_ = sqrtf(log->w.Ir[1]);
	log->Ir3_ = sqrtf(log->w.Ir[2]);
	log->Ih1_ = sqrtf(log->w.Ir[3]);
	log->Ih2_ = sqrtf(log->w.Ir[4]);
	log->Ih3_ = sqrtf(log->w.Ir[5]);

	if (log->nast_.bl2h) //jesli aktywna blokada od drugiej harmonicznej
	{

		// druga harmoniczna
		przelicz_2h_nowe(log,0);
		przelicz_2h_nowe(log,1);
		przelicz_2h_nowe(log,2);


		log->P_.Bl2h_L1 = kryterium1(&log->l.l2h1[0],
			((log->w.I2h[0] > log->nast_.khr_2h*log->w.Ir[0]) && war1),
			((log->w.I2h[0]<log->nast_.khp_2h*log->w.Ir[0]) || !war1),
			log->P_.Bl2h_L1,
			c2r, c2p);
		log->P_.Bl2h_L2 = kryterium1(&log->l.l2h2[0],
			((log->w.I2h[1]>log->nast_.khr_2h*log->w.Ir[1]) && war2),
			((log->w.I2h[1]<log->nast_.khp_2h*log->w.Ir[1]) || !war2),
			log->P_.Bl2h_L2,
			c2r, c2p);
		log->P_.Bl2h_L3 = kryterium1(&log->l.l2h3[0],
			((log->w.I2h[2]>log->nast_.khr_2h*log->w.Ir[2]) && war3),
			((log->w.I2h[2]<log->nast_.khp_2h*log->w.Ir[2]) || !war3),
			log->P_.Bl2h_L3,
			c2r, c2p);

		if (log->nast_.cross_block != 0)
		{
			if (log->P_.Bl2h_L1 || log->P_.Bl2h_L2 || log->P_.Bl2h_L3)
			{
				log->P_.Bl2h_L1 = 1;
				log->P_.Bl2h_L2 = 1;
				log->P_.Bl2h_L3 = 1;
			}
			else
			{
				log->P_.Bl2h_L1 = 0;
				log->P_.Bl2h_L2 = 0;
				log->P_.Bl2h_L3 = 0;
			}
		}
	}
	else log->P_.Bl2h_L1 = log->P_.Bl2h_L2 = log->P_.Bl2h_L3 = 0;

	if (log->nast_.bl5h) //jesli aktywna blokada od piatej harmonicznej
	{
		// piata harmoniczna
		przelicz_5h_nowe(log,0);
		przelicz_5h_nowe(log,1);
		przelicz_5h_nowe(log,2);

		log->P_.Bl5h_L1 = kryterium1(&log->l.l5h1[0],
			((log->w.I5h[0]>log->nast_.khr_5h*log->w.Ir[0]) && war1),
			((log->w.I5h[0]<log->nast_.khp_5h*log->w.Ir[0]) || !war1),
			log->P_.Bl5h_L1,
			c3r, c3p);
		log->P_.Bl5h_L2 = kryterium1(&log->l.l5h2[0],
			((log->w.I5h[1]>log->nast_.khr_5h*log->w.Ir[1]) && war2),
			((log->w.I5h[1]<log->nast_.khp_5h*log->w.Ir[1]) || !war2),
			log->P_.Bl5h_L2,
			c3r, c3p);
		log->P_.Bl5h_L3 = kryterium1(&log->l.l5h3[0],
			((log->w.I5h[2]>log->nast_.khr_5h*log->w.Ir[2]) && war3),
			((log->w.I5h[2] < log->nast_.khp_5h*log->w.Ir[2]) || !war3),
			log->P_.Bl5h_L3,
			c3r, c3p);

		if (log->nast_.cross_block != 0)
		{
			if (log->P_.Bl5h_L1 || log->P_.Bl5h_L2 || log->P_.Bl5h_L3)
			{
				log->P_.Bl5h_L1 = 1;
				log->P_.Bl5h_L2 = 1;
				log->P_.Bl5h_L3 = 1;
			}
			else
			{
				log->P_.Bl5h_L1 = 0;
				log->P_.Bl5h_L2 = 0;
				log->P_.Bl5h_L3 = 0;
			}
		}

	}
	else log->P_.Bl5h_L1 = log->P_.Bl5h_L2 = log->P_.Bl5h_L3 = 0;
}

void przelicz_1200Hz_2_nowe(struct diff_1_bis_logic *log)
{
	if (log->nast_.sbc) //jesli wwlaczona czasowa blokada dzialania
	{
		log->dw.sbc_P = (log->Ih1_ > log->nast_.Ihdr) && (log->Ir1_ < (log->Ih1_ * log->nast_.kh1r - log->nast_.Irdr));
		check_and_set_struct(log->P_.BlZw1 && kryterium1(&log->l.sbc1[0],
				log->dw.sbc_P,
				!log->dw.sbc_P,
				check_struct(&log->Zw1),
				c4r, log->nast_.t_sbc),
				&log->Zw1);

		log->dw.sbc_P = (log->Ih2_ > log->nast_.Ihdr) && (log->Ir2_ < (log->Ih2_ * log->nast_.kh1r - log->nast_.Irdr));
		check_and_set_struct(log->P_.BlZw2 && kryterium1(&log->l.sbc2[0],
				log->dw.sbc_P,
				!log->dw.sbc_P,
				check_struct(&log->Zw2),
				c4r, log->nast_.t_sbc),
				&log->Zw2);

		log->dw.sbc_P = (log->Ih3_ > log->nast_.Ihdr) && (log->Ir3_ < (log->Ih3_ * log->nast_.kh1r - log->nast_.Irdr));
		check_and_set_struct(log->P_.BlZw3 && kryterium1(&log->l.sbc3[0],
			log->dw.sbc_P,
			!log->dw.sbc_P,
			check_struct(&log->Zw3),
			c4r, log->nast_.t_sbc),
			&log->Zw3);
	}
	else
	{
		clear_struct(&log->Zw1);
		clear_struct(&log->Zw2);
		clear_struct(&log->Zw3);
	}

	log->P_.Psf1 = kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, c5r, c5p);
	log->P_.Psf2 = kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, c5r, c5p);
	log->P_.Psf3 = kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, c5r, c5p);

	//---------------------------------------------------------

	//ustawienie wyjcia

	check_and_set_struct(log->P_.Irr1 || //pobudzenie bezwarunkowe
			(log->P_.Psf1 && //prad w strefie dzialania
				!check_struct(&log->Zw1) && //brak blokady czasowej
				!log->P_.Bl2h_L1 && //brak blokady od drugiej harmonicznej
				!log->P_.Bl5h_L1),//brak blokady od piatej harmonicznej)
				&log->Pf1);

	check_and_set_struct(log->P_.Irr2 || //pobudzenie bezwarunkowe
			(log->P_.Psf2 && //prad w strefie dzialania
				!check_struct(&log->Zw2) && //brak blokady czasowej
				!log->P_.Bl2h_L2 && //brak blokady od drugiej harmonicznej
				!log->P_.Bl5h_L2),//brak blokady od piatej harmonicznej
				&log->Pf2);

	check_and_set_struct(log->P_.Irr3 || //pobudzenie bezwarunkowe
		(log->P_.Psf3 && //prad w strefie dzialania
			!check_struct(&log->Zw3) && //brak blokady czasowej
			!log->P_.Bl2h_L3 && //brak blokady od drugiej harmonicznej
			!log->P_.Bl5h_L3),//brak blokady od piatej harmonicznej
			&log->Pf3);

//-------------------

	check_and_set_struct(check_struct(&log->Pf1)||check_struct(&log->Pf2)||check_struct(&log->Pf3),&log->P);

	//obsluga wejscia testu przekaznika
	if (czy_test_R())
	{
		check_and_set_struct(check_struct(&log->test),&log->P);
		check_and_set_struct(check_struct(&log->test),&log->Pf1);
		check_and_set_struct(check_struct(&log->test),&log->Pf2);
		check_and_set_struct(check_struct(&log->test),&log->Pf3);
	}

	// realizacja blokady dzialania przekaznika

	if (((log->nast_.bl && check_struct(&log->stan_bl))||check_struct(&log->nieobs))/* || czy_start()*/)
	{
		clear_struct(&log->P);
		clear_struct(&log->Pf1);
		clear_struct(&log->Pf2);
		clear_struct(&log->Pf3);
        clear_struct(&log->pob_bezwar);
	}
	//

   // realizacja przekaznika czasowego
	if (check_struct(&log->P))
		log->dw.licz_t+=LOOP_CYCLE_MS;
	else
		log->dw.licz_t = 0; // odliczanie czasu dzialania

	if (log->dw.licz_t > log->nast_.tz)
	{ // sprawdzenie przekroczenia nastawionego czasu
		//if (!check_struct(&log->Z)) log->dw.czas_ = time;
		set_struct(&log->Z); // ustawienie zadzialania po odliczeniu czasu
		log->dw.licz_t-=LOOP_CYCLE_MS;
	}
	else clear_struct(&log->Z);
	//

	check_and_set_struct(check_struct(&log->Z) && log->nast_.w,&log->W); //wyprowadzenie wylaczenia zaleznie od nastawy


//wystawienie pomiarow

	if(!check_struct(&log->nieobs))
	{
		*log->Ir1 = log->w.Ir[0];
		*log->Ir2 = log->w.Ir[1];
		*log->Ir3 = log->w.Ir[2];
		*log->Ih1 = log->w.Ir[3];
		*log->Ih2 = log->w.Ir[4];
		*log->Ih3 = log->w.Ir[5];
		*log->Ir_L1_ortb = log->w.buf_Lr[log->w.nr_probki][0];
		*log->Ir_L2_ortb = log->w.buf_Lr[log->w.nr_probki][1];
		*log->Ir_L3_ortb = log->w.buf_Lr[log->w.nr_probki][2];
		*log->I2h1 = log->w.I2h[0];
		*log->I2h2 = log->w.I2h[1];
		*log->I2h3 = log->w.I2h[2];
		*log->I5h1 = log->w.I5h[0];
		*log->I5h2 = log->w.I5h[1];
		*log->I5h3 = log->w.I5h[2];
	}
	else
	{
		*log->Ir1 = 0;
		*log->Ir2 = 0;
		*log->Ir3 = 0;
		*log->Ih1 = 0;
		*log->Ih2 = 0;
		*log->Ih3 = 0;
		*log->Ir_L1_ortb = 0;
		*log->Ir_L2_ortb = 0;
		*log->Ir_L3_ortb = 0;
		*log->I2h1 = 0;
		*log->I2h2 = 0;
		*log->I2h3 = 0;
		*log->I5h1 = 0;
		*log->I5h2 = 0;
		*log->I5h3 = 0;
	}

//----------------------------------------------
	check_and_set_struct(log->P_.Bl2h_L1 && log->P_.Psf1,&log->BL2h1);
	check_and_set_struct(log->P_.Bl2h_L2 && log->P_.Psf2,&log->BL2h2);
	check_and_set_struct(log->P_.Bl2h_L3 && log->P_.Psf3,&log->BL2h3);
	check_and_set_struct(log->P_.Bl5h_L1 && log->P_.Psf1,&log->BL5h1);
	check_and_set_struct(log->P_.Bl5h_L2 && log->P_.Psf2,&log->BL5h2);
	check_and_set_struct(log->P_.Bl5h_L3 && log->P_.Psf3,&log->BL5h3);
}	


void diff_1_bis(void *arguments, void *logic)
{
	struct diff_1_bis_logic *log = (struct diff_1_bis_logic *)logic;

    if(log->nast_.on)   // jesli zabezpieczenie aktywne
    {
		przelicz_1200Hz_1_nowe(log);

		//Sprawdzenie pobudzenia w stabilizowanej strefie dzialania
		kryterium2_nowe(log,log->Ir1_,log->Ih1_,&log->P_.Pb1);
		kryterium2_nowe(log,log->Ir2_,log->Ih2_,&log->P_.Pb2);
		kryterium2_nowe(log,log->Ir3_,log->Ih3_,&log->P_.Pb3);

		przelicz_1200Hz_2_nowe(log);
	} else {
		*log->Ir1=0;
		*log->Ir2=0;
		*log->Ir3=0;
		*log->Ih1=0;
		*log->Ih2=0;
		*log->Ih3=0;
		*log->Ir_L1_ortb=0;
		*log->Ir_L2_ortb=0;
		*log->Ir_L3_ortb=0;
	//	*log->Ih_L1_ortb=0;
	  //*log->Ih_L2_ortb=0;
	  //*log->Ih_L3_ortb=0;
		*log->I2h1=0;
		*log->I2h2=0;
		*log->I2h3=0;
		*log->I5h1=0;
		*log->I5h2=0;
		*log->I5h3=0;
		clear_struct(&log->P);
		clear_struct(&log->Pf1);
		clear_struct(&log->Pf2);
		clear_struct(&log->Pf3);
		clear_struct(&log->W);
		clear_struct(&log->Z);
		clear_struct(&log->Zw1);
		clear_struct(&log->Zw2);
		clear_struct(&log->Zw3);
		clear_struct(&log->BL2h1);
		clear_struct(&log->BL2h2);
		clear_struct(&log->BL2h3);
		clear_struct(&log->BL5h1);
		clear_struct(&log->BL5h2);
		clear_struct(&log->BL5h3);
		clear_struct(&log->pob_bezwar);
	}
}
