/*
 * Diff.c
 *
 *  Created on: 12-04-2017
 *      Author: Krzysztof Jakubczyk
 */

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

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

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


	uint16_t pY_ZPrAE[25] = { 0,1,0,2,0,3,0,4,0,5,0,6,0,0,7,0,8,0,9,0,10,0,11,0,12 }; // off,y0,y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11,d0,d1,d2,d3,d4,d5,d6,d7,d8,d9,d10,d11
	uint16_t pD_ZPrAE[25] = { 0,0,1,0,2,0,3,0,4,0,5,0,6,7,0,8,0,9,0,10,0,11,0,12,0 };

	struct kom1_zprae Y2w_ZPrAE = {
		zero,zero,zero, // off
		A,A,zero, // y0
		A,O,zero, // y2
		A,V,zero, // y4
		A,B,zero, // y6
		A,N,zero, // y8
		A,U,zero, // y10
		A,C,zero, // yd1
		A,W,zero, // yd3
		A,Z,zero, // yd5
		A,Y,zero, // yd7
		A,X,zero, // yd9
		A,E,zero // yd11
	};

	struct kom2_zprae Y3w_ZPrAE = {
		zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero,
		zero,zero,zero, A,A,A, A,A,O, A,A,V, A,A,B, A,A,N, A,A,U, A,A,C, A,A,W, A,A,Z, A,A,Y, A,A,X, A,A,E, // Yy0xx
		zero,zero,zero, A,O,A, A,O,O, A,O,V, A,O,B, A,O,N, A,O,U, A,O,C, A,O,W, A,O,Z, A,O,Y, A,O,X, A,O,E, // Yy2xx
		zero,zero,zero, A,V,A, A,V,O, A,V,V, A,V,B, A,V,N, A,V,U, A,V,C, A,V,W, A,V,Z, A,V,Y, A,V,X, A,V,E, // Yy4xx
		zero,zero,zero, A,B,A, A,B,O, A,B,V, A,B,B, A,B,N, A,B,U, A,B,C, A,B,W, A,B,Z, A,B,Y, A,B,X, A,B,E, // Yy6xx
		zero,zero,zero, A,N,A, A,N,O, A,N,V, A,N,B, A,N,N, A,N,U, A,N,C, A,N,W, A,N,Z, A,N,Y, A,N,X, A,N,E, // Yy8xx
		zero,zero,zero, A,U,A, A,U,O, A,U,V, A,U,B, A,U,N, A,U,U, A,U,C, A,U,W, A,U,Z, A,U,Y, A,U,X, A,U,E, // Yy10xx
		zero,zero,zero, A,C,A, A,C,O, A,C,V, A,C,B, A,C,N, A,C,U, A,C,C, A,C,W, A,C,Z, A,C,Y, A,C,X, A,C,E, // Yd1xx
		zero,zero,zero, A,W,A, A,W,O, A,W,V, A,W,B, A,W,N, A,W,U, A,W,C, A,W,W, A,W,Z, A,W,Y, A,W,X, A,W,E, // Yd3xx
		zero,zero,zero, A,Z,A, A,Z,O, A,Z,V, A,Z,B, A,Z,N, A,Z,U, A,Z,C, A,Z,W, A,Z,Z, A,Z,Y, A,Z,X, A,Z,E, // Yd5xx
		zero,zero,zero, A,Y,A, A,Y,O, A,Y,V, A,Y,B, A,Y,N, A,Y,U, A,Y,C, A,Y,W, A,Y,Z, A,Y,Y, A,Y,X, A,Y,E, // Yd7xx
		zero,zero,zero, A,X,A, A,X,O, A,X,V, A,X,B, A,X,N, A,X,U, A,X,C, A,X,W, A,X,Z, A,X,Y, A,X,X, A,X,E, // Yd9xx
		zero,zero,zero, A,E,A, A,E,O, A,E,V, A,E,B, A,E,N, A,E,U, A,E,C, A,E,W, A,E,Z, A,E,Y, A,E,X, A,E,E // Yd11xx
	};

	struct kom1_zprae D2w_ZPrAE = {
		zero,zero,zero,
		A,C,zero, // dy1
		A,W,zero, // dy3
		A,F,zero, // dy5
		A,D,zero, // dy7
		A,X,zero, // dy9
		A,E,zero, // dy11
		A,A,zero, // dd0
		A,O,zero, // dd2
		A,V,zero, // dd4
		A,B,zero, // dd6
		A,N,zero, // dd8
		A,U,zero // dd10
	};

	struct kom1_zprae D3w_ZPrAE = {
			zero,zero,zero,
			A,C,C,
			A,W,W,
			A,F,F,
			A,D,D,
			A,X,X,
			A,E,E,
			A,A,A,
			A,O,O,
			A,V,V,
			A,B,B,
			A,N,N,
			A,U,U
	};

