/*
 * i_fail.c
 *
 *  Created on: 03-08-2016
 *      Author: Krzysztof Jakubczyk
 */

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

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

int i_fail_initlog(void *arguments, void *logic)
{
	struct i_fail_args *args = (struct i_fail_args *)arguments;
	struct i_fail_logic *log = (struct i_fail_logic *)logic;

	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->I1S0a = (float *)(log_manager.nets_data + (args->io.I1S0a_float_in >> 3));
	if((u8 *)log->I1S0a > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

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

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

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

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

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

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

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

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

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

	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->Bl_Ifail = (u8 *)(log_manager.nets_data + (args->io.Bl_Ifail_out >> 3));
	if((u8 *)log->Bl_Ifail > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->Bl_Ifail_bit = (args->io.Bl_Ifail_out & 0x07);

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

	log->Ifailsyg_bit = (args->io.Ifailsyg_out & 0x07);

	log->nast_.on = args->params.bity & 0x0001;
	log->nast_.WyBl_ON = args->params.bity & 0x002;
	log->nast_.Idbl_ON = args->params.bity & 0x004;
	log->nast_.Udbl_ON = args->params.bity & 0x008;
	log->nast_.bl = args->params.bity & 0x0010;

	log->nast_.Imin = args->params.Imin * args->params.Imin * 2;
	log->nast_.Ikh = args->params.Ikh * args->params.Ikh * 2;
	log->nast_.ksch = args->params.ksch;
	log->nast_.ksch2 = args->params.ksch * args->params.ksch * 2;


	log->nast_.Idbl = args->params.Idbl * args->params.Idbl;
	log->nast_.Udbl = args->params.Udbl * args->params.Udbl;


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

	return 0;
}

void i_fail(void *arguments, void *logic)
{
	struct i_fail_logic *log = (struct i_fail_logic *)logic;

	u8 local, local2;
	float tempa,tempb,max;

    if(log->nast_.on)   // jesli zabezpieczenie aktywne
    {
		tempa = *log->I1S0a - log->nast_.ksch * *log->Irefa;
		tempb =	*log->I1S0b - log->nast_.ksch * *log->Irefb;

		max = *log->I1L1;
		if (max < *log->I1L2) max = *log->I1L2;
		if (max < *log->I1L3) max = *log->I1L3;

		log->dw.Irr = tempa * tempa + tempb * tempb;
		local = (log->dw.Irr > log->nast_.Imin) && (log->dw.Irr > log->nast_.Ikh * max);
		local2 = (log->dw.Irr < (0.96 * log->nast_.Imin)) || (log->dw.Irr < (0.96
				* log->nast_.Ikh * max));

		//sprawdzenie kryterium rznicowego
		sprawdz_P(
			&log->dw.PIrr,
			local,
			local2,
			&log->dw.licz_Irr,
			3,3
			);



		//sprawdzenie deblokady prdowej
		if (log->nast_.Idbl_ON)
		{
			local = max > log->nast_.Idbl;
			sprawdz_P(
				&log->dw.PIdbl,
				local,
				!local,
				&log->dw.licz_Idbl,
				3,10
				);
		} else log->dw.PIdbl = 0;

		//sprawdzenie deblokady napicia zerowego
		if (log->nast_.Udbl_ON)
		{
			local = *log->U1S0 > log->nast_.Udbl;
			sprawdz_P(
				&log->dw.PUdbl,
				local,
				!local,
				&log->dw.licz_Udbl,
				3,10
				);
		} else log->dw.PUdbl = 0;


		//ustawienie wyjcia

		if (*log->Ifailsyg & (1<<log->Ifailsyg_bit))
		{
			log->dw.P = log->dw.PIrr;
		} else {
			log->dw.P = log->dw.PIrr & !log->dw.PIdbl & !log->dw.PUdbl;
		}



		//obsluga wejscia testu przekaznika
		if (czy_test_R())
		{
			log->dw.P = (*log->test & (1<<log->test_bit))?1:0;
		}
	    //


        // realizacja blokady dzialania przekaznika
		if(log->nast_.bl && (*log->stan_bl & (1<<log->stan_bl_bit))) {
			log->dw.P = 0;
		}
        //

		if(log->nast_.WyBl_ON)
		{
			if(log->dw.P)
				*log->Bl_Ifail|=(1<<log->Bl_Ifail_bit);
			else
				*log->Bl_Ifail&=~(1<<log->Bl_Ifail_bit);
		} else *log->Bl_Ifail&=~(1<<log->Bl_Ifail_bit);


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


	} else {
		*log->Bl_Ifail&=~(1<<log->Bl_Ifail_bit);
	}
}

void i_fail_100hz(void *arguments, void *logic)
{
	struct i_fail_logic *log = (struct i_fail_logic *)logic;

    if(log->nast_.on)   // jesli zabezpieczenie aktywne
    {
		//realizacja przekaznika czasowego
		if (*log->Bl_Ifail & (1<<log->Bl_Ifail_bit))
		{
			log->dw.liczt++;
			if (log->dw.liczt>log->nast_.tz)
			{
				log->dw.Syg_Ifail = 1;
				log->dw.liczt--;
			}
		} else {
			log->dw.Syg_Ifail = 0;
			log->dw.liczt = 0;
		}




			if(log->dw.Syg_Ifail)
				*log->Ifailsyg|=(1<<log->Ifailsyg_bit);
			else
				*log->Ifailsyg&=~(1<<log->Ifailsyg_bit);
	} else {
		*log->Ifailsyg&=~(1<<log->Ifailsyg_bit);
	}
}

void i_fail_20hz(void *arguments, void *logic)
{
	struct i_fail_logic *log = (struct i_fail_logic *)logic;

	if(!(*log->Bl_Ifail & (1<<log->Bl_Ifail_bit)) && log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo
    {
//		log->dw.R.Irr = sqrt(log->dw.RIrr / 2);
//		log->dw.R.I1L1 = sqrt(log->dw.RI1L1);
//		log->dw.R.I1L2 = sqrt(log->dw.RI1L2);
//		log->dw.R.I1L3 = sqrt(log->dw.RI1L3);
//		log->dw.R.Iref = sqrt(log->dw.RIref);
//		log->dw.R.I1S0 = sqrt(log->dw.RI1S0);
//		log->dw.R.US0 = sqrt(log->dw.RUS0);
//      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
    }
}
