/*
 * R_67N_LWN.c
 *
 *  Created on: 20-07-2016
 *      Author: Krzysztof Jakubczyk
 */

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

#include "r_67n_lwn.h"
#include "analog_in.h"
#include "helper.h"
#include "ZDistL.h"

#include <math.h>

int r_67n_lwn_initlog(void *arguments, void *logic)
{
	struct r_67n_lwn_args *args = (struct r_67n_lwn_args *)arguments;
	struct r_67n_lwn_logic *log = (struct r_67n_lwn_logic *)logic;

	if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl_))
		return -1;

	if(set_float_ptr(args->io.orta_I_float_in,&log->orta_I_))
		return -1;

	if(set_float_ptr(args->io.ortb_I_float_in,&log->ortb_I_))
		return -1;

	if(set_float_ptr(args->io.kwampl_I_float_in,&log->kwampl_I_))
		return -1;

	if(set_float_ptr(args->io.orta_U_float_in,&log->orta_U_))
		return -1;

	if(set_float_ptr(args->io.ortb_U_float_in,&log->ortb_U_))
		return -1;

	if(set_float_ptr(args->io.kwampl_U_float_in,&log->kwampl_U_))
		return -1;

	if(set_float_ptr(args->io.IL1_float_in,&log->IL1))
		return -1;

	if(set_float_ptr(args->io.IL2_float_in,&log->IL2))
		return -1;

	if(set_float_ptr(args->io.IL3_float_in,&log->IL3))
		return -1;

	if(set_float_ptr(args->io.Io2h_float_in,&log->Io2h))
		return -1;

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

	if(set_bit_ptr_struct(args->io.l_odb_in,&log->l_odb))
		return -1;

	if(set_bit_ptr_struct(args->io.l_ok_in,&log->l_ok))
		return -1;

	if(set_bit_ptr_struct(args->io.w_out,&log->W))
		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.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.kier_out,&log->kier))
		return -1;

	if(set_bit_ptr_struct(args->io.NRL_out,&log->NRL))
		return -1;


	////
	//status_rej = oczekiwanie;

	log->nast_.on_ = args->params.bity & 0x0001;
	log->nast_.w1 = args->params.bity & 0x0002;
	log->nast_.w2 = args->params.bity & 0x0004;
	log->nast_.Bl_2h1_ON = (args->params.bity & 0x0008)?1:0;
	log->nast_.Bl_2h2_ON = (args->params.bity & 0x0010)?1:0;
	log->nast_.bl_ = args->params.bity & 0x0020;


	log->nast_.dir = args->params.dir;

    log->nast_.Ir1 = args->params.I1 *  args->params.I1; // wyliczenie wartosci rozruchowej pradu
    log->nast_.Ir2 = args->params.I2 *  args->params.I2; // wyliczenie wartosci rozruchowej pradu

	log->nast_.Ip1 = log->nast_.Ir1 * args->params.kp * args->params.kp; // wyliczenie wartoci powrotu algorytmu
    log->nast_.Ip2 = log->nast_.Ir2 * args->params.kp * args->params.kp; // wyliczenie wartoci powrotu algorytmu

	log->nast_.kh1 = args->params.kh1 * args->params.kh1;
	log->nast_.khp1 = log->nast_.kh1 * args->params.kp * args->params.kp;
	log->nast_.kh2 = args->params.kh2 * args->params.kh2;
	log->nast_.khp2 = log->nast_.kh2 * args->params.kp * args->params.kp;

    log->nast_.U_echo = args->params.L_U_echo *  args->params.L_U_echo; // wyliczenie wartosci rozruchowej blokady napieciowej
    log->nast_.Ur = args->params.U *  args->params.U; // wyliczenie wartosci rozruchowej blokady napieciowej
    log->nast_.Up = log->nast_.Ur * args->params.kp * args->params.kp; // wyliczenie wartosci powrotu blokady napieciowej
    log->nast_.orta = cos(args->params.kat * (3.14159f / 180)); // wartosc ortogonalnej a wektora przesuniecia dla kata charakterystycznego
    log->nast_.ortb = sin(args->params.kat * (3.14159f / 180)); // wartosc ortogonalnej b wektora przesuniecia dla kata charakterystycznego

	log->nast_.I1_2hr = args->params.Io2h1 * args->params.Io2h1;
	log->nast_.I1_2hr_powr = (0.95 * args->params.Io2h1) * (0.95 * args->params.Io2h1);


	log->nast_.t1 = ((args->params.t1 - 0.005f) * 100); //czas dzialania
	if (log->nast_.t1) log->nast_.t0_1 = 0; else  log->nast_.t0_1 = 1;
	log->nast_.t2 = ((args->params.t2 - 0.005f) * 100); //czas dzialania
	if (log->nast_.t2) log->nast_.t0_2 = 0; else  log->nast_.t0_2 = 1;

	log->nast_.l_nad_time=(u32)(args->params.L_t_nad * 1000);
	log->nast_.l_t_blok=(u32)(args->params.L_T_odwr * 1000);
	log->nast_.t1_min=(u32)((args->params.t1_min - 0.005f) * 100);
	log->nast_.t2_min=(u32)((args->params.t2_min - 0.005f) * 100);
