/*
 * RNT.c
 *
 *  Created on: 14-11-2018
 *      Author: Krzysztof Jakubczyk
 */

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

#include "RNT.h"
#include "analog_in.h"
/*#include "helper.h"
#include "ZDistL.h"
*/
#include <math.h>


int rnt_initlog(void *arguments, void *logic)
{
	struct rnt_args *args = (struct rnt_args *)arguments;
	struct rnt_logic *log = (struct rnt_logic *)logic;

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

	if(set_bit_ptr_struct(args->io.man_in,&log->man_))
		return -1;

	if(set_bit_ptr_struct(args->io.auto_in,&log->auto_))
		return -1;

	if(set_float_ptr(args->io.orta_U1_float_in,&log->orta_U1_))
		return -1;

	if(set_float_ptr(args->io.ortb_U1_float_in,&log->ortb_U1_))
		return -1;

	if(set_float_ptr(args->io.kwampl_U1_float_in,&log->kwampl_U1_))
		return -1;

	if(set_bit_ptr_struct(args->io.kon_U1_in,&log->kon_U1_))
		return -1;

	if(set_float_ptr(args->io.orta_I1_float_in,&log->orta_I1_))
		return -1;

	if(set_float_ptr(args->io.ortb_I1_float_in,&log->ortb_I1_))
		return -1;

	if(set_float_ptr(args->io.kwampl_I1_float_in,&log->kwampl_I1_))
		return -1;

	if(set_float_ptr(args->io.orta_U2_float_in,&log->orta_U2_))
		return -1;

	if(set_float_ptr(args->io.ortb_U2_float_in,&log->ortb_U2_))
		return -1;

	if(set_float_ptr(args->io.kwampl_U2_float_in,&log->kwampl_U2_))
		return -1;

	if(set_bit_ptr_struct(args->io.kon_U2_in,&log->kon_U2_))
		return -1;

	if(set_float_ptr(args->io.orta_I2_float_in,&log->orta_I2_))
		return -1;

	if(set_float_ptr(args->io.ortb_I2_float_in,&log->ortb_I2_))
		return -1;

	if(set_float_ptr(args->io.kwampl_I2_float_in,&log->kwampl_I2_))
		return -1;

	if(set_float_ptr(args->io.nr_zacz_in,&log->nr_zacz_))
		return -1;

	if(set_bit_ptr_struct(args->io.zo_zacz_in,&log->zo_zacz_))
		return -1;

	if(set_bit_ptr_struct(args->io.kon_u_nr_zacz_in,&log->kon_u_nr_zacz_))
		return -1;


	if(set_bit_ptr_struct(args->io.bieg_in,&log->bieg_))
		return -1;


	if(set_bit_ptr_struct(args->io.gora_in,&log->gora_))
		return -1;

	if(set_bit_ptr_struct(args->io.dol_in,&log->dol_))
		return -1;

	if(set_bit_ptr_struct(args->io.aktU1z_in,&log->aktU1z_))
		return -1;

	if(set_bit_ptr_struct(args->io.aktU2z_in,&log->aktU2z_))
		return -1;

	if(set_bit_ptr_struct(args->io.aktU3z_in,&log->aktU3z_))
		return -1;

	if(set_bit_ptr_struct(args->io.aktU4z_in,&log->aktU4z_))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_gora_in,&log->bl_gora_))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_dol_in,&log->bl_dol_))
		return -1;


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

	if(set_bit_ptr_struct(args->io.kas_in,&log->kas_))
		return -1;


	if(set_bit_ptr_struct(args->io.gora_out,&log->gora_out_))
		return -1;

	if(set_bit_ptr_struct(args->io.dol_out,&log->dol_out_))
		return -1;

	if(set_bit_ptr_struct(args->io.e_bieg_out,&log->e_bieg_out_))
		return -1;
	if(set_bit_ptr_struct(args->io.e_poz_out,&log->e_poz_out_))
		return -1;

	if(set_bit_ptr_struct(args->io.pob_G_out,&log->pob_G))
		return -1;

	if(set_bit_ptr_struct(args->io.pob_D_out,&log->pob_D))
		return -1;
	if(set_bit_ptr_struct(args->io.blok_out,&log->blok_out_))
		return -1;
	if(set_bit_ptr_struct(args->io.blok_trw_out,&log->blok_trw_out_))
		return -1;

	if(set_bit_ptr_struct(args->io.ster_poz_in,&log->ster_poz_))
		return -1;
	if(set_float_ptr(args->io.zad_zacz_in,&log->nr_zacz_zad))
		return -1;

	if(set_bit_ptr_struct(args->io.blok_trw_in,&log->blok_trw_))
		return -1;
	if(set_bit_ptr_struct(args->io.blad_trybu_out,&log->blad_trybu_out_))
		return -1;



	if(set_float_ptr(args->io.Uodn_float_out,&log->U_odn))
		return -1;
	if(set_float_ptr(args->io.tg_out,&log->tg_out))
		return -1;
	if(set_float_ptr(args->io.td_out,&log->td_out))
		return -1;


	if(set_pointer_in_ptr(args->io.I1_params_ptr_in,(u32 *)&log->I_params[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.I2_params_ptr_in,(u32 *)&log->I_params[1]))
		return -1;
	if(set_pointer_in_ptr(args->io.U1_params_ptr_in,(u32 *)&log->U_params[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.U2_params_ptr_in,(u32 *)&log->U_params[1]))
		return -1;


	////
	//status_rej = oczekiwanie;

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

	log->nast_.t1 = ((args->params.t1) * 1000); //czas dzialania t1
	log->nast_.t2 = ((args->params.t2) * 1000); //czas dzialania t2
	
	log->nast_.dUzr = args->params.dUz; //napicie nieczuoci
	log->nast_.dUzp = log->nast_.dUzr * args->params.kp; //napicie nieczuoci

	log->nast_.Uz1 = args->params.Uz1 ; //napicie zadane nr 1
	log->nast_.Uz2 = args->params.Uz2 ; //napicie zadane nr 2
	log->nast_.Uz3 = args->params.Uz3 ; //napicie zadane nr 3
	log->nast_.Uz4 = args->params.Uz4 ; //napicie zadane nr 4


	log->nast_.t_ster_zacz = ((args->params.t_ster_zacz) * 1000); //czas sterowania przecznikiem zaczepw
	log->nast_.t_poz_zacz = ((args->params.t_poz_zacz) * 1000); //czas odpowiedzi pozycji przecznika zaczepw
	log->nast_.t_imp_ster = ((args->params.t_imp_ster) * 1000);// czas impulsu gra/d
	log->nast_.on = ((args->params.akt & 0x0001)!=0) ? 1:0;
	log->nast_.kon_biegu = ((args->params.akt & 0x0002)!=0) ? 1:0;
	log->nast_.kon_poz_ster = ((args->params.akt & 0x0004)!=0) ? 1:0;
	log->nast_.akt_kon_U1 = ((args->params.akt & 0x0008)!=0) ? 1:0;
	log->nast_.akt_kon_U2 = ((args->params.akt & 0x0010)!=0) ? 1:0;
	log->nast_.akt_kon_Uzacz = ((args->params.akt & 0x0020)!=0) ? 1:0;
	log->nast_.akt_blok_biegu = ((args->params.akt & 0x0040)!=0) ? 1:0;
	log->nast_.akt_blok_pozycja = ((args->params.akt & 0x0080)!=0) ? 1:0;
	log->nast_.akt_mfazowe = ((args->params.akt & 0x0100)!=0) ? 1:0;

	log->nast_.akt_komp1 = ((args->params.akt_komp1 & 0x0001)!=0) ? 1:0;
	log->nast_.akt_komp2 = ((args->params.akt_komp2 & 0x0001)!=0) ? 1:0;


	log->_stan_man = CZEKANIE_NA_STEROWANIE_MANUAL;
	log->dw.blokada_trw = 0;
	log->dw.timer_blokad_pod_nap1 = 0;
	log->dw.timer_blokad_pod_nap2 = 0;

	log->mul[0]=(log->I_params[0]->znam_pierw / log->U_params[0]->znam_pierw);
	log->mul[1]=(log->I_params[1]->znam_pierw / log->U_params[1]->znam_pierw);
	
	log->nast_.R1 = args->params.R1 * log->mul[0];
	log->nast_.X1 = - (args->params.X1 * log->mul[0]);
	
	log->nast_.R2 = args->params.R2 * log->mul[1];
	log->nast_.X2 = - (args->params.X2 * log->mul[1]);
		
	return 0;
}


void rnt(void *arguments, void *logic)
{
	//struct rnt_args *args = (struct rnt_args *)arguments;
	struct rnt_logic *log = (struct rnt_logic *)logic;
	float local_orta_U1_ ,local_ortb_U1_,local_orta_U2_ ,local_ortb_U2_, local_kwa_U_, local_Uz;
	float local_U_;

	//u8 wk,wk2,l_nrl;
	u8 blok; //blokada 


//check_struct(&log->stan_bl_) - sprawdzenie blokady
//check_and_set_struct(log->dw.,&log->gora_out_);
//set_struct(&log->gora_out_);
//log->dw.go

    if(log->nast_.on)   // jesli zabezpieczenie aktywne
    {
		log->w.kwampl_U1_ = *log->kwampl_U1_;
		log->w.kwampl_U2_ = *log->kwampl_U2_;
		log->w.kwampl_I1_ = *log->kwampl_I1_;
		log->w.kwampl_I2_ = *log->kwampl_I2_;
		if (log->nast_.akt_mfazowe)
			{
			log->w.orta_U1_ = *log->ortb_U1_;
			log->w.orta_U2_ = *log->ortb_U2_;
			log->w.ortb_U1_ = -*log->orta_U1_;
			log->w.ortb_U2_ = -*log->orta_U2_;
			log->w.orta_I1_ = *log->orta_I1_ * 1.73f;
			log->w.orta_I2_ = *log->orta_I2_ * 1.73f;
			log->w.ortb_I1_ = *log->ortb_I1_ * 1.73f;
			log->w.ortb_I2_ = *log->ortb_I2_ * 1.73f;
			}
		else
			{
			log->w.orta_U1_ = *log->orta_U1_;
			log->w.orta_U2_ = *log->orta_U2_;
			log->w.ortb_U1_ = *log->ortb_U1_;
			log->w.ortb_U2_ = *log->ortb_U2_;
			log->w.orta_I1_ = *log->orta_I1_;
			log->w.orta_I2_ = *log->orta_I2_;
			log->w.ortb_I1_ = *log->ortb_I1_;
			log->w.ortb_I2_ = *log->ortb_I2_;
			}
		log->w.nr_zacz_ = *log->nr_zacz_;
		log->w.nr_zacz_zad = *log->nr_zacz_zad;

		//sprawdzenie czy mona sterowa w dol
		log->dw.blokada_ster_dol  = (check_struct(&log->bl_dol_))!=0 ? 1:0;
		//sprawdzenie czy mona sterowa w gore
		log->dw.blokada_ster_gora = (check_struct(&log->bl_gora_))!=0 ? 1:0;

		if (check_struct(&log->kas_))
			{
				//kasowanie
				clear_struct(&log->e_bieg_out_);
				clear_struct(&log->e_poz_out_);
			}

		if (check_struct(&log->kas_))
			{
				if (!check_struct(&log->blok_trw_))
				{
					log->dw.blokada_trw = 0;
				}
				
			}
		if (check_struct(&log->blok_trw_))
				{
				log->dw.blokada_trw = 1;

				}
		check_and_set_struct(log->dw.blokada_trw, &log->blok_trw_out_);
		
		
		if (check_struct(&log->man_) && (!check_struct(&log->ster_poz_)) && (!check_struct(&log->auto_)))
		{
			//tryb rczny
			clear_struct(&log->blad_trybu_out_);
			blok = check_struct(&log->stan_bl_);
			check_and_set_struct( blok, &log->blok_out_);
			//sprawdzenie blokady trwalej
			blok |= check_struct(&log->blok_trw_out_);
			
			clear_struct(&log->pob_G);
			clear_struct(&log->pob_D);
			*log->U_odn = 0;
			*log->tg_out = 0;
			*log->td_out = 0;
		}
		else
		if (!check_struct(&log->man_)&&(!check_struct(&log->ster_poz_))&&(check_struct(&log->auto_)))
		{
			//tryb automatyczny	
			clear_struct(&log->blad_trybu_out_);

			//sprawdzanie kompensacji linii 1
			if (log->nast_.akt_komp1)
				{
					local_orta_U1_ = log->w.orta_U1_ - ((log->w.orta_I1_ * log->nast_.R1 - log->w.ortb_I1_ * log->nast_.X1));
					local_ortb_U1_ = log->w.ortb_U1_ - ((log->w.orta_I1_ * log->nast_.X1 + log->w.ortb_I1_ * log->nast_.R1));
				}
				else
				{
					local_orta_U1_ = log->w.orta_U1_;
					local_ortb_U1_ = log->w.ortb_U1_;
				}
			//sprawdzanie kompensacji linii 2
			if (log->nast_.akt_komp2)
				{
					local_orta_U2_ = log->w.orta_U2_ - ((log->w.orta_I2_ * log->nast_.R2 - log->w.ortb_I2_ * log->nast_.X2));
					local_ortb_U2_ = log->w.ortb_U2_ - ((log->w.orta_I2_ * log->nast_.X2 + log->w.ortb_I2_ * log->nast_.R2));
				}
				else
				{
					local_orta_U2_ = log->w.orta_U2_;
					local_ortb_U2_ = log->w.ortb_U2_;
				}
			
			blok = check_struct(&log->stan_bl_);
			//sprawdzenie blokady trwalej
			blok |= check_struct(&log->blok_trw_out_);

			switch(log->nast_.tryb) ///< nastawa trybu dziaania 0-U1 1-U2 2-redniaU1/U2 3-maxU1/U2
				{
					case U1:
						//U1
						local_kwa_U_= (local_orta_U1_ * local_orta_U1_ + local_ortb_U1_* local_ortb_U1_);
						if (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1)
							log->dw.timer_blokad_pod_nap1 = 0;
						log->dw.timer_blokad_pod_nap1 +=LOOP_CYCLE_MS;;
						//blok |= (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1);
						blok |=(log->dw.timer_blokad_pod_nap1 < TP_OP_BLOKADY_NAP);
						break;
					case U2:
						//U2
						local_kwa_U_= (local_orta_U2_ * local_orta_U2_ + local_ortb_U2_* local_ortb_U2_);
						if (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2)
							log->dw.timer_blokad_pod_nap2 = 0;
						log->dw.timer_blokad_pod_nap2 +=LOOP_CYCLE_MS;;
						//blok |= (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1);
						blok |=(log->dw.timer_blokad_pod_nap2 < TP_OP_BLOKADY_NAP);
						//blok |= (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2);
						break;
					case sredniaU1U2:
						//redniaU1/U2
						local_kwa_U_ = (local_orta_U1_ * local_orta_U1_ + local_ortb_U1_* local_ortb_U1_);
						local_kwa_U_ += (local_orta_U2_ * local_orta_U2_ + local_ortb_U2_* local_ortb_U2_);
						local_kwa_U_ *= 0.5f;
						//blok |= (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1);
						//blok |= (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2);

						if (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1)
							log->dw.timer_blokad_pod_nap1 = 0;
						log->dw.timer_blokad_pod_nap1 +=LOOP_CYCLE_MS;;
						blok |=(log->dw.timer_blokad_pod_nap1 < TP_OP_BLOKADY_NAP);


						if (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2)
							log->dw.timer_blokad_pod_nap2 = 0;
						log->dw.timer_blokad_pod_nap2 +=LOOP_CYCLE_MS;;
						blok |=(log->dw.timer_blokad_pod_nap2 < TP_OP_BLOKADY_NAP);

						break;
					case maxU1U2:
						//max U1/U2 
						//blok |= (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1);
						//blok |= (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2);
						if (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1)
							log->dw.timer_blokad_pod_nap1 = 0;
						log->dw.timer_blokad_pod_nap1 +=LOOP_CYCLE_MS;;
						blok |=(log->dw.timer_blokad_pod_nap1 < TP_OP_BLOKADY_NAP);


						if (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2)
							log->dw.timer_blokad_pod_nap2 = 0;
						log->dw.timer_blokad_pod_nap2 +=LOOP_CYCLE_MS;;
						blok |=(log->dw.timer_blokad_pod_nap2 < TP_OP_BLOKADY_NAP);

						if ((log->w.kwampl_U1_)>(log->w.kwampl_U2_)) 
						{
						local_kwa_U_= (local_orta_U1_ * local_orta_U1_ + local_ortb_U1_* local_ortb_U1_);
						}
						else
						{
						local_kwa_U_= (local_orta_U2_ * local_orta_U2_ + local_ortb_U2_* local_ortb_U2_);
						}
						break;
					default:
						break;
				}
			//
			blok |= (check_struct(&log->kon_u_nr_zacz_) && log->nast_.akt_kon_Uzacz);
			check_and_set_struct( blok, &log->blok_out_);

			//w local_kwa_U_ - kwadrat napicia odniesienia
			local_kwa_U_ *= 0.5f;
			local_U_ = sqrtf (local_kwa_U_);
			*log->U_odn = local_kwa_U_;
			
			//Wybr napicia Uz - napicie zadane
			if (check_struct(&log->aktU1z_))
				local_Uz = log->nast_.Uz1;
				else
				if (check_struct(&log->aktU2z_))
					local_Uz = log->nast_.Uz2;
					else
					if (check_struct(&log->aktU3z_))
						local_Uz = log->nast_.Uz3;
						else
						if (check_struct(&log->aktU4z_))
							local_Uz = log->nast_.Uz4;
							else
							local_Uz = log->nast_.Uz1;
			//porownanie war pobudzenia
			//local_Uz - local_kwa_U_ ;
			sprawdz_P(&log->dw.pob_D,
					  ((local_Uz - local_U_ ) < (- log->nast_.dUzr)),
					  ((local_Uz - local_U_ ) > (- log->nast_.dUzp)),
					  &log->dw.lp_D,
					  100,100
					  );
			sprawdz_P(&log->dw.pob_G,
					  ((local_Uz - local_U_ ) > log->nast_.dUzr),
					  ((local_Uz - local_U_ ) < log->nast_.dUzp),
					  &log->dw.lp_G,
					  100,100
					  );
			check_and_set_struct((log->dw.pob_G > 0)&&(!blok)&&(!log->dw.blokada_ster_gora),&log->pob_G);
			check_and_set_struct((log->dw.pob_D > 0)&&(!blok)&&(!log->dw.blokada_ster_dol),&log->pob_D);
		}
		else
		if (!check_struct(&log->man_)&&(check_struct(&log->ster_poz_))&&(!check_struct(&log->auto_)))
			{
			//tryb nr zaczepu
			clear_struct(&log->blad_trybu_out_);
			//jesli zadany jest nr zaczepu 
			blok = check_struct(&log->stan_bl_);
			blok |= (check_struct(&log->kon_u_nr_zacz_) && log->nast_.akt_kon_Uzacz);
			check_and_set_struct( blok, &log->blok_out_);
			//sprawdzenie blokady trwalej
			blok |= check_struct(&log->blok_trw_out_);
		
			//(check_struct(&log->ster_poz_)) == 1
			if (((log->w.nr_zacz_zad) < (log->w.nr_zacz_)) && (!log->dw.blokada_ster_dol) && (!blok))
				{
					log->dw.pob_D = 1;
					log->dw.pob_G = 0;
				}
				else
				if (((log->w.nr_zacz_zad) > (log->w.nr_zacz_)) && (!log->dw.blokada_ster_gora) && (!blok))
				{
					log->dw.pob_D = 0;
					log->dw.pob_G = 1;
				}
				else
				{
					log->dw.pob_D = 0;
					log->dw.pob_G = 0;
				}

			check_and_set_struct((log->dw.pob_G > 0)&&(!blok)&&(!log->dw.blokada_ster_gora) ,&log->pob_G);
			check_and_set_struct((log->dw.pob_D > 0)&&(!blok)&&(!log->dw.blokada_ster_dol) ,&log->pob_D);
			}
			else
			{
				//blad wyboru rodzaju pracy
				clear_struct(&log->pob_D);
				clear_struct(&log->pob_G);
				set_struct( &log->blok_out_);
				set_struct(&log->blad_trybu_out_);

			}
		//
		
		switch (log->_stan_man)
			{
				case CZEKANIE_NA_STEROWANIE_MANUAL:
					if (!check_struct(&log->man_))
					{
						//sprawdzenie czy nie przeczy na AUTO
						log->_stan_man = CZEKANIE_NA_STEROWANIE_AUTO;
						log->licz_t1G = log->licz_t2G = 0;
						log->licz_t1D = log->licz_t2D = 0;
					}
					else
					{
					if(check_struct(&log->gora_) && (!check_struct(&log->dol_)) && (!log->dw.blokada_ster_gora) && (!blok))
					{
						*log->tg_out = 0;
						*log->td_out = 0;
						log->licznik_ster_gora+=LOOP_CYCLE_MS;
						if (log->licznik_ster_gora > T_STER_RECZNEGO)					
						{
							log->_stan_man = STEROWANIE_W_GORE;
							log->licznik_ster_gora =0;
						}
						log->licznik_ster_dol =0;
					}
						else
						//log->dw.pob_G;
					if(check_struct(&log->dol_) && (!check_struct(&log->gora_)) && (!log->dw.blokada_ster_dol) && (!blok))
					{
						log->licznik_ster_dol+=LOOP_CYCLE_MS;
						if (log->licznik_ster_dol > T_STER_RECZNEGO)					
						{
						log->_stan_man = STEROWANIE_W_DOL;
						log->licznik_ster_dol =0;
						}
						log->licznik_ster_gora =0;
					}
					else
					{
						log->licznik_ster_dol = 0;
						log->licznik_ster_gora = 0;
						clear_struct(&log->gora_out_);
						clear_struct(&log->dol_out_);

					}
					}
					break;
				case CZEKANIE_NA_STEROWANIE_NR_POZYCJI:
					if (!check_struct(&log->ster_poz_))
					{
						//sprawdzenie czy nie przeczy na AUTO
						log->_stan_man = CZEKANIE_NA_STEROWANIE_AUTO;
						log->licz_t1G = log->licz_t2G = 0;
						log->licz_t1D = log->licz_t2D = 0;
					}
					else
					{
					if(log->dw.pob_G && (!log->dw.pob_D) && (!log->dw.blokada_ster_gora) && (!blok))
					{
					//(check_struct(&log->ster_poz_)) == 1
									//gora nr pozycji
									if (log->licz_t2G < log->nast_.t2)
									{
										log->licz_t2G += LOOP_CYCLE_MS;
									}
									else
									{
									log->_stan_man = STEROWANIE_W_GORE;
									log->licz_t2G = 0;
									}

					*log->tg_out = (float)(log->nast_.t2 - log->licz_t2G) * 0.001f;
					*log->td_out = 0;

					}
					else
					if((log->dw.pob_D) && (!log->dw.pob_G) && (!log->dw.blokada_ster_dol) && (!blok))
					{
									//dol nr pozycji
									if (log->licz_t2D < log->nast_.t2)
									{
										log->licz_t2D += LOOP_CYCLE_MS;
									}
									else
									{
									log->_stan_man = STEROWANIE_W_DOL;
									log->licz_t2D = 0;
									}
					*log->td_out = (float)(log->nast_.t2 - log->licz_t2D) * 0.001f;
					*log->tg_out = 0;
					}
					}
					break;
				case CZEKANIE_NA_STEROWANIE_AUTO:
					if (check_struct(&log->man_))
					{
						//sprawdzenie czy nie przeczy na MANUAL
						log->_stan_man = CZEKANIE_NA_STEROWANIE_MANUAL;
					}
					else
					if (check_struct(&log->ster_poz_))
					{
						//sprawdzenie czy nie przeczy na NR_POZ
						log->_stan_man = CZEKANIE_NA_STEROWANIE_NR_POZYCJI;
					}
					else
					if (log->dw.pob_G && (!log->dw.pob_D) && (!log->dw.blokada_ster_gora)&& (!blok))
					{
						//gora auto
						if (log->licz_t1G < log->nast_.t1)
						{
							log->licz_t1G += LOOP_CYCLE_MS * ((local_Uz - local_U_ ) / log->nast_.dUzr);
						}
						else
						if (log->licz_t2G < log->nast_.t2)
						{
							log->licz_t2G += LOOP_CYCLE_MS;
						}
						else
						{
						log->_stan_man = STEROWANIE_W_GORE;
						log->licz_t1G = log->licz_t2G = 0;
						}
					*log->tg_out = (float)((log->nast_.t1 + log->nast_.t2 - log->licz_t1G - log->licz_t2G) * 0.001f);
					*log->td_out = 0;
					}
						else
					if(log->dw.pob_D && (!log->dw.pob_G) && (!log->dw.blokada_ster_dol)&& (!blok))
					{
						//dol auto
						if (log->licz_t1D < log->nast_.t1)
						{
							log->licz_t1D += LOOP_CYCLE_MS *((local_Uz - local_U_ ) / -log->nast_.dUzr);
						}
						else
						if (log->licz_t2D < log->nast_.t2)
						{
							log->licz_t2D += LOOP_CYCLE_MS;
						}
						else
						{
						log->_stan_man = STEROWANIE_W_DOL;
						log->licz_t1D = log->licz_t2D = 0;
						}
					*log->td_out = (log->nast_.t1 + log->nast_.t2 - log->licz_t1D - log->licz_t2D) * 0.001f;
					*log->tg_out = 0;
					}
					else
					{
						*log->td_out = 0;
						*log->tg_out = 0;
						log->licz_t1G = log->licz_t2G = 0;
						log->licz_t1D = log->licz_t2D = 0;
						clear_struct(&log->gora_out_);
						clear_struct(&log->dol_out_);
					}
					break;
			//gora
				case STEROWANIE_W_GORE:
						*log->td_out = 0;
						*log->tg_out = 0;
						set_struct(&log->gora_out_);
						log->_stan_man = IMPULS_W_GORE;
						log->licznik_ster_biegu = 0;
						log->licznik_ster_gora = 0;
						log->nr_zaczepu_przed_ster = log->w.nr_zacz_;
						clear_struct(&log->e_bieg_out_);
						clear_struct(&log->e_poz_out_);
						log->dw.byl_bieg = 0;

					break;
				case IMPULS_W_GORE:
					log->licznik_ster_gora+=LOOP_CYCLE_MS;
					if ((log->licznik_ster_gora > log->nast_.t_imp_ster))
					{
						log->licznik_ster_gora = 0;
						log->licznik_poz_gora = 0;
						log->_stan_man = SPRAWDZENIE_ODP_PRZE_ZACZEPOW_GORA;
						clear_struct(&log->gora_out_);
						if ((log->nast_.kon_biegu) && (!(log->dw.byl_bieg & 0x1 >0) ))
								set_struct(&log->e_bieg_out_);
					}
					else
					{
						if ((log->nast_.kon_biegu) && (check_struct(&log->bieg_)))
						{
							log->dw.byl_bieg |=1;
							log->licznik_ster_biegu+=LOOP_CYCLE_MS;
							if (((log->licznik_ster_biegu > log->nast_.t_ster_zacz)))
								{
									set_struct(&log->e_bieg_out_);
								}
						}


					}
					
					break;
				case SPRAWDZENIE_ODP_PRZE_ZACZEPOW_GORA:
				//sprawdzenie odpowiedzi przecznika zaczepw tj.  czas max, zmiana nr +1, itp
				//trzeba si zastanowic jak kontrolowa sygna BIEG_PRZE_ZACZ??
					if ((((log->nast_.kon_biegu)&&((!check_struct(&log->bieg_))&&(log->dw.byl_bieg & 0x01))||(check_struct(&log->e_bieg_out_))||(!(log->nast_.kon_biegu)))) && (!check_struct(&log->zo_zacz_)))
						{
							log->_stan_man = KONIEC_STEROWANIA;
							if ((log->nast_.kon_poz_ster)&&(log->w.nr_zacz_ <= log->nr_zaczepu_przed_ster))							
							{
								set_struct(&log->e_poz_out_);
							}
						}
						else
						{
						log->licznik_ster_biegu+=LOOP_CYCLE_MS;
						if (((log->licznik_ster_biegu > log->nast_.t_ster_zacz))&&(check_struct(&log->bieg_))&&(log->nast_.kon_biegu))
							{
								log->_stan_man = KONIEC_STEROWANIA;
								set_struct(&log->e_bieg_out_);
							}
						}
					break;
			//dol		
				case STEROWANIE_W_DOL:
						*log->td_out = 0;
						*log->tg_out = 0;
						set_struct(&log->dol_out_);
						log->_stan_man = IMPULS_W_DOL;
						log->licznik_ster_dol = 0;
						log->licznik_ster_biegu = 0;
						log->nr_zaczepu_przed_ster = log->w.nr_zacz_;
						clear_struct(&log->e_bieg_out_);
						clear_struct(&log->e_poz_out_);
						log->dw.byl_bieg =0;
					break;
				case IMPULS_W_DOL:
					{
					log->licznik_ster_dol+=LOOP_CYCLE_MS;
					if ((log->licznik_ster_dol > log->nast_.t_imp_ster))
					{
						log->licznik_ster_gora = 0;
						log->licznik_poz_gora = 0;
						log->_stan_man = SPRAWDZENIE_ODP_PRZE_ZACZEPOW_DOL;
						clear_struct(&log->dol_out_);
						if ((log->nast_.kon_biegu) && (!(log->dw.byl_bieg & 0x1 > 0)))
								set_struct(&log->e_bieg_out_);
					}
					else
					{
						if ((log->nast_.kon_biegu) && (check_struct(&log->bieg_)))
						{
							 log->dw.byl_bieg |=1;
							 log->licznik_ster_biegu+=LOOP_CYCLE_MS;
							 if (((log->licznik_ster_biegu > log->nast_.t_ster_zacz)))
									{
										set_struct(&log->e_bieg_out_);
									}
						}


					}
					}
					
					break;
				case SPRAWDZENIE_ODP_PRZE_ZACZEPOW_DOL:
				//sprawdzenie odpowiedzi przecznika zaczepw tj.  czas max, zmiana nr +1, itp
					if ((((log->nast_.kon_biegu)&&((!check_struct(&log->bieg_))&&(log->dw.byl_bieg & 0x01))||(check_struct(&log->e_bieg_out_))||(!(log->nast_.kon_biegu)))) && (!check_struct(&log->zo_zacz_)))
					//if ((log->nast_.kon_biegu)&&(!check_struct(&log->bieg_))||(!(log->nast_.kon_biegu)))
						{
							log->_stan_man = KONIEC_STEROWANIA;
							if ((log->nast_.kon_poz_ster)&&(log->w.nr_zacz_ >= log->nr_zaczepu_przed_ster))							
							{
								set_struct(&log->e_poz_out_);
							}
						}
						else
						{
						log->licznik_ster_biegu+=LOOP_CYCLE_MS;
						if (((log->licznik_ster_biegu > log->nast_.t_ster_zacz))&&(check_struct(&log->bieg_))&&(log->nast_.kon_biegu))
							{
								log->_stan_man = KONIEC_STEROWANIA;
								set_struct(&log->e_bieg_out_);
							}
						}
					break;
				case KONIEC_STEROWANIA:
					if (check_struct(&log->man_))
						{
							//sprawdzenie czy nie przeczy na MANUAL
							log->_stan_man = CZEKANIE_NA_STEROWANIE_MANUAL;
						}
					else
						if (check_struct(&log->ster_poz_))
						{
							//sprawdzenie czy nie przeczy na NR_POZ
							log->_stan_man = CZEKANIE_NA_STEROWANIE_NR_POZYCJI;
						}
					else
						{
							//sprawdzenie czy nie przeczy na AUTO
							log->_stan_man = CZEKANIE_NA_STEROWANIE_AUTO;
							log->licz_t1G = log->licz_t2G = 0;
							log->licz_t1D = log->licz_t2D = 0;
						}
					break;

				
				default:
					log->_stan_man = CZEKANIE_NA_STEROWANIE_MANUAL;
					break;
				
			}
	
	}
	else
	{
		clear_struct(&log->gora_out_);
		clear_struct(&log->dol_out_);
		clear_struct(&log->e_bieg_out_);
		clear_struct(&log->e_poz_out_);
		clear_struct(&log->pob_G);
		clear_struct(&log->pob_D);
		*log->U_odn = 0;
	}
}

void rnt_100hz(void *arguments, void *logic)
{
	struct rnt_logic *log = (struct rnt_logic *)logic;
//	int t;

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

void rnt_20hz(void *arguments, void *logic)
{
	//struct rnt_logic *log = (struct rnt_logic *)logic;

}