/*
 * ZDistS.c
 *
 *  Created on: 16-03-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "ZDistS.h"
#include "ZDistA.h"

struct ZDistS_nast
{
	long Stf1; ///< Sposób działania strefy 1
	long Stf2; ///< Sposób działania strefy 2
	long Stf3; ///< Sposób działania strefy 3
	long Stf4; ///< Sposób działania strefy 4
	long Stf5; ///< Sposób działania strefy 5
};

int ZDistS_initlog(void *arguments, void *logic)
{
	struct ZDistS_args *args = (struct ZDistS_args *)arguments;
	struct ZDistS_logic *log = (struct ZDistS_logic *)logic;
	struct ZDistS_nast snast;

	if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl))
		return -1;
	if(set_pointer_in_ptr(args->io.we_zdist_ptr_in,(u32 *)&log->WE_Zdist))
		return -1;
	if(set_bit_ptr_struct(args->io.bl_ps_in,&log->Bl_PS))
		return -1;
	if(set_bit_ptr_struct(args->io.bl_ufail_in,&log->Bl_Ufail))
		return -1;
	if(set_bit_ptr_struct(args->io.z_in,&log->Z))
		return -1;
	if(set_bit_ptr_struct(args->io.w_on_in,&log->W_ON))
		return -1;
	if(set_bit_ptr_struct(args->io.w_off_in,&log->W_OFF))
		return -1;
	if(set_bit_ptr_struct(args->io.test_in,&log->test))
		return -1;

	if(set_bit_ptr_struct(args->io.p_out,&log->P))
		return -1;
	if(set_bit_ptr_struct(args->io.p1_out,&log->P1))
		return -1;
	if(set_bit_ptr_struct(args->io.p2_out,&log->P2))
		return -1;
	if(set_bit_ptr_struct(args->io.p3_out,&log->P3))
		return -1;
	if(set_bit_ptr_struct(args->io.p4_out,&log->P4))
		return -1;
	if(set_bit_ptr_struct(args->io.p5_out,&log->P5))
		return -1;
	if(set_bit_ptr_struct(args->io.r_out,&log->R))
		return -1;
	if(set_bit_ptr_struct(args->io.s_out,&log->S))
		return -1;
	if(set_bit_ptr_struct(args->io.t_out,&log->T))
		return -1;
	if(set_bit_ptr_struct(args->io.e_out,&log->E))
		return -1;
	if(set_bit_ptr_struct(args->io.z1_out,&log->Z1))
		return -1;
	if(set_bit_ptr_struct(args->io.z2_out,&log->Z2))
		return -1;
	if(set_bit_ptr_struct(args->io.z3_out,&log->Z3))
		return -1;
	if(set_bit_ptr_struct(args->io.z4_out,&log->Z4))
		return -1;
	if(set_bit_ptr_struct(args->io.z5_out,&log->Z5))
		return -1;
	if(set_bit_ptr_struct(args->io.w_out,&log->W))
		return -1;

	log->nast_.Bl_PS[0] = (args->params.bity & 0x0001)?1:0;
	log->nast_.Bl_PS[1] = (args->params.bity & 0x0002)?1:0;
	log->nast_.Bl_PS[2] = (args->params.bity & 0x0004)?1:0;
	log->nast_.Bl_PS[3] = (args->params.bity & 0x0008)?1:0;
	log->nast_.Bl_PS[4] = (args->params.bity & 0x0010)?1:0;
	log->nast_.ZZw_ON = (args->params.bity & 0x0020)?1:0;
	log->nast_.bl_ = (args->params.bity & 0x0040)?1:0;

	log->nast_.ZdistA = log->WE_Zdist;

	uint16_t i;
	int16_t temp_ti;
	float temp_tf;

	for (i=0;i<10;i++)
	{
		switch(i)
		{
		case 0:
			 temp_tf = args->params.ts1LE;
			 break;
		case 1:
			 temp_tf = args->params.ts1LL;
			 break;
		case 2:
			 temp_tf = args->params.ts2LE;
			 break;
		case 3:
			 temp_tf = args->params.ts2LL;
			 break;
		case 4:
			 temp_tf = args->params.ts3LE;
			 break;
		case 5:
			 temp_tf = args->params.ts3LL;
			 break;
		case 6:
			 temp_tf = args->params.ts4LE;
			 break;
		case 7:
			 temp_tf = args->params.ts4LL;
			 break;
		case 8:
			 temp_tf = args->params.ts5LE;
			 break;
		case 9:
			 temp_tf = args->params.ts6LL;
			 break;
		default:
			 temp_tf = args->params.ts1LE;
			 break;

		}

		//przeliczenie czasow stref
		temp_ti = (100 * (temp_tf - 0.005f));

		if (temp_ti)
		{
			log->nast_.Zbz[i/2][i%2] = 0;
			log->nast_.t_stf[i/2][i%2] = temp_ti;
		} else {
			log->nast_.Zbz[i/2][i%2] = 1;
		}
		//--------------------------
	}
	//-------------------------

	log->nast_.ZZw_st = args->params.ZZw_st + 1;

	switch(log->nast_.ZZw_st)
	{
		case 0:
			log->PZZw=log->P1;
			log->ZZZw=log->Z1;
			break;
		case 1:
			log->PZZw=log->P2;
			log->ZZZw=log->Z2;
			break;
		case 2:
			log->PZZw=log->P3;
			log->ZZZw=log->Z3;
			break;
		case 3:
			log->PZZw=log->P4;
			log->ZZZw=log->Z4;
			break;
		case 4:
			log->PZZw=log->P5;
			log->ZZZw=log->Z5;
			break;
		default:
			log->PZZw=log->P1;
			log->ZZZw=log->Z1;
			break;
	}



	snast.Stf1=args->params.Stf1;
	snast.Stf2=args->params.Stf2;
	snast.Stf3=args->params.Stf3;
	snast.Stf4=args->params.Stf4;
	snast.Stf5=args->params.Stf5;

	long *temp_e = &snast.Stf1;

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

		//ustawienie bitow sposobu dzialania stref
		switch (*(temp_e))
		{
			case 0:
				log->nast_.Stf_ON[i] = 0;
				log->nast_.Stf_W[i] = 0;
				break;
			case 1:
				log->nast_.Stf_ON[i] = 1;
				log->nast_.Stf_W[i] = 0;
				break;
			case 2:
				log->nast_.Stf_ON[i] = 1;
				log->nast_.Stf_W[i] = 1;
				break;
		}
		temp_e++;
		//--------------------------
	}
	//-------------------------

	log->nast_.ZZw_ta = (int16_t)(100 * (args->params.ZZw_ta + 0.005f));
	log->nast_.ZZw_to = (int16_t)(100 * (args->params.ZZw_to + 0.005f));

	log->nast_.ZZw_Ir = args->params.ZZw_Ir * args->params.ZZw_Ir;
	log->nast_.ZZw_Ur = args->params.ZZw_Ur * args->params.ZZw_Ur;

	return 0;
}

/**
Funkcja okraślania petli zwarciwej przy wyłaczeniu
\param nr_strefy - numer strefy wyłaczającej
*/
void wybiornik_f_ZDistS(struct ZDistS_logic *log,unsigned short nr_strefy)
{
	log->dw.R = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][0]
		|| log->nast_.ZdistA->Zdist_dw->P[nr_strefy][3]
		|| log->nast_.ZdistA->Zdist_dw->P[nr_strefy][5];

	log->dw.S = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][1]
		|| log->nast_.ZdistA->Zdist_dw->P[nr_strefy][3]
		|| log->nast_.ZdistA->Zdist_dw->P[nr_strefy][4];

	log->dw.T = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][2]
		|| log->nast_.ZdistA->Zdist_dw->P[nr_strefy][4]
		|| log->nast_.ZdistA->Zdist_dw->P[nr_strefy][5];

	log->dw.E = log->nast_.ZdistA->Zdist_dw->Iogr;

}