//	*((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = log->nast_.on_;

	return 0;
}


void r_67n_lwn(void *arguments, void *logic)
{
	struct r_67n_lwn_args *args = (struct r_67n_lwn_args *)arguments;
	struct r_67n_lwn_logic *log = (struct r_67n_lwn_logic *)logic;
	float local_orta_U_ ,local_ortb_U_, ILmax;
	u8 wk,l_nrl;

    if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {
		log->w.kwampl_U_ = *log->kwampl_U_;
		log->w.kwampl_I_ = *log->kwampl_I_;
		log->w.orta_I_ = *log->orta_I_;
		log->w.orta_U_ = *log->orta_U_;
		log->w.ortb_I_ = *log->ortb_I_;
		log->w.ortb_U_ = *log->ortb_U_;
		log->w.IL1 = *log->IL1;
		log->w.IL2 = *log->IL2;
		log->w.IL3 = *log->IL3;
		log->w.Io2h = *log->Io2h;

     // przesuniecie napiecia o wektor kata nastawy
        local_orta_U_ = log->w.orta_U_ * log->nast_.orta - log->w.ortb_U_ * log->nast_.ortb;
        local_ortb_U_ = log->w.orta_U_ * log->nast_.ortb + log->w.ortb_U_ * log->nast_.orta;

     // okreslenie kierunku
        log->dw.kierunek = (log->w.orta_I_ * local_orta_U_ + log->w.ortb_I_ * local_ortb_U_) > 0;
        if(!czy_test_R()) // w tescie nie uzywaj kierunku
        	check_and_set_struct(log->dw.kierunek,&log->kier);

        switch(log->nast_.dir) //okeslenie kierunku stopnia 1
		{
			case 0:
				wk = log->dw.kierunek;
				break;
			case 1:
				wk = !log->dw.kierunek;
				break;
			case 2:
				wk = 1;
				break;
		}

		//sprawdzenie warunku napiciowego
		log->dw.P_U |= log->w.kwampl_U_ > log->nast_.Ur;
		log->dw.P_U &= log->w.kwampl_U_ > log->nast_.Up;

		if(log->dw.Bl_2h1)
			log->dw.Bl_2h1 = log->w.Io2h > log->nast_.I1_2hr_powr * log->w.kwampl_I_;
		else
			log->dw.Bl_2h1 = log->w.Io2h > log->nast_.I1_2hr * log->w.kwampl_I_;

		if(log->dw.Bl_2h2)
			log->dw.Bl_2h2 = log->w.Io2h > log->nast_.I1_2hr_powr * log->w.kwampl_I_;
		else
			log->dw.Bl_2h2 = log->w.Io2h > log->nast_.I1_2hr * log->w.kwampl_I_;

        log->dw.Bl_2h1 &= log->nast_.Bl_2h1_ON;
        log->dw.Bl_2h2 &= log->nast_.Bl_2h2_ON;

		//Sprawdzenie przekaznikw pradowych
		//szukanie maksymalnego pradu
		ILmax = log->w.IL1;
		if (ILmax < log->w.IL2) ILmax = log->w.IL2;
		if (ILmax < log->w.IL3) ILmax = log->w.IL3;

		//sprawdzenie pobudzenia stopnia 1
		sprawdz_P(&log->dw.P1,
				  wk && log->dw.P_U && !log->dw.Bl_2h1 && log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > (log->nast_.kh1 * ILmax),
				  !wk || !log->dw.P_U || log->dw.Bl_2h1 || log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < (log->nast_.khp1 * ILmax),
				  &log->dw.lp1,
				  11,3
				  );

        //sprawdzenie pobudzenia stopnia 2
        sprawdz_P(&log->dw.P2,
        		  wk && log->dw.P_U && !log->dw.Bl_2h2 && log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > log->nast_.kh2 * ILmax,
                  !wk || !log->dw.P_U || log->dw.Bl_2h2 || log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < log->nast_.khp2 * ILmax,
                  &log->dw.lp2,
                  11,3
                  );

		//obsluga wejscia testu przekaznika
		if (czy_test_R())
		{
			log->dw.P1 = log->dw.P2 = check_struct(&log->test)?1:0;
		}
        //

        // realizacja blokady dzialania przekaznika
		if(log->nast_.bl_ && check_struct(&log->stan_bl_) /*| czy_start()*/)
				log->dw.P1 = log->dw.P2 = 0;
        //

		log->dw.Z1 |= (log->dw.P1 && log->nast_.t0_1);  // jesli dziaanie bezzwoczne stopnia 1
		log->dw.Z2 |= log->dw.P2 && log->nast_.t0_2; // jesli dziaanie bezzwoczne stopnia 2
		log->dw.Z = log->dw.Z1 || log->dw.Z2;

		if (log->dw.Z && !log->dw.Z_last) //zapisanie momentu zadziaania
		{
			log->dw.Z_last = 1;
//			log->dw.czas_ = time;
		}

		check_and_set_struct(log->dw.P1,&log->P1);
		check_and_set_struct(log->dw.P2,&log->P2);
		check_and_set_struct(log->dw.Z1,&log->Z1);
		check_and_set_struct(log->dw.Z2,&log->Z2);

		check_and_set_struct(((log->dw.Z1 && log->nast_.w1) || (log->dw.Z2 && log->nast_.w2) || (log->l_echo && (args->params.bity & R67N_L_ECHO_WYL))) && !log->l_blok_wyl && !log->dw.Podwr,&log->W);

		if(args->params.bity & R67N_L_AKT)
		{
			//nadawanie
			l_nrl=0;

			if(log->dw.P1 && args->params.bity & R67N_L_NADP1)
			{
				l_nrl=1;
				log->l_nad_cnt=0;
			}

			if(log->dw.P2 && args->params.bity & R67N_L_NADP2)
			{
				l_nrl=1;
				log->l_nad_cnt=0;
			}
			//odbior
			if(!(args->params.bity & R67N_L_BLOK)) // gdy tryb zezwalajacy
			{
				if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.Podwr)
				{
					if(log->dw.P1)
						log->skr_t1=1;
					else
						log->skr_t1=0;

					if(log->dw.P2)
						log->skr_t2=1;
					else
						log->skr_t2=0;
				}
				else
				{
					log->skr_t1=0;
					log->skr_t2=0;
				}
			}
			else // tryb blokujacy
			{
				if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.Podwr)
					log->l_blok_wyl=1;
				else
					log->l_blok_wyl=0;
			}
			//

			// echo
			if(args->params.bity & R67N_L_ECHO)
			{
		        sprawdz_P(&log->dw.PI0,
		        		  (log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > log->nast_.kh1 * ILmax)||(log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > log->nast_.kh2 * ILmax),
		                  (log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < log->nast_.khp1 * ILmax)&&(log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < log->nast_.khp2 * ILmax),
		                  &log->dw.lp3,
		                  5,200
		                  );

		        if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.PI0 && log->w.kwampl_U_ > log->nast_.U_echo && !log->dw.Podwr)
		        {
		        	l_nrl=1;
					log->l_nad_cnt=0;
		        	log->l_echo=1;
		        }
		        else
		        	log->l_echo=0;
			}


			// LRC
			if(args->params.bity & R67N_L_ODWR_PR)
			{
				sprawdz_P(&log->dw.Podwr,
						  !wk && log->dw.P_U && ((!log->dw.Bl_2h1 && log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > (log->nast_.kh1 * ILmax))||(!log->dw.Bl_2h2 && log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > (log->nast_.kh2 * ILmax))),
						  wk || !log->dw.P_U || ((log->dw.Bl_2h1 || log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < (log->nast_.khp1 * ILmax))&&(log->dw.Bl_2h2 || log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < (log->nast_.khp2 * ILmax))),
						  &log->dw.lp4,
						  40,log->nast_.l_t_blok
						  );

				if(log->dw.Podwr)
				{
					l_nrl=0;
					clear_struct(&log->NRL);
					log->l_nad_cnt=0;
				}
			}

			// wyslanie impulsu nadawania
			if(l_nrl && !log->l_nad_cnt)
				set_struct(&log->NRL);

			if(log->l_nad_cnt>=log->nast_.l_nad_time)
			{
				clear_struct(&log->NRL);
				if(!l_nrl)
					log->l_nad_cnt=0;
			}
			else if(check_struct(&log->NRL))
				log->l_nad_cnt++;


		}
	}
    else
	{
		clear_struct(&log->Z1);
		clear_struct(&log->Z2);
		clear_struct(&log->W);
		clear_struct(&log->P1);
		clear_struct(&log->P2);

		clear_struct(&log->NRL);
	}
}

