/*
 *  R87L.c
 *
 *  Created on: 20-05-2019
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>
#include <string.h>

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

#include "helper.h"
#include "R87L.h"
#include "analog_in.h"
#include "Diff.h"

/**
Ustawia wskazniki na prady po skompensowaniu
\param typ wskazuje na podstawie nastawy odpowiednia konfiguracje pradow
\param nr_pradu okreslenie numeru przetwarzanago pradu
*/

static void ustaw_kompensacje(struct r87l_logic *log, enum typ_kompensacji typ, u16 nr_pradu)
{
	float *a0,*a1,*a2;
	float *a0_prev,*a1_prev,*a2_prev;
	switch(nr_pradu)
	{
		case 0:
			a0 = &log->A[0][0];//&log->stream[0]->A[0];
			a1 = &log->A[0][1];//stream[0]->A[1];
			a2 = &log->A[0][2];//stream[0]->A[2];
			a0_prev = &log->A_prev[0][0];//&log->stream[0]->A[0];
			a1_prev = &log->A_prev[0][1];//stream[0]->A[1];
			a2_prev = &log->A_prev[0][2];//stream[0]->A[2];
			break;

		case 1:
			a0 = &log->A[1][0];//&log->stream[1]->A[0];
			a1 = &log->A[1][1];//stream[1]->A[1];
			a2 = &log->A[1][2];//stream[1]->A[2];
			a0_prev = &log->A_prev[1][0];//&log->stream[1]->A[0];
			a1_prev = &log->A_prev[1][1];//stream[1]->A[1];
			a2_prev = &log->A_prev[1][2];//stream[1]->A[2];
			break;
		default: // 2 and others
			a0 = &log->A[2][0];//&log->stream[0]->A[0];
			a1 = &log->A[2][1];//stream[0]->A[1];
			a2 = &log->A[2][2];//stream[0]->A[2];
			a0_prev = &log->A_prev[2][0];//&log->stream[0]->A[0];
			a1_prev = &log->A_prev[2][1];//stream[0]->A[1];
			a2_prev = &log->A_prev[2][2];//stream[0]->A[2];
			break;
	}

	switch (typ)
	{
	case zero:
		log->nast_.L[0].Ip[nr_pradu] = &log->nast_.zero;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero;
		log->nast_.a[nr_pradu] = 0;

		log->nast_.L[0].Ip_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero;

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

		log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero;

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

		log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev;//
		log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero;
		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;

		log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = a1_prev;
		log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = a0_prev;
		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;

		log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = a1_prev;
		log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = a0_prev;
		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;

		log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = a2_prev;
		log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = a0_prev;
		log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = a1_prev;
		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;

		log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = a2_prev;
		log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = a0_prev;
		log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = a1_prev;

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

		log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero;
		break;
	case O:
		log->nast_.L[0].Ip[nr_pradu] = a1;
		log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip[nr_pradu] = a2;
		log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip[nr_pradu] = a0;
		log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero;
		log->nast_.a[nr_pradu] = -1;

		log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero;
		break;

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

		log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero;
		break;

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

		log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero;
		log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero;
		break;


	case W:
		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;

		log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = a1_prev;
		log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = a2_prev;
		log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = a0_prev;
		break;

	case X:
		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;

		log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = a2_prev;
		log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = a0_prev;
		log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = a1_prev;
		break;
	case Y:
		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;

		log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = a0_prev;
		log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = a1_prev;
		log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = a2_prev;
		break;
	case Z:
		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;

		log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev;
		log->nast_.L[0].Im_prev[nr_pradu] = a0_prev;
		log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev;
		log->nast_.L[1].Im_prev[nr_pradu] = a1_prev;
		log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev;
		log->nast_.L[2].Im_prev[nr_pradu] = a2_prev;
		break;
	}
}

