/*
 * ps.c
 *
 *  Created on: 09-08-2016
 *      Author: Krzysztof Jakubczyk
 */

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

#include "ps.h"
#include "helper.h"
#include "analog_in.h"
#include <math.h>

int ps_initlog(void *arguments, void *logic)
{
	struct ps_args *args = (struct ps_args *)arguments;
	struct ps_logic *log = (struct ps_logic *)logic;
	u32 *an_params_ptr;

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

	log->U1_ortb = (float *)(log_manager.nets_data + (args->io.U1_ortb_float_in >> 3));
	if((u8 *)log->U1_ortb > (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->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;

	an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_I_ptr_in >> 3));
	if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;

	if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before)
		return -1;

	log->param_I = (struct analog_in_params *)*an_params_ptr;

	an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_U_ptr_in >> 3));
	if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;

	if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before)
		return -1;

	log->param_U = (struct analog_in_params *)*an_params_ptr;

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

	log->Bl_PS_bit = (args->io.Bl_PS_out & 0x07);

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

	log->PS_syn_bit = (args->io.PS_syn_out & 0x07);

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

	log->PS_asyn_bit = (args->io.PS_asyn_out & 0x07);
*/
	log->Z = (u8 *)(log_manager.nets_data + (args->io.Z_out >> 3));
	if((u8 *)log->Z > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->Z_bit = (args->io.Z_out & 0x07);

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

	log->W_bit = (args->io.W_out & 0x07);


	log->nast_.on = args->params.bity & 0x0001;
	log->nast_.W = args->params.bity & 0x0002;
	log->nast_.bl = args->params.bity & 0x0004;
	log->nast_.D3Io = args->params.bity & 0x0008;
	log->nast_.D3I2 = args->params.bity & 0x0010;


	log->nast_.tpass = (uint32_t)(1000 * args->params.tpass);
	log->nast_.tPS = (uint32_t)(100 * (args->params.tPS + 0.005f));
	log->nast_.tdbl = (uint32_t)(100 * (args->params.tdbl + 0.005f));
	log->nast_.lPS = (uint16_t)args->params.lPS;

	//przaeliczenie nastaw

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

	log->nast_.R1f = args->params.Rf;
	log->nast_.R1r = - args->params.Rr;
	log->nast_.X1r = - args->params.Xr;
	log->nast_.X1f = args->params.Xf;

	log->nast_.R2f = args->params.Rf - args->params.dRf;
	log->nast_.R2r = args->params.dRr - args->params.Rr;
	log->nast_.X2r = args->params.dXr - args->params.Xr;
	log->nast_.X2f = args->params.Xf - args->params.dXf;

	log->nast_.dRlf = args->params.dRf;
	log->nast_.dRlr = args->params.dRr;

	log->nast_.dRl1f = args->params.RLf;
	log->nast_.dRl1r = - args->params.RLr;
	log->nast_.dRl2f = args->params.RLf - args->params.dRf;
	log->nast_.dRl2r = args->params.dRr - args->params.RLr;

	log->nast_.tanfi1 = tan((3.141593 / 180) * args->params.fi1);
	log->nast_.tanfi2 = tan((3.141593 / 180) * args->params.fi2);
	log->nast_.przekladnia = log->param_U->znam_wtor / log->param_I->znam_wtor;

	log->dw.stan_kolysanie = kolysanie_OK;


	return 0;
}