int diff_init_io(struct diff_io *io, struct diff_logic *log)
{
	if(set_bit_ptr_struct(io->stan_bl_in,&log->stan_bl))
		return -1;
	if(set_pointer_in_ptr(io->ia_l1_ptr_in,(u32 *)&log->I1_l1))
		return -1;
	if(set_pointer_in_ptr(io->ia_l1_params_ptr_in,(u32 *)&log->I1_l1_params))
		return -1;
	if(set_pointer_in_ptr(io->ia_l2_ptr_in,(u32 *)&log->I1_l2))
		return -1;
	if(set_pointer_in_ptr(io->ia_l2_params_ptr_in,(u32 *)&log->I1_l2_params))
		return -1;
	if(set_pointer_in_ptr(io->ia_l3_ptr_in,(u32 *)&log->I1_l3))
		return -1;
	if(set_pointer_in_ptr(io->ia_l3_params_ptr_in,(u32 *)&log->I1_l3_params))
		return -1;

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

	if(set_pointer_in_ptr(io->ic_l1_ptr_in,(u32 *)&log->I3_l1))
		return -1;
	if(set_pointer_in_ptr(io->ic_l1_params_ptr_in,(u32 *)&log->I3_l1_params))
		return -1;
	if(set_pointer_in_ptr(io->ic_l2_ptr_in,(u32 *)&log->I3_l2))
		return -1;
	if(set_pointer_in_ptr(io->ic_l2_params_ptr_in,(u32 *)&log->I3_l2_params))
		return -1;
	if(set_pointer_in_ptr(io->ic_l3_ptr_in,(u32 *)&log->I3_l3))
		return -1;
	if(set_pointer_in_ptr(io->ic_l3_params_ptr_in,(u32 *)&log->I3_l3_params))
		return -1;

	if(set_pointer_in_ptr(io->id_l1_ptr_in,(u32 *)&log->I4_l1))
		return -1;
	if(set_pointer_in_ptr(io->id_l1_params_ptr_in,(u32 *)&log->I4_l1_params))
		return -1;
	if(set_pointer_in_ptr(io->id_l2_ptr_in,(u32 *)&log->I4_l2))
		return -1;
	if(set_pointer_in_ptr(io->id_l2_params_ptr_in,(u32 *)&log->I4_l2_params))
		return -1;
	if(set_pointer_in_ptr(io->id_l3_ptr_in,(u32 *)&log->I4_l3))
		return -1;
	if(set_pointer_in_ptr(io->id_l3_params_ptr_in,(u32 *)&log->I4_l3_params))
		return -1;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

/*	if(set_float_ptr(io->ih_l1_ch_float_out,&log->Ih_L1_ortb))
		return -1;
	if(set_float_ptr(io->ih_l2_ch_float_out,&log->Ih_L2_ortb))
		return -1;
	if(set_float_ptr(io->ih_l3_ch_float_out,&log->Ih_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->an_struct_i4_l1.samples_buf=log->I4_l1;
	log->an_struct_i4_l1.params=log->I4_l1_params;
	log->an_struct_i4_l2.samples_buf=log->I4_l2;
	log->an_struct_i4_l2.params=log->I4_l2_params;
	log->an_struct_i4_l3.samples_buf=log->I4_l3;
	log->an_struct_i4_l3.params=log->I4_l3_params;


	return 0;
}

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

void ustaw_kompensacje(struct diff_logic *log, enum typ_kompensacji typ, u16 nr_pradu)
{
	struct we_an_diff *a0,*a1,*a2;
/*
 	u16 *temp;
  	temp = ((uint16_t *)&nast_.adr.I1_L1 + 3 * nr_pradu);
	const WA::Wyjscie* a0 = (const WA::Wyjscie*)wskazniki[*temp];U
	if (*temp == 0) a0 = &nast_.wejscie_zero;
	const WA::Wyjscie* a1 = (const WA::Wyjscie*)wskazniki[*(temp + 1)];
	if (*(temp + 1) == 0) a1 = &nast_.wejscie_zero;
	const WA::Wyjscie* a2 = (const WA::Wyjscie*)wskazniki[*(temp + 2)];
	if (*(temp + 2) == 0) a2 = &nast_.wejscie_zero;
*/
	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:
			if((u32)log->I4_l1==(u32)&log_manager.nets_data[0])
				a0=&log->nast_.wejscie_zero;
			else
				a0 = &log->an_struct_i4_l1;


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

			if((u32)log->I4_l3==(u32)&log_manager.nets_data[0])
				a2=&log->nast_.wejscie_zero;
			else
				a2 = &log->an_struct_i4_l3;
			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_nastawy1(struct diff_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(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
			;
		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] *= 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(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] *= 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(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] *= 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));
}