/**
Funkcja okraślania petli zwarciwej przy wyłaczeniu - działanie bezkierunkowe
\param nr_strefy - numer strefy wyłaczającej
*/
void wybiornik_fbk_ZDistS(struct ZDistS_logic *log, unsigned short nr_strefy)
{
	log->dw.Rbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][0]
		|| log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][3]
		|| log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][5];

	log->dw.Sbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][1]
		|| log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][3]
		|| log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][4];

	log->dw.Tbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][2]
		|| log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][4]
		|| log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][5];

	log->dw.E = log->nast_.ZdistA->Zdist_dw->Iogr;
}



void ZDistS(void *arguments, void *logic)
{
	struct ZDistS_logic *log = (struct ZDistS_logic *)logic;

	uint16_t i;
	log->dw.P = 0;
	log->dw.Zz = 0;

	u8 temp_Bl,temp_Zezwolenie;

	if (*log->nast_.ZdistA->on)
	{
		temp_Bl = check_struct(&log->Bl_Ufail)
			|| (check_struct(&log->stan_bl) && log->nast_.bl_);// | czy_start();


			for (i=0;i<5;i++)
			{
				if (log->nast_.Stf_ON[i]) //jesli strefa wlaczona
				{
					temp_Zezwolenie = !(temp_Bl  || (log->nast_.Bl_PS[i] && check_struct(&log->Bl_PS)));


						//obsluga wejscia testu przekaznika
					if (czy_test_R())
					{
						log->dw.Pf[i][0] = log->dw.Pf[i][1] = check_struct(&log->test) && temp_Zezwolenie;
					}
					else
					{

						//ustawienie pobudzen
						log->dw.Pf[i][0] = (log->nast_.ZdistA->Zdist_dw->P[i+1][0] ||
													log->nast_.ZdistA->Zdist_dw->P[i+1][1] ||
																	log->nast_.ZdistA->Zdist_dw->P[i+1][2])
																		&& temp_Zezwolenie;
						log->dw.Pf[i][1] = (log->nast_.ZdistA->Zdist_dw->P[i+1][3] ||
													log->nast_.ZdistA->Zdist_dw->P[i+1][4] ||
																	log->nast_.ZdistA->Zdist_dw->P[i+1][5])
																		&& temp_Zezwolenie;
					}
					log->dw.Pzf[i] = log->dw.Pf[i][0] || log->dw.Pf[i][1];

					//ustawienie zadzialan jesli bezzwloczny
					if (log->nast_.Zbz[i][0])
					{
						log->dw.Zf[i][0] = log->dw.Pf[i][0];
					}
					if (log->nast_.Zbz[i][1])
					{
						log->dw.Zf[i][1] = log->dw.Pf[i][1];
					}
					//ustawienie zadzialania strefy
					log->dw.Z[i] = log->dw.Zf[i][0] || log->dw.Zf[i][1];
					log->dw.Ws[i] = log->dw.Z[i] && log->nast_.Stf_W[i];


				} else {
					log->dw.Pzf[i] = 0;
					log->dw.Z[i] = 0;
				}

				log->dw.P |= log->dw.Pzf[i];
				log->dw.Zz |= log->dw.Z[i]; //zadzialanie zabezpieczenia
				if(!log->dw.W)
				{
					log->dw.W |= log->dw.Ws[i];
					if(log->dw.W)
					{
						wybiornik_f_ZDistS(log,i+1);
					}
				}
			}

			//-------------------------------------------------------------------------------------------------------
			//automatyka załaczenia na zwarcie




			if (check_struct(&log->Z)) {
				log->dw.ZZw |= 1;
			}

			if (log->dw.ZZw && log->nast_.ZZw_ON) //jeseli jest właczony i jest spełnione kryterium załączenia na zwarcie
			{
				wybiornik_fbk_ZDistS(log,log->nast_.ZZw_st);

				if (log->dw.Rbk || log->dw.Sbk || log->dw.Tbk)
				{
					log->dw.W |= 1;
					log->dw.ZZwP = 1;
				} else log->dw.ZZwP = 0;


			} else {
				log->dw.Rbk = log->dw.Sbk = log->dw.Tbk = 0;
				log->dw.ZZwP = 0;
			}


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




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

			//wyprowadzenie wyjść
			check_and_set_struct(log->dw.P,&log->P);
			check_and_set_struct(log->dw.Pzf[0],&log->P1);
			check_and_set_struct(log->dw.Pzf[1],&log->P2);
			check_and_set_struct(log->dw.Pzf[2],&log->P3);
			check_and_set_struct(log->dw.Pzf[3],&log->P4);
			check_and_set_struct(log->dw.Pzf[4],&log->P5);

			check_and_set_struct(log->dw.Z[0],&log->Z1);
			check_and_set_struct(log->dw.Z[1],&log->Z2);
			check_and_set_struct(log->dw.Z[2],&log->Z3);
			check_and_set_struct(log->dw.Z[3],&log->Z4);
			check_and_set_struct(log->dw.Z[4],&log->Z5);

			check_and_set_struct(log->dw.R || log->dw.Rbk,&log->R);
			check_and_set_struct(log->dw.S || log->dw.Sbk,&log->S);
			check_and_set_struct(log->dw.T || log->dw.Tbk,&log->T);
			check_and_set_struct(log->dw.E,&log->E);

			check_and_set_struct(log->dw.W,&log->W);

			if(log->dw.ZZwP)
			{
				set_struct(&log->PZZw);
				set_struct(&log->ZZZw);
			}
		} else {
			clear_struct(&log->P);
			clear_struct(&log->P1);
			clear_struct(&log->P2);
			clear_struct(&log->P3);
			clear_struct(&log->P4);
			clear_struct(&log->P5);

			clear_struct(&log->Z1);
			clear_struct(&log->Z2);
			clear_struct(&log->Z3);
			clear_struct(&log->Z4);
			clear_struct(&log->Z5);

			clear_struct(&log->R);
			clear_struct(&log->S);
			clear_struct(&log->T);
			clear_struct(&log->E);
			clear_struct(&log->W);
		}
}

