/*
 * ov_3ph2h_slow.c
 *
 *  Created on: 08-06-2017
 *      Author: Krzysztof Jakubczyk
 */

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

#include "helper.h"
#include "ov_3ph2h_slow.h"


int ov_3ph2h_slow_initlog(void *arguments, void *logic)
{
	struct ov_3ph2h_slow_args *args = (struct ov_3ph2h_slow_args *)arguments;
	struct ov_3ph2h_slow_logic *log = (struct ov_3ph2h_slow_logic *)logic;

	if(set_bit_ptr(args->io.stan_bl,&log->stan_bl,&log->stan_bl_bit_no))
		return -1;

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

	if(set_bit_ptr(args->io.W_out,&log->W,&log->W_bit_no))
		return -1;

	if(set_bit_ptr(args->io.Z_out,&log->Z,&log->Z_bit_no))
		return -1;

	if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no))
		return -1;

	if(set_bit_ptr(args->io.PL1_out,&log->PL1,&log->PL1_bit_no))
		return -1;

	if(set_bit_ptr(args->io.PL2_out,&log->PL2,&log->PL2_bit_no))
		return -1;

	if(set_bit_ptr(args->io.PL3_out,&log->PL3,&log->PL3_bit_no))
		return -1;

	if(set_float_ptr(args->io.I1_float_in,&log->I1))
		return -1;

	if(set_float_ptr(args->io.I2_float_in,&log->I2))
		return -1;

	if(set_float_ptr(args->io.I3_float_in,&log->I3))
		return -1;

	if(set_float_ptr(args->io.I12h_float_in,&log->I12h))
		return -1;

	if(set_float_ptr(args->io.I22h_float_in,&log->I22h))
		return -1;

	if(set_float_ptr(args->io.I32h_float_in,&log->I32h))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_2h_out,&log->bl_2h))
		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_.or_ = (args->params.bity & 0x0008)?1:0;
    log->nast_.limit1_ = args->params.Wr_ *  args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu
    log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu
	log->nast_.t_ = (args->params.t_ - 0.005f) * 100; //czas dzialania

	log->nast_.I2h = args->params.I2h * args->params.I2h;
//	log->nast_.I2h_powr = (args->params.kp_ * args->params.I2h) * (args->params.kp_ * args->params.I2h);
	log->nast_.I2h_powr = (0.99 * args->params.I2h) * (0.99 * args->params.I2h);

	log->nast_.I2h_min = log->nast_.I2h * log->nast_.limit1_;

	log->nast_.Ibezwar = args->params.Ibezwar * args->params.Ibezwar;
	log->nast_.Ibezwar_powr =  (args->params.kp_ * args->params.Ibezwar) * (args->params.kp_ * args->params.Ibezwar);
	//*((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = nast_.on_;

	return 0;
}