void przelicz_nastawy1_r87l(struct r87l_logic *log,struct nast_trafo n)
{
	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>6)
		log->nast_.p2d=1;
	else
		log->nast_.p2d=0;

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

	//ustawienie skladnikow kompensacji pradow
	switch (log->jaki_uklad)
	{
	case tylko_T2w_Y:
		ustaw_kompensacje(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Ig, 0);
		ustaw_kompensacje(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Id, 1);
		ustaw_kompensacje(log,zero, 2);
		ustaw_kompensacje(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
			;
		log->nast_.stream_cnt=2;
		break;
	case tylko_T2w_D:
		ustaw_kompensacje(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0);
		ustaw_kompensacje(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1);
		ustaw_kompensacje(log,zero, 2);
		ustaw_kompensacje(log,zero, 3);

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

		log->nast_.a[1] /= n.PT12;
		log->nast_.stream_cnt=2;

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

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

		log->nast_.a[1] /= n.PT12;
		log->nast_.a[2] /= n.PT13;
		log->nast_.stream_cnt=3;

		break;
	case tylko_T3w_D:
		ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0);
		ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1);
		ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP3]].Io, 2);
		ustaw_kompensacje(log,zero, 3);

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

		log->nast_.a[1] /= n.PT12;
		log->nast_.a[2] /= n.PT13;
		log->nast_.stream_cnt=3;

		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] * 1;//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] * 1;//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] * 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] * 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] * 1;//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] * 1;//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] * 1;//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] * 1;//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;

		log->nast_.L[i].a_pi0[0] = log->nast_.a[0] * 0.333333f;//log->nast_.L[i].Ip[0]->params->nast.mnoznik;
		log->nast_.L[i].a_pi0[1] = log->nast_.a[1] * 0.333333f;//log->nast_.L[i].Ip[1]->params->nast.mnoznik;
		log->nast_.L[i].a_pi0[2] = log->nast_.a[2] * 0.333333f;//log->nast_.L[i].Ip[2]->params->nast.mnoznik;
	}
/*
	uint16_t j;

	if(log->nast_.test_1str)
	{
		for(j=0;j<3;j++)
		{
			if(j==log->my_addr)
				continue;

			for(i=0;i<3;i++)
			{
				log->nast_.L[i].a_p[j][0]=0;
				log->nast_.L[i].a_m[j][0]=0;
				log->nast_.L[i].a_pi0[j]=0;
			}
		}
	}
*/
	// wyznaczenie wspolczynnikow filtru


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

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

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

/**
Wylicza pobudzenie blokad od drugiej i piatej harmonicznej
\param *licz wskaznik na licznik pobudzen
\param pobudzenie warunek pobudzenia przekaznika
\param powrot warunek powrotu przekaznika
\param st stan przekaznika z poprzedniej iteracji
\param powz ilosc powtorzen przy zadzialaniu
\param powp ilosc powtorzen przy powrocie
\return aktualny stan przekaznika
*/

u8 r87l_kryterium1(short *licz, u8 pobudzenie, u8 powrot, u8 st, unsigned short powz, unsigned short powp)
{
	u8 stp;
	stp = st;
	if (st)
	{
		if (powrot)
		{
			*(licz + 1) += LOOP_CYCLE_MS;
			if (*(licz + 1) > powp)
			{
				stp = 0;
				*(licz) = 0;
				*(licz + 1) = powp;
			}
		}
		else {
			*(licz + 1) -= LOOP_CYCLE_MS;
			if (*(licz + 1) < 0) *(licz + 1) = 0;
		}
	}
	else {
		if (pobudzenie)
		{
			*(licz) += LOOP_CYCLE_MS;
			if (*(licz) > powz)
			{
				stp = 1;
				*(licz) = powz;
				*(licz + 1) = 0;
			}
		}
		else {
			*(licz) -= LOOP_CYCLE_MS;
			if (*(licz) < 0) *(licz) = 0;
		}
	}
	return(stp);
}
//------------------
/**
Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji
\param Ir wartoscs pradu roznicowego
\param Ih wartosc pradu stabilizacji
*/

void r87l_kryterium2(struct r87l_logic *log,float Ir, float Ih, u8 *pob, u8 idx)
{
	u16 n = 2;// 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;

	log->n[idx]=n;

	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;
        *pob = Ir > ((Ih * log->nast_.kh1r)+log->nast_.Iror);
        *(pob + 1) = Ir < ((Ih * log->nast_.kh1p)+log->nast_.Irop);

		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);

		break;
	}
}

/**
Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji
\param Ir wartosc pradu roznicowego
\param Ih wartosc pradu stabilizacji
*/
void r87l_kryterium3(struct r87l_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 r87l_przelicz_1h_ip(struct r87l_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 < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++)
		{
			i1--;
			if (i1 < 0) i1 += MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*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 * 0.5f);
	}

	log->w.Ir[Lnr]*=0.5f;
	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 r87l_przelicz_1h(struct r87l_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 < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++)
	{
		i1--;
		if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*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 * 0.5f;
}