void ps(void *arguments, void *logic)
{
	struct ps_logic *log = (struct ps_logic *)logic;

	float local;
	u8 wk1,wk2,wk3;


	if(log->nast_.on)   // jesli zabezpieczenie aktywne
    {
		log->w.I0 = *log->I0;
		log->w.I1 = *log->I1;
		log->w.I2 = *log->I2;
		log->w.I1_orta = *log->I1_orta;
		log->w.I1_ortb = *log->I1_ortb;
		log->w.U1 = *log->U1;
		log->w.U1_orta = *log->U1_orta;
		log->w.U1_ortb = *log->U1_ortb;


		// sprawdzenie stanu przekanikw skadowej zerowej i przeciwnej.
		if(log->nast_.D3Io)
		{
			wk1 = (log->w.I0 > log->nast_.Iomin) && (log->w.I1 * log->nast_.Iokh < log->w.I0);
			sprawdz_P(
				&log->dw.P_Io,
				wk1,
				!wk1,
				&log->dw.liczIo,
				3,3
				);
		} else log->dw.P_Io = 0;
		if(log->nast_.D3I2)
		{
			wk1 = (log->w.I2 > log->nast_.I2min) && (log->w.I1 * log->nast_.I2kh < log->w.I2);
			sprawdz_P(
				&log->dw.P_I2,
				wk1,
				!wk1,
				&log->dw.liczI2,
				3,3
				);
		} else log->dw.P_I2 = 0;

		wk3 = log->dw.P_Io || log->dw.P_I2; ///< jest zwarcie (na pewno nie koysania mocy)
//--------------------------------------------------------------------------------------------------------------

		//wyliczenie impedancji
		log->dw.R = log->w.I1_orta * log->w.U1_orta + log->w.I1_ortb * log->w.U1_ortb;
		log->dw.X = log->w.I1_ortb * log->w.U1_orta - log->w.I1_orta * log->w.U1_ortb;
		local = log->w.I1_orta * log->w.I1_orta + log->w.I1_ortb * log->w.I1_ortb;
		local = log->nast_.przekladnia / local;
		log->dw.R *= local;
		log->dw.X *= local;
		log->dw.absR = fabs(log->dw.R);
		log->dw.absX = fabs(log->dw.X);



		// okreslenie polozenia wektora impedancji wzgledem plaszczyzny nastaw
		wk1 = (log->w.I1 > log->nast_.Imin) &&
			(log->dw.X < log->nast_.X1f) &&
			(log->dw.X > log->nast_.X1r) &&
			((log->dw.X > (log->dw.R - log->nast_.R1f) * log->nast_.tanfi1) || (log->dw.R < log->nast_.R1f)) &&
			((log->dw.X < (log->dw.R - log->nast_.R1r) * log->nast_.tanfi1) || (log->dw.R > log->nast_.R1r)) &&
			((log->dw.absX > (log->dw.absR * log->nast_.tanfi2)) || ((log->dw.R < log->nast_.dRl1f) && (log->dw.R > log->nast_.dRl1r))) ;

float Rwk2;
		if(log->dw.R>0)
		{
			Rwk2 = log->dw.absR + log->nast_.dRlf;
		} else {
			Rwk2 = log->dw.absR + log->nast_.dRlr;
		}

		wk2 = (log->w.I1 > log->nast_.Imin) &&
			(log->dw.X < log->nast_.X2f) &&
			(log->dw.X > log->nast_.X2r) &&
			((log->dw.X > (log->dw.R - log->nast_.R2f) * log->nast_.tanfi1) || (log->dw.R < log->nast_.R2f)) &&
			((log->dw.X < (log->dw.R - log->nast_.R2r) * log->nast_.tanfi1) || (log->dw.R > log->nast_.R2r)) &&
			((log->dw.absX > (Rwk2 * log->nast_.tanfi2)) ||
				((log->dw.R < log->nast_.dRl2f) && (log->dw.R > log->nast_.dRl2r)));

		sprawdz_P(
				&log->dw.Ps1,
				wk1,
				!wk1,
				&log->dw.liczs1,
				5,5
				);

		sprawdz_P(
				&log->dw.Ps2,
				wk2,
				!wk2,
				&log->dw.liczs2,
				5,5
				);

		switch(log->dw.stan_kolysanie)
		{
			case kolysanie_OK:

				//check_and_set(0, log->Bl_PS, log->Bl_PS_bit);

				if(log->dw.Ps1 && !wk3)
				{
					log->dw.liczk = log->nast_.tpass;
					log->dw.stan_kolysanie = wejscie_w_strefe1;
					log->dw.tpass = 0;
					log->dw.dl_ = 0;
					log->dw.t1 = 0;
				}
				break;
			case wejscie_w_strefe1:

				//check_and_set(1, log->Bl_PS, log->Bl_PS_bit);

				if(log->dw.liczk>LOOP_CYCLE_MS)
					log->dw.liczk-=LOOP_CYCLE_MS;
				else
					log->dw.liczk=0;
				log->dw.tpass+=LOOP_CYCLE_MS;
				log->dw.dl_+=LOOP_CYCLE_MS;

				if(log->dw.Ps2)
				{
					log->dw.stan_kolysanie = blokada_kolysania;
				} else {
					if(!log->dw.liczk)
					{
						log->dw.stan_kolysanie = oczekiwanie_na_strefe2;
					}
				}
				if (!log->dw.Ps1 || wk3)
				{
					log->dw.stan_kolysanie = kolysanie_OK;
				}
				break;
			case oczekiwanie_na_strefe2:
				log->dw.tpass+=LOOP_CYCLE_MS;
				log->dw.dl_+=LOOP_CYCLE_MS;
				if (wk3)
				{
					log->dw.stan_kolysanie = blokada_kolysania;
				} else {
					if(log->dw.Ps2)
					{
						log->dw.b.Bl_PS = 1; ///< wczenie blokady
						log->dw.liczdbl = log->nast_.tdbl;
						log->dw.stan_kolysanie = kolysanie_mocy;
						log->dw.Rz = log->dw.R;
						//log->dw.czas_ = Time();
						log->dw.t1++;
					}
				}
				break;
			case kolysanie_mocy:
				log->dw.dl_+=LOOP_CYCLE_MS;
				if (log->dw.Ps2) log->dw.t1+=LOOP_CYCLE_MS;
				if(wk3)
				{
					log->dw.stan_kolysanie = blokada_kolysania;
					log->dw.wsk_rej_ = 1;
				}
				break;
			case blokada_kolysania:
				log->dw.b.Bl_PS = 0; ///< wyczenie blokady
				break;
		}

		//obsuga wejcia testu przekanika
			if (czy_test_R())
			{
				if(*log->test & (1<<log->test_bit))
				{
					log->dw.b.Bl_PS =
					log->dw.b.PS_asyn =
					log->dw.b.PS_syn =
					log->dw.b.W =
					log->dw.b.Z = 1;
				}
				else
				{
					log->dw.b.Bl_PS =
					log->dw.b.PS_asyn =
					log->dw.b.PS_syn =
					log->dw.b.W =
					log->dw.b.Z = 0;
				}
			}
	        //


        // realizacja blokady dzialania przekaznika
			if(log->nast_.bl && (*log->stan_bl & (1<<log->stan_bl_bit))) {
				log->dw.b.Bl_PS =
				log->dw.b.PS_asyn =
				log->dw.b.PS_syn =
				log->dw.b.W =
				log->dw.b.Z =
				log->dw.liczlPS = 0;
				}
        //

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


		check_and_set(log->dw.b.Bl_PS, log->Bl_PS, log->Bl_PS_bit);
		check_and_set(log->dw.b.W, log->W, log->W_bit);
		check_and_set(log->dw.b.Z, log->Z, log->Z_bit);
//		check_and_set(log->dw.b.PS_asyn, log->PS_asyn, log->PS_asyn_bit);
//		check_and_set(log->dw.b.PS_syn, log->PS_syn, log->PS_syn_bit);


  } else {
	  *log->Z&=~(1<<log->Z_bit);
	  *log->W&=~(1<<log->W_bit);
	  *log->Bl_PS&=~(1<<log->Bl_PS_bit);
//	  *log->PS_syn&=~(1<<log->PS_syn_bit);
//	  *log->PS_asyn&=~(1<<log->PS_asyn_bit);
  }
}