/**
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 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 kryterium2(struct diff_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(struct diff_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(struct diff_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(struct diff_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(struct diff_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(struct diff_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;
}


/**
Sprawdza kryterium dzialania przekaznika
Wywolywana powinna byc po kazdej nowej probce z czestotliwoscia 1200Hz
Ustawia zmienne zawarte w structurze "stan_""

*/

void przelicz_1200Hz_1(struct diff_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
		{

			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 = log->P_.Irr2 = log->P_.Irr3 = 0;

	}

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

	//filtracja przebiegow


	// 1h pradu rozruchowego


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

	przelicz_1h_ip(log,3);
	przelicz_1h_ip(log,4);
	przelicz_1h_ip(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(log,0);
		przelicz_2h(log,1);
		przelicz_2h(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(log,0);
		przelicz_5h(log,1);
		przelicz_5h(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;
}


/**
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 przelicz_1200Hz_2(struct diff_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);
	}
	//

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

#ifdef IZAZ600
	int nr_pr;
	if (nast_.Rej3kHz_adr_wej_OK) {
		for (int i = 0; i < 3; i++)
		{
			int j = 2 - i;
			nr_pr = w.nr_probki;
			nr_pr = nr_pr - j;
			if (nr_pr < 0) nr_pr += 120;

			dane_analogowe_3[nast_.adr.Ir_L1_ortb][i] = w.buf_Lr[nr_pr][0];
			dane_analogowe_3[nast_.adr.Ir_L2_ortb][i] = w.buf_Lr[nr_pr][1];
			dane_analogowe_3[nast_.adr.Ir_L3_ortb][i] = w.buf_Lr[nr_pr][2];
			dane_analogowe_3[nast_.adr.Ih_L1_ortb][i] = w.buf_Lr[nr_pr][3];
			dane_analogowe_3[nast_.adr.Ih_L2_ortb][i] = w.buf_Lr[nr_pr][4];
			dane_analogowe_3[nast_.adr.Ih_L3_ortb][i] = w.buf_Lr[nr_pr][5];
		}
	}
#endif

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



/**
Funkcja drugiego poziomu.
Funkcja wywolywana z czestotliwoscia drugiego poziomu (w iZAZ400...800 to 100Hz)
Wszystkie funkcje tego typu (z calej konfiguracji) powinny sie bezwzglednie wykonac w kadym cyklu.
Funkcje powinny byc wykonywane z priorytetem srednim
(przerywane tylko obsluga przetwornika AC i funkcja przelicz_1())
Przekroczenie 60% czasu petli powinno generowac zdarzenie systemowe typu WARRNING,
a kilkukrotne przekroczenie 100% czasu petli powinno przelaczyc zabezpieczenie w stan ERROR..
*/
void przelicz_2(struct diff_logic *log)
{
if(log->nast_.on)   // jesli zabezpieczenie aktywne
{


	// realizacja rejestratora parametrow ostatniego zaklocenia

	if (log->nast_.w)
		switch(status_rej)
		{
			case oczekiwanie:
				if (check_struct(&log->P))
				{
					log->dw.maxIr1 = log->Ir1_;
					log->dw.maxIr2 = log->Ir2_;
					log->dw.maxIr3 = log->Ir3_;
					log->dw.maxIh1 = log->Ih1_;
					log->dw.maxIh2 = log->Ih2_;
					log->dw.maxIh3 = log->Ih3_;
					log->dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia
					status_rej = sledzenie;
					if (check_struct(&log->Z))
					{
						status_rej = sledzenie_zad;
					}
				}
				break;
			case sledzenie:
				if (check_struct(&log->Z))
				{
					status_rej = sledzenie_zad;
					//log->dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji
				}
				if (check_struct(&log->P))
				{
					if(log->dw.maxIr1 < log->Ir1_)
					{
						log->dw.maxIr1 = log->Ir1_;
						log->dw.maxIh1 = log->Ih1_;
					}
					if(log->dw.maxIr2 < log->Ir2_)
					{
						log->dw.maxIr2 = log->Ir2_;
						log->dw.maxIh2 = log->Ih2_;
					}
					if(log->dw.maxIr3 < log->Ir3_)
					{
						log->dw.maxIr3 = log->Ir3_;
						log->dw.maxIh3 = log->Ih3_;
					}
					log->dw.dl_++;
				} else {
					status_rej = oczekiwanie;
				}
				break;
			case sledzenie_zad:
				if (!check_struct(&log->P))
				{
					log->dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem
					status_rej = zapis;
				} else {
					log->dw.dl_++;
					if(log->dw.maxIr1 < log->Ir1_)
					{
						log->dw.maxIr1 = log->Ir1_;
						log->dw.maxIh1 = log->Ih1_;
					}
					if(log->dw.maxIr2 < log->Ir2_)
					{
						log->dw.maxIr2 = log->Ir2_;
						log->dw.maxIh2 = log->Ih2_;
					}
					if(log->dw.maxIr3 < log->Ir3_)
					{
						log->dw.maxIr3 = log->Ir3_;
						log->dw.maxIh3 = log->Ih3_;
					}
				}
				break;
			case zapis:
				if (!log->dw.wsk_rej)
				{
					status_rej = oczekiwanie;
				}
				break;
		}
}
}

/**
Funkcja piatego poziomu
Funkcja wywolywana bez priorytetu w czasie wolnym procesora ale nie rzadziej ni 1 s.
*/
void przelicz_5(struct diff_logic *log)
{
	if(log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo
	{
	//	log->dw.R.max_L1_ = log->dw.maxIr1; // wyliczenie maksymalnej wartosci  faza 1
	//	log->dw.R.Ih1 = log->dw.maxIh1;
	//	log->dw.R.max_L2_ = log->dw.maxIr2; // wyliczenie maksymalnej wartosci  faza 2
	//	log->dw.R.Ih2 = log->dw.maxIh2;
	//	log->dw.R.max_L3_ = log->dw.maxIr3; // wyliczenie maksymalnej wartosci  faza 3
		//log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia
	//	log->dw.R.Ih3 = log->dw.maxIh3;
		//log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia
	//	log->dw.R.dlugosc = (float)log->dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia
	 //   log->dw.R.czy_nowa = 1; // sygnalizacja powstania nowej rejestracji
		log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia
	}
}


