/*
 * u_fail.c
 *
 *  Created on: 29-07-2016
 *      Author: Krzysztof Jakubczyk
 */

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

#include "u_fail.h"
#include "helper.h"
#include <math.h>

int u_fail_initlog(void *arguments, void *logic)
{
	struct u_fail_args *args = (struct u_fail_args *)arguments;
	struct u_fail_logic *log = (struct u_fail_logic *)logic;

	log->stan_bl = (u8 *)(log_manager.nets_data + (args->io.stan_bl_in >> 3));
	if((u8 *)log->stan_bl > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->stan_bl_bit = (args->io.stan_bl_in & 0x07);

	log->I1_orta = (float *)(log_manager.nets_data + (args->io.I1_orta_float_in >> 3));
	if((u8 *)log->I1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->I1_ortb = (float *)(log_manager.nets_data + (args->io.I1_ortb_float_in >> 3));
	if((u8 *)log->I1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->I1 = (float *)(log_manager.nets_data + (args->io.I1_float_in >> 3));
	if((u8 *)log->I1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->I2 = (float *)(log_manager.nets_data + (args->io.I2_float_in >> 3));
	if((u8 *)log->I2 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->I0 = (float *)(log_manager.nets_data + (args->io.I0_float_in >> 3));
	if((u8 *)log->I0 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->U1 = (float *)(log_manager.nets_data + (args->io.U1_float_in >> 3));
	if((u8 *)log->U1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->U2 = (float *)(log_manager.nets_data + (args->io.U2_float_in >> 3));
	if((u8 *)log->U2 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->U0 = (float *)(log_manager.nets_data + (args->io.U0_float_in >> 3));
	if((u8 *)log->U0 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->NHS = (u8 *)(log_manager.nets_data + (args->io.NHS_in >> 3));
	if((u8 *)log->NHS > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->NHS_bit = (args->io.NHS_in & 0x07);

	log->W_ON = (u8 *)(log_manager.nets_data + (args->io.W_ON_in >> 3));
	if((u8 *)log->W_ON > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->W_ON_bit = (args->io.W_ON_in & 0x07);

	log->test = (u8 *)(log_manager.nets_data + (args->io.test_in >> 3));
	if((u8 *)log->test > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->test_bit = (args->io.test_in & 0x07);

	log->Bl_Ufail = (u8 *)(log_manager.nets_data + (args->io.Bl_Ufail_out >> 3));
	if((u8 *)log->Bl_Ufail > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->Bl_Ufail_bit = (args->io.Bl_Ufail_out & 0x07);

	log->Syg_Ufail = (u8 *)(log_manager.nets_data + (args->io.Syg_Ufail_out >> 3));
	if((u8 *)log->Syg_Ufail > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->Syg_Ufail_bit = (args->io.Syg_Ufail_out & 0x07);

	log->nast_.on = args->params.bity & 0x0001;
	log->nast_.IU0 = args->params.bity & 0x002;
	log->nast_.IU1 = args->params.bity & 0x004;
	log->nast_.IU2 = args->params.bity & 0x008;
	log->nast_.bl = args->params.bity & 0x0010;

	log->nast_.Iomin = args->params.Iomin * args->params.Iomin;
	log->nast_.Iokh = args->params.Iokh * args->params.Iokh;
	log->nast_.Uomin = args->params.Uomin * args->params.Uomin;
	log->nast_.I2min = args->params.I2min * args->params.I2min;
	log->nast_.I2kh = args->params.I2kh * args->params.I2kh;
	log->nast_.U2min = args->params.U2min * args->params.U2min;

	log->nast_.U0_dbl = args->params.U0_dbl * args->params.U0_dbl;
	log->nast_.U1_dbl = args->params.U1_dbl * args->params.U1_dbl;
	log->nast_.U2_dbl = args->params.U2_dbl * args->params.U2_dbl;
	log->nast_.DU1 = args->params.DU1;
	log->nast_.DI1k = 2 * args->params.DI1 * args->params.DI1;
	log->nast_.DI1 = args->params.DI1;
	log->nast_.I1max = args->params.I1max * args->params.I1max;

	log->nast_.tz = (uint16_t)(100 * (args->params.tbl-0.005f));

	return 0;
}

void u_fail(void *arguments, void *logic)
{
	struct u_fail_logic *log = (struct u_fail_logic *)logic;

	u8 local;

    if(log->nast_.on)   // jesli zabezpieczenie aktywne
    {
		log->w.I0 = *log->I0;
		log->w.I1 = *log->I1;
		log->w.I2 = *log->I2;
		log->w.U0 = *log->U0;
		log->w.U1 = *log->U1;
		log->w.U2 = *log->U2;

		//-------------------------------------------
		//przekaznik skladowych zerowych
		if(log->nast_.IU0) //jeeli kryterium aktywne
		{
			if (log->dw.Syg_Ufail)
			{
				local = !log->dw.U_OK && log->dw.Ufail_0;
			} else {
				local = (log->w.U0 > log->nast_.Uomin) && !((log->w.I0 > log->nast_.Iomin) && (log->w.I0 > (log->nast_.Iokh * log->w.I1))) && (*log->W_ON & (1<<log->W_ON_bit));
			}

			sprawdz_P(
				&log->dw.Ufail_0,
				local,
				!local,
				&log->dw.licz0,
				3,3
				);
		}

		//-------------------------------------------
		//przekaznik skladowych przeciwnych
		if(log->nast_.IU2) //jeeli kryterium aktywne
		{
			if (log->dw.Syg_Ufail)
			{
				local = !log->dw.U_OK && log->dw.Ufail_2;
			} else {
				local = (log->w.U2 > log->nast_.U2min) && !((log->w.I2 > log->nast_.I2min) && (log->w.I2 > (log->nast_.I2kh * log->w.I1))) && (*log->W_ON & (1<<log->W_ON_bit));
			}
			sprawdz_P(
				&log->dw.Ufail_2,
				local,
				!local,
				&log->dw.licz2,
				3,3
				);
		}


		//-------------------------------------------
		//przekaznik skladowych zgodnych
		if(log->nast_.IU1) //jeeli kryterium aktywne
		{


			log->dw.I1p = sqrtf(log->w.I1);
			local = (fabs(log->dw.I1_old[2] - log->dw.I1p) > log->nast_.DI1) || (log->w.I1 > log->nast_.I1max);
			sprawdz_P(
				&log->dw.UI1_Iw,
				local,
				!local,
				&log->dw.liczI1,
				3,48
				);

			log->dw.U1p = sqrtf(log->w.U1);
			local = (log->dw.U1_old - log->dw.U1p) > log->nast_.DU1;
			sprawdz_P(
				&log->dw.UI1_dUw,
				local,
				!local,
				&log->dw.liczdU1,
				3,3
				);


			local = (log->w.U1 > log->nast_.U1_dbl) || (log->w.U0 > log->nast_.U0_dbl) || (log->w.U2 > log->nast_.U2_dbl);
			sprawdz_P(
				&log->dw.UI1_Uw,
				local,
				!local,
				&log->dw.liczU1,
				3,3
				);


			if (log->dw.Syg_Ufail)
			{
				log->dw.Ufail_1 &= !log->dw.U_OK;
			} else {
				log->dw.Ufail_1 |= log->dw.UI1_dUw;
				log->dw.Ufail_1 &= !(log->dw.UI1_Iw || log->dw.UI1_Uw || log->dw.UI1_Iwk || !(*log->W_ON & (1<<log->W_ON_bit)));
			}
		}


		if(log->dw.Ufail_0 || log->dw.Ufail_1 || log->dw.Ufail_2 || (*log->NHS & (1<<log->NHS_bit)))
			*log->Bl_Ufail|=(1<<log->Bl_Ufail_bit);
		else
			*log->Bl_Ufail&=~(1<<log->Bl_Ufail_bit);

		//obsluga wejscia testu przekaznika
			if (czy_test_R())
			{
				check_and_set(*log->test & (1<<log->test_bit),log->Bl_Ufail,log->Bl_Ufail_bit);
			}
	        //


        // realizacja blokady dzialania przekaznika
			if(log->nast_.bl && (*log->stan_bl & (1<<log->stan_bl_bit))) {
				*log->Bl_Ufail&=~(1<<log->Bl_Ufail_bit);
				log->dw.Syg_Ufail = 0;
				log->dw.Ufail_0 = log->dw.Ufail_1 = log->dw.Ufail_2 = 0;
			}
        //


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


	} else {
		*log->Bl_Ufail&=~(1<<log->Bl_Ufail_bit);
	}

}

void u_fail_100hz(void *arguments, void *logic)
{
	struct u_fail_logic *log = (struct u_fail_logic *)logic;

	u8 local;
	float local1,local2;


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

		if(log->nast_.IU1)
		{
			log->w.I1_ort_a = *log->I1_orta;
			log->w.I1_ort_b = *log->I1_ortb;

			log->dw.idx_bufor++;
			if(log->dw.idx_bufor>9) log->dw.idx_bufor = 0;
			log->dw.I1_old[0] = log->dw.buforI[log->dw.idx_bufor][0];
			log->dw.I1_old[1] = log->dw.buforI[log->dw.idx_bufor][1];
			log->dw.I1_old[2] = log->dw.buforI[log->dw.idx_bufor][2];
			log->dw.U1_old = log->dw.buforU[log->dw.idx_bufor];
			log->dw.buforI[log->dw.idx_bufor][0] = log->w.I1_ort_a;
			log->dw.buforI[log->dw.idx_bufor][1] = log->w.I1_ort_b;
			log->dw.buforI[log->dw.idx_bufor][2] = log->dw.I1p;
			log->dw.buforU[log->dw.idx_bufor] = log->dw.U1p;


			local1 = log->w.I1_ort_a - log->dw.I1_old[0];
				local2 = log->w.I1_ort_b - log->dw.I1_old[1];
				local = (local1*local1 + local2*local2) > log->nast_.DI1k;


			sprawdz_P(
				&log->dw.UI1_Iwk,
				local,
				!local,
				&log->dw.liczI1k,
				3,4
				);


		}


			local = log->w.U0 < log->nast_.U0_dbl && log->w.U1 > log->nast_.U1_dbl && log->w.U2 < log->nast_.U2_dbl;

			sprawdz_P(
				&log->dw.U_OK,
				local,
				!local,
				&log->dw.liczOK,
				6,3
				);




		//realizacja przekaznika czasowego
		if ((*log->Bl_Ufail & (1<<log->Bl_Ufail_bit)))
		{
			log->dw.liczt++;
			if (log->dw.liczt>log->nast_.tz)
			{
				log->dw.Syg_Ufail = 1;
				log->dw.liczt--;
			}
		} else {
			log->dw.Syg_Ufail = 0;
			log->dw.liczt = 0;
		}




			if(log->dw.Syg_Ufail)
				*log->Syg_Ufail|=(1<<log->Syg_Ufail_bit);
			else
				*log->Syg_Ufail&=~(1<<log->Syg_Ufail_bit);

	} else {
		*log->Syg_Ufail&=~(1<<log->Syg_Ufail_bit);
	}
}

void u_fail_20hz(void *arguments, void *logic)
{
	struct u_fail_logic *log = (struct u_fail_logic *)logic;

	if(!(*log->Bl_Ufail & (1<<log->Bl_Ufail_bit)) && log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo
    {
	//	log->dw.R.I0 = sqrt(log->dw.I0);
	//	log->dw.R.I1 = sqrt(log->dw.I1);
//		log->dw.R.I2 = sqrt(log->dw.I2);
	//	log->dw.R.U0 = sqrt(log->dw.U0);
	//	log->dw.R.U1 = sqrt(log->dw.U1);
//		log->dw.R.U2 = sqrt(log->dw.U2);
//        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 = true; // sygnalizacja powstania nowej rejestracji
        log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia
    }
}