/**
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 r87l_przelicz_2h(struct r87l_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 < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++)
	{
		i1--;
		if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*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 * 0.5f;

}


/**
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 r87l_przelicz_5h(struct r87l_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 < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++)
	{
		i1--;
		if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*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 * 0.5f;
}

int R87L_initlog(void *arguments, void *logic)
{
	struct r87l_args *args = (struct r87l_args *)arguments;
	struct r87l_logic *log = (struct r87l_logic *)logic;

	if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl))
		return -1;

	if(set_bit_ptr_struct(args->io.test_in,&log->test))
		return -1;

	if(set_pointer_in_ptr(args->io.stream1_ptr_in,(u32*)&log->stream[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream2_ptr_in,(u32*)&log->stream[1]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream3_ptr_in,(u32*)&log->stream[2]))
		return -1;

	if(set_bit_ptr_struct(args->io.streams_ok_in,&log->streams_ok))
		return -1;

	if(set_bit_ptr_struct(args->io.zezw_w1f_in,&log->zezw_w1f))
		return -1;

	if(set_bit_ptr_struct(args->io.przeglad_in,&log->przeglad))
		return -1;

	if(set_bit_ptr_struct(0/*args->io.test_1str_in*/,&log->test_1str)) // poprawka wycofujaca test1str
		return -1;

//	if(set_pointer_in_ptr(args->io.stream1_ptr_in/*args->io.ownstream_ptr_in*/,(u32*)&log->ownstream)) // poprawka wycofujaca test1str
//		return -1;

	if(set_bit_ptr_struct(args->io.W_out,&log->W))
		return -1;
	if(set_bit_ptr_struct(args->io.WL1_out,&log->Wf[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.WL2_out,&log->Wf[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.WL3_out,&log->Wf[2]))
		return -1;

	if(set_bit_ptr_struct(args->io.Z_out,&log->Z))
		return -1;
	if(set_bit_ptr_struct(args->io.ZL1_out,&log->Zf[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.ZL2_out,&log->Zf[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.ZL3_out,&log->Zf[2]))
		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->Pf[0]))
		return -1;

	if(set_bit_ptr_struct(args->io.PL2_out,&log->Pf[1]))
		return -1;

	if(set_bit_ptr_struct(args->io.PL3_out,&log->Pf[2]))
		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_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;

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

	if(set_bit_ptr_struct(args->io.blok_out,&log->blok))
		return -1;

	if(set_float_ptr(args->io.comm_bits_float_out,(float *)&log->comm_bits))
		return -1;

	if(set_bit_ptr_struct(args->io.blok_w_out,&log->blok_w))
		return -1;

    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->nast_.przyspiesz_dzialanie = (args->params.bity & 0x0200)?1:0;
    log->nast_.przyjmuj_zadzialania = (args->params.bity & 0x0400)?1:0;
    log->nast_.rozwijajace = (args->params.bity & 0x0800)?1:0;
    //log->nast_.test_1str = (args->params.bity & 0x1000)?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 * args->params.Ir;// * 0.93f // 1.3577f;
	log->nast_.Ipr = args->params.Ir * args->params.kp * 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_.Ir3c = args->params.Ihp * (args->params.kh1 - args->params.kh2)+ args->params.Iro;

	log->nast_.Ip3c = log->nast_.Ir3c * args->params.kp;
	log->nast_.Iror = args->params.Iro;
	log->nast_.I2h_min = log->nast_.Iror * log->nast_.Iror * log->nast_.khr_2h;
	log->nast_.I5h_min = log->nast_.Iror * log->nast_.Iror * log->nast_.khr_5h;
	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_.Ir4 = args->params.Ihp * args->params.kh1 + args->params.kh2 * (args->params.Ihg - args->params.Ihp)+ args->params.Iro;
	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_.zero_frame.A[0] = 0.0f;
	log->nast_.zero_frame.A[1] = 0.0f;
	log->nast_.zero_frame.A[2] = 0.0f;
	log->nast_.zero=0.0f;

	//
	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_nastawy1_r87l(log,n1);
	*log->comm_bits=0;

	log->my_addr=args->params.adres;
	log->original_stream=log->stream[log->my_addr];
	log->ownstream=log->original_stream; // poprawka usuwajaca 1stronne testowanie

	log->startup_timer=R87L_START_DELAY;
	log->no_stream_timer=R87L_NO_STREAM_BLOK_WYL_TIME;

	return 0;
}

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

	if(log->startup_timer)
		*log->comm_bits|=R87L_COMM_BLOK_WYL;
	else
		*log->comm_bits&=~R87L_COMM_BLOK_WYL;

//	while (log->w.nr_probki != bus_an_cur_sample_num)
	{
		log->w.nr_probki++;
		log->w.nr_probki%=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*2);

		if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y)
		{
			i0[0]=log->nast_.L[0].a_pi0[0] * (*log->nast_.L[0].Ip_prev[0] - log->nast_.L[0].a_p[0][1])
			+  log->nast_.L[1].a_pi0[0] * (*log->nast_.L[1].Ip_prev[0] - log->nast_.L[1].a_p[0][1])
			+  log->nast_.L[2].a_pi0[0] * (*log->nast_.L[2].Ip_prev[0] - log->nast_.L[2].a_p[0][1]);

			i0[1]=log->nast_.L[0].a_pi0[1] * (*log->nast_.L[0].Ip_prev[1] - log->nast_.L[0].a_p[1][1]);
			i0[1]+=log->nast_.L[1].a_pi0[1] * (*log->nast_.L[1].Ip_prev[1] - log->nast_.L[1].a_p[1][1]);
			i0[1]+=log->nast_.L[2].a_pi0[1] * (*log->nast_.L[2].Ip_prev[1] - log->nast_.L[2].a_p[1][1]);

			i0[2]=log->nast_.L[0].a_pi0[2] * (*log->nast_.L[0].Ip_prev[2] - log->nast_.L[0].a_p[2][1])
			+  log->nast_.L[1].a_pi0[2] * (*log->nast_.L[1].Ip_prev[2] - log->nast_.L[1].a_p[2][1])
			+  log->nast_.L[2].a_pi0[2] * (*log->nast_.L[2].Ip_prev[2] - log->nast_.L[2].a_p[2][1]);

			if(check_struct(&log->test_1str))
			{
				for(i=0;i<3;i++)
				{
					if(i==log->my_addr)
						continue;
					i0[i]=0;
				}
			}
		}


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


			local1 = log->nast_.L[i].a_p[0][0] * (*log->nast_.L[i].Ip_prev[0] - log->nast_.L[i].a_p[0][1])
				- log->nast_.L[i].a_m[0][0] * (*log->nast_.L[i].Im_prev[0] - log->nast_.L[i].a_m[0][1]);

			if(check_struct(&log->test_1str) && log->my_addr!=0)
				local1=0;

			b = log->nast_.L[i].a_p[1][0] * (*log->nast_.L[i].Ip_prev[1] - log->nast_.L[i].a_p[1][1])
			  - log->nast_.L[i].a_m[1][0] * (*log->nast_.L[i].Im_prev[1] - log->nast_.L[i].a_m[1][1]);

			if(check_struct(&log->test_1str) && log->my_addr!=1)
				b=0;

			c = log->nast_.L[i].a_p[2][0] * (*log->nast_.L[i].Ip_prev[2] - log->nast_.L[i].a_p[2][1])
			  - log->nast_.L[i].a_m[2][0] * (*log->nast_.L[i].Im_prev[2] - log->nast_.L[i].a_m[2][1]);

			if(check_struct(&log->test_1str) && log->my_addr!=2)
				c=0;


			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] * (*log->nast_.L[i].Ip[3] - log->nast_.L[i].a_p[3][1])
			//	- log->nast_.L[i].a_m[3][0] * (*log->nast_.L[i].Im[3] - log->nast_.L[i].a_m[3][1]);