void ps_100hz(void *arguments, void *logic)
{
	struct ps_logic *log = (struct ps_logic *)logic;

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


		switch(log->dw.stan_kolysanie)
		{
			case kolysanie_OK:
				log->dw.licztPS--;
				if(log->dw.licztPS<0)
				{
					log->dw.b.PS_asyn = 0;
					log->dw.b.PS_syn = 0;
					log->dw.b.W = 0;
					log->dw.b.Z = 0;
					log->dw.liczlPS = 0;
				}
				break;
			case wejscie_w_strefe1:
				break;
			case oczekiwanie_na_strefe2:
				if (!log->dw.Ps1)
				{
					log->dw.stan_kolysanie = kolysanie_OK;
				}

				break;
			case kolysanie_mocy:
				if(!log->dw.Ps1)
				{
					log->dw.b.Bl_PS = 0; //wylaczenie blokady
					log->dw.licztPS = log->nast_.tPS;
					if((log->dw.Rz * log->dw.R) > 0) //kolysania synchroniczne
					{
						log->dw.b.PS_asyn = 1;

					} else { //kolysania asynchroniczne
						log->dw.b.PS_syn = 1;
						log->dw.liczlPS++;
						if (log->dw.liczlPS==log->nast_.lPS)
						{
							log->dw.b.Z = 1;
						}
					}
					log->dw.stan_kolysanie = kolysanie_OK;
					log->dw.wsk_rej_ = 1;
				} else {
					log->dw.liczdbl--;
					if(!log->dw.liczdbl)
					{
						log->dw.stan_kolysanie = blokada_kolysania;
						log->dw.b.Bl_PS = 0; //wylaczenie blokady od kolysan mocy
						log->dw.wsk_rej_ = 1;
					}
				}

				break;
			case blokada_kolysania:
				if (!log->dw.Ps1)
				{
					log->dw.stan_kolysanie = kolysanie_OK;
				}

				break;
		}

		log->dw.b.W = log->dw.b.Z && log->nast_.W;
	}
}

void ps_20hz(void *arguments, void *logic)
{
	struct ps_logic *log = (struct ps_logic *)logic;
	if(log->dw.wsk_rej_) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo
    {
//		log->dw.Rej.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia
//      log->dw.Rej.dlugosc = (float)log->dw.dl_ * 0.001f;// / 1000; // wpisanie czasu trwania zwarcia
//		log->dw.Rej.tpass = (float)log->dw.tpass * 0.001f;// / 1000;//log->dw.tpass / 1000;
//		log->dw.Rej.t1 = (float)log->dw.t1 * 0.001f;// / 1000;
//      log->dw.Rej.czy_nowa = true; // sygnalizacja powstania nowej rejestracji
        log->dw.wsk_rej_ = 0; // zerowanie wskaznika powstania nowego zaklocenia
    }
}