void ZDistS_100hz(void *arguments, void *logic)
{
	struct ZDistS_logic *log = (struct ZDistS_logic *)logic;
	unsigned short i;
	u8 tempbool = 0;


		if (*log->nast_.ZdistA->on)
		{

			//wczytanie danych o stanie wyłącznika
			log->dw.W_ON = check_struct(&log->W_ON) && !check_struct(&log->W_OFF);
			log->dw.W_OFF = !check_struct(&log->W_ON) && check_struct(&log->W_OFF);


			for (i=0;i<5;i++)
			{
				//realizacja przekaznikow czasowych
				if (log->nast_.Stf_ON[i]) //jesli strefa wlaczona
				{

					if (log->dw.Pf[i][0])
					{
						if(!log->nast_.Zbz[i][0]) // jesli nie bezzwloczny
						{
							if (!log->dw.liczts[i][0])
							{
								log->dw.Zf[i][0] = 1;
								log->dw.liczts[i][0]++;
							}
							log->dw.liczts[i][0]--;

						}

					}
					else//wylaczenie zadzialania (odpad)
					{
						log->dw.liczts[i][0] = log->nast_.t_stf[i][0];
						log->dw.Zf[i][0] = 0;
					}
					if (log->dw.Pf[i][1])
					{
						if(!log->nast_.Zbz[i][1]) // jesli nie bezzwloczny
						{
							if (!log->dw.liczts[i][1])
							{
								log->dw.Zf[i][1] = 1;
								log->dw.liczts[i][1]++;
							}
							log->dw.liczts[i][1]--;
						}
					}
					else//wylaczenie zadzialania (odpad)
					{
						log->dw.liczts[i][1] = log->nast_.t_stf[i][1];
						log->dw.Zf[i][1] = 0;
					}

				}
				//-----------------------------------------------------------
			}

			//odpad wyłaczeń



			for(i=0;i<5;i++)
			{
				tempbool |= log->dw.Ws[i];
			}
			if (!tempbool) log->dw.W = log->dw.R = log->dw.S = log->dw.T = log->dw.E = 0;




			//automatyka załaczenia na zwarcie - identyfikacja stanu załączenia na zwarcie

			//właczenie od otwarcia wyłącznika

			tempbool = 	(log->nast_.ZdistA->Zdist_dw->I1<log->nast_.ZZw_Ir) &&
				(log->nast_.ZdistA->Zdist_dw->I2<log->nast_.ZZw_Ir) &&
				(log->nast_.ZdistA->Zdist_dw->I3<log->nast_.ZZw_Ir) &&
				(log->nast_.ZdistA->Zdist_dw->U1<log->nast_.ZZw_Ur) &&
				(log->nast_.ZdistA->Zdist_dw->U2<log->nast_.ZZw_Ur) &&
				(log->nast_.ZdistA->Zdist_dw->U3<log->nast_.ZZw_Ur);

			sprawdz_P(&log->dw.ZZw_PIU,tempbool,!tempbool,&log->dw.licznik_PIU,log->nast_.ZZw_to,2);

			log->dw.ZZw_start = log->dw.ZZw_PIU || //włączenie ZZw jeśli brak napięcia i prądu
							check_struct(&log->Z) || //włączenie ZZw jeśli aktywny sygnał zamknięcia wyłącznika
								(log->dw.W_OFF); //włączenie ZZw jeśli otwarte styki i nie ma cyklu SPZ

			log->dw.ZZw |= log->dw.ZZw_start;
			if(log->dw.ZZw_start)
			{
				log->dw.licznik_ZZwa = 0;
			} else {
				log->dw.licznik_ZZwa++;
				if(log->dw.licznik_ZZwa>log->nast_.ZZw_ta)
				{
					if (!log->dw.ZZwP) log->dw.ZZw = 0;
					log->dw.licznik_ZZwa--;
				}
			}


			///

				// realizacja rejestratora parametrow ostatniego zaklocenia
					switch(status_rej)
					{
						case oczekiwanie:
							if (log->dw.P)
							{
								log->dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia
								status_rej = sledzenie;
							}
							break;
						case sledzenie:
							if (log->dw.W)
							{
								status_rej = sledzenie_zad;
								if (log->dw.Ws[0] | log->dw.Ws[1])
								{
									log->dw.RL1Em = log->nast_.ZdistA->Zdist_dw->Z.R[0];
									log->dw.RL2Em = log->nast_.ZdistA->Zdist_dw->Z.R[1];
									log->dw.RL3Em = log->nast_.ZdistA->Zdist_dw->Z.R[2];
									log->dw.XL1Em = log->nast_.ZdistA->Zdist_dw->Z.X[0];
									log->dw.XL2Em = log->nast_.ZdistA->Zdist_dw->Z.X[1];
									log->dw.XL3Em = log->nast_.ZdistA->Zdist_dw->Z.X[2];
								} else {
									log->dw.RL1Em = log->nast_.ZdistA->Zdist_dw->Z.R[3];
									log->dw.RL2Em = log->nast_.ZdistA->Zdist_dw->Z.R[4];
									log->dw.RL3Em = log->nast_.ZdistA->Zdist_dw->Z.R[5];
									log->dw.XL1Em = log->nast_.ZdistA->Zdist_dw->Z.X[3];
									log->dw.XL2Em = log->nast_.ZdistA->Zdist_dw->Z.X[4];
									log->dw.XL3Em = log->nast_.ZdistA->Zdist_dw->Z.X[5];
								}
								log->dw.RL1L2m = log->nast_.ZdistA->Zdist_dw->Z.R[6];
								log->dw.RL2L3m = log->nast_.ZdistA->Zdist_dw->Z.R[7];
								log->dw.RL3L1m = log->nast_.ZdistA->Zdist_dw->Z.R[8];
								log->dw.XL1L2m = log->nast_.ZdistA->Zdist_dw->Z.X[6];
								log->dw.XL2L3m = log->nast_.ZdistA->Zdist_dw->Z.X[7];
								log->dw.XL3L1m = log->nast_.ZdistA->Zdist_dw->Z.X[8];

								//log->dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji
							}
							if (log->dw.P)
							{
								log->dw.dl_++;
							} else {
								status_rej = oczekiwanie;
							}
							break;
						case sledzenie_zad:
							if (!log->dw.P)
							{
								log->dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem
								status_rej = zapis;
							} else {
								log->dw.dl_++;
							}
							break;
						case zapis:
							if (!log->dw.wsk_rej)
							{
								status_rej = oczekiwanie;
							}
							break;
					}

		} else {

		}

}