//			przed zmianami plus na minus w roznicowym
//			log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2;
//			log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f;  // /2;

			log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2;
			log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f;  // /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;
		}

////// druga probka
		log->w.nr_probki++;
		log->w.nr_probki%=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*2);

		if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y)
		{
			i0[0]=log->nast_.L[0].a_pi0[0] * (*log->nast_.L[0].Ip[0] - log->nast_.L[0].a_p[0][1])
			+  log->nast_.L[1].a_pi0[0] * (*log->nast_.L[1].Ip[0] - log->nast_.L[1].a_p[0][1])
			+  log->nast_.L[2].a_pi0[0] * (*log->nast_.L[2].Ip[0] - log->nast_.L[2].a_p[0][1]);

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

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

			if(check_struct(&log->test_1str))
			{
				for(i=0;i<3;i++)
				{
					if(i==log->my_addr)
						continue;
					i0[i]=0;
				}
			}

		}

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


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

			if(check_struct(&log->test_1str) && log->my_addr!=0)
				local1=0;

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

			if(check_struct(&log->test_1str) && log->my_addr!=1)
				b=0;

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

			if(check_struct(&log->test_1str) && log->my_addr!=2)
				c=0;

			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] * (*log->nast_.L[i].Ip[3] - log->nast_.L[i].a_p[3][1])
			//	- log->nast_.L[i].a_m[3][0] * (*log->nast_.L[i].Im[3] - log->nast_.L[i].a_m[3][1]);