void ov_3ph2h_slow(void *arguments, void *logic)
{
	struct ov_3ph2h_slow_logic *log = (struct ov_3ph2h_slow_logic *)logic;
	u8 jest_jedna_blokada = 0;
	u8 jest_jeden_bezwar = 0;

	if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {
		sprawdz_P(&log->dw.pob12h,
			*log->I12h>(log->nast_.I2h * *log->I1) && *log->I12h>log->nast_.I2h_min,
			*log->I12h<(log->nast_.I2h_powr * *log->I1) || *log->I12h<log->nast_.I2h_min,
			&log->dw.lp12h,
			20,20); //realizacja przekaznika

		sprawdz_P(&log->dw.pob11h,
			*log->I1>log->nast_.limit1_,
			*log->I1<log->nast_.limit2_,
			&log->dw.lp11h,
			30,20); //realizacja przekaznika faza 1

		sprawdz_P(&log->dw.pob1bezwar,*log->I1>log->nast_.Ibezwar,*log->I1<log->nast_.Ibezwar_powr,
			&log->dw.lp1bezwar,
			1,1); //realizacja przekaznika

		sprawdz_P(&log->dw.pob1,
			log->dw.pob11h && (!log->dw.pob12h || log->dw.pob1bezwar),
			!log->dw.pob11h || (log->dw.pob12h && !log->dw.pob1bezwar),
			&log->dw.lp1,
			1,1); //realizacja przekaznika

//
		sprawdz_P(&log->dw.pob22h,
			*log->I22h>(log->nast_.I2h * *log->I2)&&*log->I22h>log->nast_.I2h_min,
			*log->I22h<(log->nast_.I2h_powr * *log->I2)||*log->I22h<log->nast_.I2h_min,
			&log->dw.lp22h,
			20,20); //realizacja przekaznika

		sprawdz_P(&log->dw.pob21h,
			*log->I2>log->nast_.limit1_,
			*log->I2<log->nast_.limit2_,
			&log->dw.lp21h,
			30,20); //realizacja przekaznika faza 1

		sprawdz_P(&log->dw.pob2bezwar,*log->I2>log->nast_.Ibezwar,*log->I2<log->nast_.Ibezwar_powr,
			&log->dw.lp2bezwar,
			1,1); //realizacja przekaznika

		sprawdz_P(&log->dw.pob2,
			log->dw.pob21h && (!log->dw.pob22h || log->dw.pob2bezwar),
			!log->dw.pob21h || (log->dw.pob22h && !log->dw.pob2bezwar),
			&log->dw.lp2,
			1,1); //realizacja przekaznika

//
		sprawdz_P(&log->dw.pob32h,
			*log->I32h>(log->nast_.I2h * *log->I3)&&*log->I32h>log->nast_.I2h_min,
			*log->I32h<(log->nast_.I2h_powr * *log->I3)||*log->I32h<log->nast_.I2h_min,
			&log->dw.lp32h,
			20,20); //realizacja przekaznika

		sprawdz_P(&log->dw.pob31h,
			*log->I3>log->nast_.limit1_,
			*log->I3<log->nast_.limit2_,
			&log->dw.lp31h,
			30,20); //realizacja przekaznika faza 1

		sprawdz_P(&log->dw.pob3bezwar,*log->I3>log->nast_.Ibezwar,*log->I3<log->nast_.Ibezwar_powr,
			&log->dw.lp3bezwar,
			1,1); //realizacja przekaznika

		sprawdz_P(&log->dw.pob3,
			log->dw.pob31h && (!log->dw.pob32h || log->dw.pob3bezwar),
			!log->dw.pob31h || (log->dw.pob32h && !log->dw.pob3bezwar),
			&log->dw.lp3,
			1,1); //realizacja przekaznika

//

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


        // realizacja blokady dzialania przekaznika
		if((log->nast_.bl_ && (*log->stan_bl & (1<<log->stan_bl_bit_no))) /*| czy_start()*/)
		{
			log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0;
		}
        //

		u8 bl2h=0;

        if(log->nast_.or_)  // zaleznie od nastawienia logiki pobudzen "and" lub "or"
        {
        	jest_jedna_blokada = (log->dw.pob11h && log->dw.pob12h) || (log->dw.pob21h && log->dw.pob22h) || (log->dw.pob31h && log->dw.pob32h);
        	jest_jeden_bezwar = log->dw.pob1bezwar || log->dw.pob2bezwar || log->dw.pob3bezwar;

			if (jest_jedna_blokada && !jest_jeden_bezwar)
			{
				bl2h=1;
        		log->dw.pob = 0; //blokowanie gdy zablokowana jest jedna faza
			}
			else
			{
				bl2h=0;
				log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "or"
			}
        } else {
			log->dw.pob = log->dw.pob1 && log->dw.pob2 && log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "and"
        	bl2h=(log->dw.pob11h && log->dw.pob12h) || (log->dw.pob21h && log->dw.pob22h) || (log->dw.pob31h && log->dw.pob32h);
        }


		if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego
			if (!log->dw.zad & log->dw.pob)
			{
				log->dw.zad = 1;
				//log->dw.czas_ = time;
			}

		check_and_set(log->dw.pob,log->P,log->P_bit_no);
		check_and_set(log->dw.pob1,log->PL1,log->PL1_bit_no);
		check_and_set(log->dw.pob2,log->PL2,log->PL2_bit_no);
		check_and_set(log->dw.pob3,log->PL3,log->PL3_bit_no);
		check_and_set(log->dw.zad,log->Z,log->Z_bit_no);
		check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no);
		check_and_set_struct(bl2h,&log->bl_2h);
    } else {
    	clear_struct(&log->bl_2h);
		*log->W&=~(1<<log->W_bit_no);
		*log->Z&=~(1<<log->Z_bit_no);
		*log->P&=~(1<<log->P_bit_no);
		*log->PL1&=~(1<<log->PL1_bit_no);
		*log->PL2&=~(1<<log->PL2_bit_no);
		*log->PL3&=~(1<<log->PL3_bit_no);
	}
}

void ov_3ph2h_slow_100hz(void *arguments, void *logic)
{
	struct ov_3ph2h_slow_logic *log = (struct ov_3ph2h_slow_logic *)logic;

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

		// realizacja przekaznika czasowego
		if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania
		if(log->dw.licz_t_ > log->nast_.t_)
		{ // sprawdzenie przekroczenia nastawionego czasu
			log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu
			log->dw.licz_t_--;
		} else log->dw.zad = 0;
		//
		// realizacja rejestratora parametrow ostatniego zaklocenia
	/*	float wej1 = dane_analogowe[nast_.adr.kwampl1];
		float wej2 = dane_analogowe[nast_.adr.kwampl2];
		float wej3 = dane_analogowe[nast_.adr.kwampl3];

			switch(status_rej)
			{
				case oczekiwanie:
					if (dw.pob)
					{
						dw.max1_ = wej1;
						dw.max2_ = wej2;
						dw.max3_ = wej3;
						dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia
						status_rej = sledzenie;
						if (dw.zad)
						{
							status_rej = sledzenie_zad;
						}
					}
					break;
				case sledzenie:
					if (dw.zad)
					{
						status_rej = sledzenie_zad;
						dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji
					}
					if (dw.pob)
					{
						if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej
						if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej
						if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej
						dw.dl_++;
					} else {
						status_rej = oczekiwanie;
					}
					break;
				case sledzenie_zad:
					if (!dw.pob)
					{
						dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem
						status_rej = zapis;
					} else {
						dw.dl_++;
						if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej
						if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej
						if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej
					}
					break;
				case zapis:
					if (!dw.wsk_rej)
					{
						status_rej = oczekiwanie;
					}
					break;
			}*/
	}
}

void ov_3ph2h_slow_20hz(void *arguments, void *logic)
{
	//struct ov_3ph_logic *log = (struct ov_3ph_logic *)logic;

	//if(dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo
    //{
    //   dw.R.max_L1_ = sqrt(dw.max1_); // wyliczenie maksymalnej wartosci  faza 1
    //    dw.R.max_L2_ = sqrt(dw.max2_); // wyliczenie maksymalnej wartosci  faza 1
    //    dw.R.max_L3_ = sqrt(dw.max3_); // wyliczenie maksymalnej wartosci  faza 1
    //    dw.R.moment_wystapienia = dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia
    //    dw.R.dlugosc = (float)dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia
    //    dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji
    //    dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia
    //}

}