void r_67n_lwn_100hz(void *arguments, void *logic)
{
	struct r_67n_lwn_logic *log = (struct r_67n_lwn_logic *)logic;
	int t;

    if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {
		// realizacja przekaznika czasowego
		if (log->dw.P1) log->dw.lt1++; else log->dw.lt1 = 0;
		if (log->dw.P2) log->dw.lt2++; else log->dw.lt2 = 0;

		if(log->skr_t1 && log->nast_.t1_min<log->nast_.t1)
			t=log->nast_.t1_min;
		else
			t=log->nast_.t1;

		if (log->dw.lt1 > t)
		{
			log->dw.Z1 = 1;
			log->dw.lt1--;
		}

		if(log->skr_t2 && log->nast_.t2_min<log->nast_.t2)
			t=log->nast_.t2_min;
		else
			t=log->nast_.t2;

		if (log->dw.lt2 > t)
		{
			log->dw.Z2 = 1;
			log->dw.lt2--;
		}

		//
		//kasowanie zadziaa po odpadniciu pobudze
		log->dw.P = log->dw.P1 || log->dw.P2;

		if(!log->dw.P1)
			log->dw.Z1=0;
		if(!log->dw.P2)
			log->dw.Z2=0;
		if(!log->dw.P)
			log->dw.Z=0;

		log->dw.Z_last = log->dw.Z;

		// 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;
						log->dw.max_orta_I = log->w.orta_I_;
						log->dw.max_ortb_I = log->w.ortb_I_;
						log->dw.max_orta_U = log->w.orta_U_;
						log->dw.max_ortb_U = log->w.ortb_U_;
						log->dw.max_I = log->w.kwampl_I_;
						log->dw.max_U = log->w.kwampl_U_;
						log->dw.I1 = log->w.IL1;
						log->dw.I2 = log->w.IL2;
						log->dw.I3 = log->w.IL3;
					}
					break;
				case sledzenie:
					if (log->dw.Z)
					{
						status_rej = sledzenie_zad;
						//log->dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji
					}
					if (log->dw.P)
					{
						if(log->w.kwampl_I_>log->dw.max_I)
						{
							log->dw.max_orta_I = log->w.orta_I_;
							log->dw.max_ortb_I = log->w.ortb_I_;
							log->dw.max_orta_U = log->w.orta_U_;
							log->dw.max_ortb_U = log->w.ortb_U_;
							log->dw.max_I = log->w.kwampl_I_;
							log->dw.max_U = log->w.kwampl_U_;
							log->dw.I1 = log->w.IL1;
							log->dw.I2 = log->w.IL2;
							log->dw.I3 = log->w.IL3;
							log->dw.Io2h = log->w.Io2h;
						}

						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 {

						if(log->w.kwampl_I_>log->dw.max_I)
						{
							log->dw.max_orta_I = log->w.orta_I_;
							log->dw.max_ortb_I = log->w.ortb_I_;
							log->dw.max_orta_U = log->w.orta_U_;
							log->dw.max_ortb_U = log->w.ortb_U_;
							log->dw.max_I = log->w.kwampl_I_;
							log->dw.max_U = log->w.kwampl_U_;
							log->dw.I1 = log->w.IL1;
							log->dw.I2 = log->w.IL2;
							log->dw.I3 = log->w.IL3;
							log->dw.Io2h = log->w.Io2h;
						}


						log->dw.dl_++;
					}
					break;
				case zapis:
					if (!log->dw.wsk_rej)
					{
						status_rej = oczekiwanie;
					}
					break;
			}

	}
}

void r_67n_lwn_20hz(void *arguments, void *logic)
{
	struct r_67n_lwn_logic *log = (struct r_67n_lwn_logic *)logic;

	// czesc ktora miala byc wykonywana max co 1s w wolnym czasie
	if(log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo
    {
		log->dw.R.max_I_ = sqrt(log->dw.max_I); // wyliczenie maksymalnej wartosci pradu
		log->dw.R.max_U_ = sqrt(log->dw.max_U); // wyliczenie wartosci napiecia
		log->dw.R.I1 = sqrt(log->dw.I1);
		log->dw.R.I2 = sqrt(log->dw.I2);
		log->dw.R.I3 = sqrt(log->dw.I3);
		log->dw.R.Io2h = sqrt(log->dw.Io2h);
        log->dw.R.max_kat_ = (180 / 3.14159f) * (atan2(log->dw.max_ortb_I,log->dw.max_orta_I) - atan2(log->dw.max_ortb_U,log->dw.max_orta_U));// wyliczenie kata
 //       log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia
 //       log->dw.R.dlugosc = (float)log->dw.dl_ * 0.01f; // / 100; // wpisanie czasu trwania zwarcia
  //      log->dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji
        log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia
    }

}