//			przed zmianami plus na minus w roznicowym
//			log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2;
//			log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f;  // /2;

			log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2;
			log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f;  // /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;
		}
//// druga probka

		if (log->nast_.r && (check_struct(&log->streams_ok) || check_struct(&log->test_1str))) // jesli wlaczone wylaczenie bezwarunkowe
		{
/*
			log->P_.Irr1 = r87l_kryterium1(&log->l.Ir1[0],
				(ich[0] > log->nast_.Irr),
				(ich[0]<log->nast_.Ipr),
				log->P_.Irr1,
				c1r, c1p);
			log->P_.Irr2 = r87l_kryterium1(&log->l.Ir2[0],
				(ich[1]>log->nast_.Irr),
				(ich[1]<log->nast_.Ipr),
				log->P_.Irr2,
				c1r, c1p);
			log->P_.Irr3 = r87l_kryterium1(&log->l.Ir3[0],
				(ich[2]>log->nast_.Irr),
				(ich[2] < log->nast_.Ipr),
				log->P_.Irr3,
				c1r, c1p);
				 */
			log->P_.Irr1 = r87l_kryterium1(&log->l.Ir1[0],
				(log->w.Ir[0] > log->nast_.Irr),
				(log->w.Ir[0]<log->nast_.Ipr),
				log->P_.Irr1,
				c1r, c1p);
			log->P_.Irr2 = r87l_kryterium1(&log->l.Ir2[0],
				(log->w.Ir[1]>log->nast_.Irr),
				(log->w.Ir[1]<log->nast_.Ipr),
				log->P_.Irr2,
				c1r, c1p);
			log->P_.Irr3 = r87l_kryterium1(&log->l.Ir3[0],
				(log->w.Ir[2]>log->nast_.Irr),
				(log->w.Ir[2]< log->nast_.Ipr),
				log->P_.Irr3,
				c1r, c1p);


		}
		else if(check_struct(&log->streams_ok) || check_struct(&log->test_1str))
			log->P_.Irr1 = log->P_.Irr2 = log->P_.Irr3 = 0;

	}

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

	//filtracja przebiegow


	// 1h pradu rozruchowego

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

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

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

	u8 war1 = log->w.Ir[0] > 0.0016 && (log->w.I2h[0] >= log->nast_.I2h_min);
	u8 war2 = log->w.Ir[1] > 0.0016 && (log->w.I2h[1] >= log->nast_.I2h_min);
	u8 war3 = log->w.Ir[2] > 0.0016 && (log->w.I2h[2] >= log->nast_.I2h_min);


	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)
	{
		// druga harmoniczna
		r87l_przelicz_2h(log,0);
		r87l_przelicz_2h(log,1);
		r87l_przelicz_2h(log,2);
	}

	if (log->nast_.bl2h && (check_struct(&log->streams_ok)||check_struct(&log->test_1str))) //jesli aktywna blokada od drugiej harmonicznej
	{
		log->P_.Bl2h_L1 = r87l_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 = r87l_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 = r87l_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 if(check_struct(&log->streams_ok)||check_struct(&log->test_1str))
		log->P_.Bl2h_L1 = log->P_.Bl2h_L2 = log->P_.Bl2h_L3 = 0;

	war1 = log->w.Ir[0] > 0.0016 && (log->w.I5h[0] >= log->nast_.I5h_min);
	war2 = log->w.Ir[1] > 0.0016 && (log->w.I5h[1] >= log->nast_.I5h_min);
	war3 = log->w.Ir[2] > 0.0016 && (log->w.I5h[2] >= log->nast_.I5h_min);

	if (log->nast_.bl5h)
	{
		// piata harmoniczna
		r87l_przelicz_5h(log,0);
		r87l_przelicz_5h(log,1);
		r87l_przelicz_5h(log,2);
	}
	if (log->nast_.bl5h && (check_struct(&log->streams_ok)||check_struct(&log->test_1str))) //jesli aktywna blokada od piatej harmonicznej
	{
		log->P_.Bl5h_L1 = r87l_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 = r87l_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 = r87l_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 if(check_struct(&log->streams_ok)||check_struct(&log->test_1str))
		log->P_.Bl5h_L1 = log->P_.Bl5h_L2 = log->P_.Bl5h_L3 = 0;
}

/**
Sprawdza kryterium dzialania przekaznika
Wywolywana powinna byc po kazdej nowej probce z czestotliwoscia 1200Hz
Ustawia zmienne zawarte w structurze "stan_""
\param w struktura danych wejsciowych algorytmlw dzialania
*/
void r87l_przelicz_2(struct r87l_logic *log)
{
	//u8 stream_ok=1;
	u8 i,j,zmiana_przeglad=0;

	if (log->nast_.sbc && (check_struct(&log->streams_ok) || check_struct(&log->test_1str))) //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 && r87l_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 && r87l_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 && r87l_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 if(check_struct(&log->streams_ok)||check_struct(&log->test_1str))
	{
		clear_struct(&log->Zw1);
		clear_struct(&log->Zw2);
		clear_struct(&log->Zw3);
	}

	if(log->nast_.przyspiesz_dzialanie)
	{
		log->P_.Psf1 = r87l_kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, log->n[0]==2 ? 12 : c5r, c5p);
		log->P_.Psf2 = r87l_kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, log->n[1]==2 ? 12 : c5r, c5p);
		log->P_.Psf3 = r87l_kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, log->n[2]==2 ? 12 : c5r, c5p);
	}
	else
	{
		log->P_.Psf1 = r87l_kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, c5r, c5p);
		log->P_.Psf2 = r87l_kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, c5r, c5p);
		log->P_.Psf3 = r87l_kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, c5r, c5p);
	}
	//---------------------------------------------------------

	//ustawienie wyjcia
	if(check_struct(&log->streams_ok)||check_struct(&log->test_1str))
	{
		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->Pf[0]);

		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->Pf[1]);

		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->Pf[2]);
	}

	u8 pob=0;
	for(i=0;i<3;i++)
		if(check_struct(&log->Pf[i]))
			pob++;
/*if(pob>=2) zmiana 2024.07.11 zeby nie zapalal pobudzen w nieswoich fazach
		for(i=0;i<3;i++)
			set_struct(&log->Pf[i]);
*/
//-------------------

	check_and_set_struct(check_struct(&log->Pf[0])||check_struct(&log->Pf[1])||check_struct(&log->Pf[2]),&log->P);

	//obsluga wejscia testu przekaznika
	if (czy_test_R())
	{
		//*log->comm_bits|=R87L_COMM_TEST;
		check_and_set_struct(check_struct(&log->test),&log->P);
		check_and_set_struct(check_struct(&log->test),&log->Pf[0]);
		check_and_set_struct(check_struct(&log->test),&log->Pf[1]);
		check_and_set_struct(check_struct(&log->test),&log->Pf[2]);
	}
	else
	{
		/*if(*log->comm_bits & R87L_COMM_TEST)
			log->startup_timer=R87L_START_DELAY;

		*log->comm_bits&=~R87L_COMM_TEST;*/
	}

	// realizacja blokady dzialania przekaznika
//	if(!check_struct(&log->streams_ok)) // przeniesione z ora ponizej zeby nie rozsylac blokady przy utracie probki
//	{
//		clear_struct(&log->P);
//		clear_struct(&log->Pf[0]);
//		clear_struct(&log->Pf[1]);
//		clear_struct(&log->Pf[2]);
//	}
	if (((log->nast_.bl && check_struct(&log->stan_bl))||check_struct(&log->nieobs))/* || czy_start()*/)
	{
		set_struct(&log->blok);
		clear_struct(&log->P);
		clear_struct(&log->Pf[0]);
		clear_struct(&log->Pf[1]);
		clear_struct(&log->Pf[2]);
		pob=0;
		*log->comm_bits|=R87L_COMM_BLOK;
	}
	else
	{
		clear_struct(&log->blok);
		if(!check_struct(&log->test_1str))
			*log->comm_bits&=~R87L_COMM_BLOK;
		else
			*log->comm_bits|=R87L_COMM_BLOK;
	}

	//blokada z innej jednostki
	u8 blok_z_zew=0;
	u8 zmiana_blok_wyl=0;

	if(check_struct(&log->streams_ok))
	{
		for(j=0;j<log->nast_.stream_cnt;j++)
		{
			if(j==log->my_addr)
				continue;
			if(log->stream[j]->bits & (R87L_COMM_BLOK/*|R87L_COMM_TEST*/))
			{
				blok_z_zew=1;
				set_struct(&log->blok);
				clear_struct(&log->P);
				clear_struct(&log->Pf[0]);
				clear_struct(&log->Pf[1]);
				clear_struct(&log->Pf[2]);
				pob=0;
			}


			if((log->stream[j]->bits & R87L_COMM_PRZEGLAD) != (log->bits_prev[j] & R87L_COMM_PRZEGLAD))
				zmiana_przeglad=1;
			if((log->stream[j]->bits & R87L_COMM_BLOK_WYL) && !(log->bits_prev[j] & R87L_COMM_BLOK_WYL) && !(log->stream[j]->bits & R87L_COMM_PRZEGLAD))
				zmiana_blok_wyl=1;

			log->bits_prev[j]=log->stream[j]->bits;

		}

		log->blok_z_zew_prev=blok_z_zew;
	}
	else if(log->blok_z_zew_prev)
	{
		blok_z_zew=1;
		set_struct(&log->blok);
		clear_struct(&log->P);
		clear_struct(&log->Pf[0]);
		clear_struct(&log->Pf[1]);
		clear_struct(&log->Pf[2]);
		pob=0;
	}

	if(check_struct(&log->przeglad))
	{
		blok_z_zew=1;
		set_struct(&log->blok);
		clear_struct(&log->P);
		clear_struct(&log->Pf[0]);
		clear_struct(&log->Pf[1]);
		clear_struct(&log->Pf[2]);
		pob=0;

		if(!(*log->comm_bits & R87L_COMM_PRZEGLAD))
		{
			*log->comm_bits|=R87L_COMM_PRZEGLAD;
			zmiana_przeglad=1;
		}
	}
	else
	{
		if(*log->comm_bits & R87L_COMM_PRZEGLAD)
		{
			*log->comm_bits&=~R87L_COMM_PRZEGLAD;
			zmiana_przeglad=1;
		}
	}

	//

   // realizacja przekaznika czasowego
//rozwijajace tutaj dopisac

	for(i=0;i<3;i++)
	{
		if (check_struct(&log->Pf[i])||pob>=2)
			log->licz_t[i]+=LOOP_CYCLE_MS;
		else
			log->licz_t[i] = 0; // odliczanie czasu dzialania

//	zmieniono 08.03.2021 po uwagach z PSE
		u8 bylo_zadz=0;
		for(j=0;j<3;j++)
		{
			if(j==i)
				continue;
			if(check_struct(&log->Zf[j]))
				bylo_zadz=1;
		}
//
		//sprawdzenie wlasnego zadzialania
		if ((log->licz_t[i] > log->nast_.tz) || (bylo_zadz && log->nast_.rozwijajace && log->licz_t[i]))
		{ // sprawdzenie przekroczenia nastawionego czasu
			set_struct(&log->Zf[i]); // ustawienie zadzialania po odliczeniu czasu
			log->licz_t[i]-=LOOP_CYCLE_MS;

			if(bylo_zadz && log->nast_.rozwijajace)
			{
				set_struct(&log->Zf[0]);
				set_struct(&log->Zf[1]);
				set_struct(&log->Zf[2]);
				log->licz_t[0]=log->nast_.tz;
				log->licz_t[1]=log->nast_.tz;
				log->licz_t[2]=log->nast_.tz;
			}
		}
		else
		{
			clear_struct(&log->Zf[i]);
		}
	}


	for(i=0;i<3;i++)
	{
		u8 bylo_wyl=0;
		if(check_struct(&log->streams_ok) && !check_struct(&log->blok))
		{
			for(j=0;j<log->nast_.stream_cnt;j++)
			{
				if(j==log->my_addr)
					continue;
				if(log->stream[j]->bits & (R87L_COMM_W_L1<<i) && log->nast_.przyjmuj_zadzialania)
					bylo_wyl=1;
			}
		}
		if(check_struct(&log->Zf[i])&&log->nast_.w && !log->startup_timer)
			*log->comm_bits|=(R87L_COMM_W_L1<<i);
		else
			*log->comm_bits&=~(R87L_COMM_W_L1<<i);

		check_and_set_struct((check_struct(&log->Zf[i])||bylo_wyl) && log->nast_.w && !log->startup_timer,&log->Wf[i]);
	}

	if(check_struct(&log->streams_ok)||check_struct(&log->test_1str))
		log->no_stream_timer=0;
	else
		if(log->no_stream_timer<=R87L_NO_STREAM_BLOK_WYL_TIME)
			log->no_stream_timer+=LOOP_CYCLE_MS;

	if(!blok_z_zew && !check_struct(&log->stan_bl) && !zmiana_przeglad && !zmiana_blok_wyl && log->no_stream_timer<R87L_NO_STREAM_BLOK_WYL_TIME)
	{
		 if(log->startup_timer)
		 {
			 if(!check_struct(&log->P))
			 {
				 if(log->startup_timer<LOOP_CYCLE_MS)
					 log->startup_timer=0;
				 else
					 log->startup_timer-=LOOP_CYCLE_MS;
			 }
			 else
				 log->startup_timer=R87L_START_DELAY;
		 }
	}
	else
	{
		log->startup_timer=R87L_START_DELAY;
	}

	if(log->startup_timer)
		*log->comm_bits|=R87L_COMM_BLOK_WYL;
	else
		*log->comm_bits&=~R87L_COMM_BLOK_WYL;

	check_and_set_struct(log->startup_timer && !check_struct(&log->blok),&log->blok_w);

	if(!check_struct(&log->zezw_w1f))
	{
		for(i=0;i<3;i++)
			if(check_struct(&log->Wf[i]))
			{
				for(j=0;j<3;j++)
					set_struct(&log->Wf[j]);
				break;
			}
	}
//	if (check_struct(&log->P))
//		log->dw.licz_t++;
//	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--;
//	}
//	else clear_struct(&log->Z);
	//

	check_and_set_struct(check_struct(&log->Zf[0])||check_struct(&log->Zf[1])||check_struct(&log->Zf[2]),&log->Z);
	check_and_set_struct(check_struct(&log->Wf[0])||check_struct(&log->Wf[1])||check_struct(&log->Wf[2]),&log->W);


//wystawienie pomiarow

	if(!check_struct(&log->nieobs) && (check_struct(&log->streams_ok)||check_struct(&log->test_1str)))
	{
		*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 = 32768.0f;
		*log->Ir2 = 32768.0f;
		*log->Ir3 = 32768.0f;
		*log->Ih1 = 32768.0f;
		*log->Ih2 = 32768.0f;
		*log->Ih3 = 32768.0f;
		*log->Ir_L1_ortb = 32768.0f;
		*log->Ir_L2_ortb = 32768.0f;
		*log->Ir_L3_ortb = 32768.0f;
		*log->I2h1 = 32768.0f;
		*log->I2h2 = 32768.0f;
		*log->I2h3 = 32768.0f;
		*log->I5h1 = 32768.0f;
		*log->I5h2 = 32768.0f;
		*log->I5h3 = 32768.0f;
	}

//----------------------------------------------
	check_and_set_struct(log->P_.Bl2h_L1 && log->P_.Psf1 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h1);
	check_and_set_struct(log->P_.Bl2h_L2 && log->P_.Psf2 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h2);
	check_and_set_struct(log->P_.Bl2h_L3 && log->P_.Psf3 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h3);
	check_and_set_struct(log->P_.Bl5h_L1 && log->P_.Psf1 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h1);
	check_and_set_struct(log->P_.Bl5h_L2 && log->P_.Psf2 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h2);
	check_and_set_struct(log->P_.Bl5h_L3 && log->P_.Psf3 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h3);
}


void R87L(void *arguments, void *logic)
{
	struct r87l_logic *log = (struct r87l_logic *)logic;
//	struct r87l_args *args = (struct r87l_args *)arguments;
	u8 i,j;

	if(check_struct(&log->test_1str))
		log->stream[log->my_addr]=log->ownstream;
	else
		log->stream[log->my_addr]=log->original_stream;

	for(i=0;i<log->nast_.stream_cnt;i++)
	{
		for(j=0;j<3;j++)
		{
			if(log->stream[i]->A[j]!=32768.0f)
				log->A[i][j]=log->stream[i]->A[j];
			if(log->stream[i]->A_prev[j]!=32768.0f)
				log->A_prev[i][j]=log->stream[i]->A_prev[j];
		}
	}

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

    	if(check_struct(&log->streams_ok)||check_struct(&log->test_1str))
    	{
			//Sprawdzenie pobudzenia w stabilizowanej strefie dzialania
			r87l_kryterium2(log,log->Ir1_,log->Ih1_,&log->P_.Pb1,0);
			r87l_kryterium2(log,log->Ir2_,log->Ih2_,&log->P_.Pb2,1);
			r87l_kryterium2(log,log->Ir3_,log->Ih3_,&log->P_.Pb3,2);
    	}

		r87l_przelicz_2(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->I2h1=0;
		*log->I2h2=0;
		*log->I2h3=0;
		*log->I5h1=0;
		*log->I5h2=0;
		*log->I5h3=0;
		clear_struct(&log->P);
		clear_struct(&log->Pf[0]);
		clear_struct(&log->Pf[1]);
		clear_struct(&log->Pf[2]);
		clear_struct(&log->W);
		clear_struct(&log->Wf[0]);
		clear_struct(&log->Wf[1]);
		clear_struct(&log->Wf[2]);

		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);
	}

}
