/*
 * LMZ.c
 *
 *  Created on: 23-05-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "LMZ.h"


int LMZ_initlog(void *arguments, void *logic)
{
	struct LMZ_args *args = (struct LMZ_args *)arguments;
	struct LMZ_logic *log = (struct LMZ_logic *)logic;
	struct analog_in_params *pwi,*pwu,*pwio2;

	if(set_pointer_in_ptr(args->io.we_zdist_ptr_in,(u32 *)&log->nast_.WYA))
		return -1;

	if(set_bit_ptr_struct(args->io.we_zdistl_p1_in,&log->WE_ZDistL_Pzf[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.we_zdistl_p1w_in,&log->WE_ZDistL_Pzf[1]))
		return -1;

	if(set_float_ptr(args->io.ioii_orta_float_in,&log->IoII_orta))
		return -1;
	if(set_float_ptr(args->io.ioii_ortb_float_in,&log->IoII_ortb))
		return -1;
	if(set_pointer_in_ptr(args->io.ioii_params_ptr_in,(u32 *)&log->IoII_params))
		return -1;

	if(set_bit_ptr_struct(args->io.clr_in,&log->clr))
		return -1;
	if(set_bit_ptr_struct(args->io.p_out,&log->P))
		return -1;
	if(set_float_ptr(args->io.distance_float_out,&log->distance))
		return -1;
 
	#ifdef LMZ_DEBUG
	if(set_float_ptr(args->io.debug_out,&log->debug))
		return -1;
	if(set_float_ptr(args->io.debug1_out,&log->debug1))
		return -1;   
	if(set_float_ptr(args->io.debug2_out,&log->debug2))
		return -1;    
 	if(set_float_ptr(args->io.debug3_out,&log->debug3))
		return -1;   
	if(set_float_ptr(args->io.debug4_out,&log->debug4))
		return -1; 
	#endif
  log->nast_.on_ = (args->params.bity & 0x0001) ? 1 : 0;
	log->nast_.L2t = (args->params.bity & 0x0002) ? 1 : 0;


	pwu = log->nast_.WYA->Zdist_dw->log_ptr->param_U;
	pwi = log->nast_.WYA->Zdist_dw->log_ptr->param_I;
	pwio2 = log->IoII_params;


	log->nast_.a.r = args->params.R1 * pwi->znam_pierw / pwu->znam_pierw;
	log->nast_.a.i = -args->params.X1 * pwi->znam_pierw / pwu->znam_pierw;
	log->nast_.b.r = (args->params.R0 - args->params.R1) * pwi->znam_pierw / pwu->znam_pierw;
	log->nast_.b.i = -(args->params.X0 - args->params.X1) * pwi->znam_pierw / pwu->znam_pierw;
	log->nast_.c.r = args->params.Rm * pwio2->znam_pierw / pwu->znam_pierw;
	log->nast_.c.i = -args->params.Xm * pwio2->znam_pierw / pwu->znam_pierw;

  log->nast_.kziem.r = ((args->params.R0 / args->params.R1)-1)/3;
  log->nast_.kziem.i = -((args->params.X0 / args->params.X1)-1)/3;

	if(log->nast_.L2t==0)
	{
		log->nast_.c.r = 0;
		log->nast_.c.i = 0;
	}


	return 0;
}

/**
Wylicza iloczyn dwoch liczb zespolonych
*/
struct zespolona p_z(struct zespolona *a,
					 struct zespolona *b)
{
	struct zespolona result;
	result.r = a->r * b->r - a->i * b->i;
	result.i = a->r * b->i + a->i * b->r;
	return(result);
}

/**
Wylicza iloczyn dwoch liczb zespolonych
*/
struct zespolona dziel(struct zespolona *a,
					 struct zespolona *b)
{
	struct zespolona result;
	result.r = ((a->r * b->r) + (a->i * b->i))/((b->r * b->r) +(b->i * b->i));
	result.i = ((a->i * b->r) - (a->r * b->i))/((b->r * b->r) +(b->i * b->i));
	return(result);
}

/**
Wylicza dane dla odlegoci w ptli jednofazowej
*/
float
wylicz_dane_1f(struct LMZ_logic *log,
	u16 nr_pr,	//indeks pooenia wektora wyliczanych danych w buforze
	u16 nr_I //numer prdu wejciowego
)
{
	struct zespolona temp;
	struct zespolona loc1,loc2,loc3;
	struct zespolona *i1,*ip;
	float loc4;

  struct zespolona imped;
  struct zespolona k3io;


	i1 = &log->dw.dp[nr_pr].IL1 + nr_I;
	ip = &log->dw.Im[nr_pr % (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)].IL1 + nr_I;

	loc4 = (float)1.0 / (i1->r * i1->r + i1->i * i1->i);
	loc1.r = i1->r * loc4;
	loc1.i = (-i1->i) * loc4;

	loc2 = p_z(&log->dw.dp[nr_pr].Io,&log->nast_.b);
	loc3 = p_z(&log->dw.dp[nr_pr].Io2,&log->nast_.c);
	loc2.r += loc3.r;
	loc2.i += loc3.i;
	loc3 = p_z(&loc2,&loc1);
	log->dw.zz.r = log->nast_.a.r - loc3.r;
	log->dw.zz.i = log->nast_.a.i - loc3.i;

	log->dw.zi = *i1;

	log->dw.zibis.r = i1->r - ip->r;
	log->dw.zibis.i = i1->i - ip->i;
 
  float ignac = 1;

	temp = p_z(&log->dw.zz,&log->dw.zi);
	
  float wynik = ( ignac * (log->dw.zu.i * log->dw.zibis.r - log->dw.zu.r * log->dw.zibis.i )
			/ (temp.i * log->dw.zibis.r - temp.r * log->dw.zibis.i ));
      
  return wynik;

}


/**
Wylicza dane dla odlegoci w ptli midzyfazowej
*/
float wylicz_dane_mf(struct LMZ_logic *log,
	u16 nr_pr,	//indeks pooenia wektora wyliczanych danych w buforze
	u16 nr_I1, //numer prdu wejciowego 1
	u16 nr_I2 //numer prdu wejciowego 2
)
{
	struct zespolona temp;
	struct zespolona *i1,*ip1;
	struct zespolona *i2,*ip2;

	i1 = &log->dw.dp[nr_pr].IL1 + nr_I1;
	ip1 = &log->dw.Im[nr_pr % (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)].IL1 + nr_I1;
	i2 = &log->dw.dp[nr_pr].IL1 + nr_I2;
	ip2 = &log->dw.Im[nr_pr % (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)].IL1 + nr_I2;

	log->dw.zz = log->nast_.a;

	log->dw.zi.r = i1->r - i2->r;
	log->dw.zi.i = i1->i - i2->i;

	log->dw.zibis.r = i1->r - i2->r - ip1->r + ip2->r;
	log->dw.zibis.i = i1->i - i2->i - ip1->i + ip2->i;

	temp = p_z(&log->dw.zz,&log->dw.zi);

	return( (log->dw.zu.i * log->dw.zibis.r - log->dw.zu.r * log->dw.zibis.i )
			/ (temp.i * log->dw.zibis.r - temp.r * log->dw.zibis.i ));

}

void zapisz_p(struct LMZ_logic *log)
{
	float *wsk;
	int i;

	log->dw.wsk_buf++;

	for(i=0;i<6;i++)
	{
		log->dw.P[log->dw.wsk_buf][i] = (log->nast_.WYA->Zdist_dw->P[0][i] || log->nast_.WYA->Zdist_dw->P[1][i])?1:0;
	}

	wsk = &log->dw.dp[log->dw.wsk_buf].IL1.r;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I1_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I1_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I2_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I2_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I3_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I3_ortb;
	wsk++;
	*wsk = -*log->nast_.WYA->Zdist_dw->log_ptr->sI0_orta;//poprawiono
	wsk++;
	*wsk = -*log->nast_.WYA->Zdist_dw->log_ptr->sI0_ortb;//poprawiono
	wsk++;
	*wsk = *log->IoII_orta;
	wsk++;
	*wsk = *log->IoII_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U1_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U1_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U2_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U2_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U3_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U3_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U12_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U12_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U23_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U23_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U31_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U31_ortb;
}

void zapisz_pre(struct LMZ_logic *log)
{
	float *wsk;

	wsk = &log->dw.Ip[log->dw.wsk_buf_pre].IL1.r;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I1_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I1_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I2_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I2_ortb;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I3_orta;
	wsk++;
	*wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I3_ortb;

	log->dw.wsk_buf_pre++;
	if(log->dw.wsk_buf_pre>MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3-1) log->dw.wsk_buf_pre = 0;
}


void LMZ(void *arguments, void *logic)
{
	struct LMZ_logic *log = (struct LMZ_logic *)logic;
	u16 local;
	int k;

    if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {
		zapisz_pre(log);
		switch(log->status_rej_LMZ)
		{
			case oczekiwanie:

				if(check_struct(&log->WE_ZDistL_Pzf[0]) || check_struct(&log->WE_ZDistL_Pzf[1]))
				{
					log->status_rej_LMZ = sledzenie;// stan_nieustalony;
					log->dw.wsk_buf = 0; //dodane
					zapisz_p(log);//dodane

					//dw.czas_ = time;
					log->dw.licz1 = 10;//20;
					          
          if (log->dw.wsk_buf_pre == 0)
            local = MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3-2;
          else if (log->dw.wsk_buf_pre == 1)
            local = MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3-1;
          else
            local = log->dw.wsk_buf_pre - 2 ;
          
           
					for(k=0;k<MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS;k++)
					{
						if (local>MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3-1) local -= MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3;
						log->dw.Im[k] = log->dw.Ip[local];
						local++;
					}
				}
				break;

			case stan_nieustalony:
				if(log->dw.licz1>LOOP_CYCLE_MS)
					log->dw.licz1-=LOOP_CYCLE_MS;
				else
					log->dw.licz1=0;

				if(!log->dw.licz1)
				{
					if(check_struct(&log->WE_ZDistL_Pzf[0]) || check_struct(&log->WE_ZDistL_Pzf[1]))
					{
						log->status_rej_LMZ = sledzenie;
						log->dw.wsk_buf = 0;
						zapisz_p(log);
						log->dw.licz1 = 10;
					} else {
						log->status_rej_LMZ = oczekiwanie;
					}
				}
				break;

			case sledzenie:

				zapisz_p(log);


				if (log->dw.wsk_buf==MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*5-1)
				{
					log->status_rej_LMZ = blokada;
					log->dw.wsk_rej = 1;
				} else {
					if(!check_struct(&log->WE_ZDistL_Pzf[0]) && !check_struct(&log->WE_ZDistL_Pzf[1]))
					{
						if(log->dw.licz1>LOOP_CYCLE_MS)
							log->dw.licz1-=LOOP_CYCLE_MS;
						else
							log->dw.licz1=0;
					} else {
						log->dw.licz1 = 10;
					}
					if(!log->dw.licz1)
					{
						if (log->dw.wsk_buf<MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)
						{
							log->status_rej_LMZ = oczekiwanie;
						} else {
							log->status_rej_LMZ = blokada;
							log->dw.wsk_rej = 1;
						}

					}

				}
				break;

			case blokada:
				if (!log->dw.wsk_rej && !(check_struct(&log->WE_ZDistL_Pzf[0]) || check_struct(&log->WE_ZDistL_Pzf[1])))
				{
					log->status_rej_LMZ = oczekiwanie;
				}
				break;

			default:
			 	break;
		}
	}
}

void LMZ_20hz(void *arguments, void *logic)
{
	struct LMZ_logic *log = (struct LMZ_logic *)logic;
	u16 licz[6], temp, temp1;
	u16 czas_mf, czas_1f, zwarcie_mf, zwarcie_1f, roznica;
	u16 czas_4, czas_5, czas_6;
	float local;
	u16 k;
	int i;
	u8 startbuf = 0;
	float sr1, sr2, sr3, iloraz;

	if(log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli zebra dostatecznie wielk ilo danych
    {
		//wyliczanie petli dla strefy 1
   
		licz[0]=0;
		licz[1]=0;
		licz[2]=0;
		licz[3]=0;
		licz[4]=0;
		licz[5]=0;

		for(i=0;i<log->dw.wsk_buf;i++)
		{
			//okrelenie ktra ptla bya pobudzona najduej (kryterium wyboru)
			for(k=0;k<6;k++)
			{
				if(log->dw.P[i][k]) licz[k]++;
			}
		}
		temp = 0;
		temp1 = 0;  
    	czas_1f = 0;
		czas_mf = 0;
		zwarcie_mf = 0;
		zwarcie_1f = 0;
   
    	//zwarcia miedzyfazowe
    	for(k=3;k<6;k++)
		{
			if(licz[k]>temp1)
			{
				temp1 = licz[k];
				temp = k+1;
        		czas_mf = temp1;
        		zwarcie_mf = k+1;
			}
		}
 
    	czas_4 = licz[3];
    	czas_5 = licz[4];
    	czas_6 = licz[5];
 
    	//zwarcia jednofazowe
    	for(k=0;k<3;k++)
		{
			if(licz[k]>czas_1f)
			{
          		czas_1f = licz[k];
          		zwarcie_1f = k+1;
      		}
    	}
       	
    	for(k=0;k<3;k++)
		{
			if(licz[k]>temp1)
			{
				temp1 = licz[k];
				temp = k+1;
			}
		}
       
		if (czas_1f > czas_mf)
    	{
			roznica = czas_1f - czas_mf;
		}
    	else
    	{
			roznica = czas_mf - czas_1f;
    	}
    
		//wyznaczanie przedzialu z ktorego beda wyioczane dane
		if(log->dw.wsk_buf<25)
		{
			startbuf=0;
			log->dw.wsk_buf-=(log->dw.wsk_buf-20);
		}
		else if(log->dw.wsk_buf>=25 && log->dw.wsk_buf<35)
		{
			startbuf=(log->dw.wsk_buf-25);
			log->dw.wsk_buf-=5;
		}
		else if(log->dw.wsk_buf>=35 && log->dw.wsk_buf<40)
		{
			startbuf=5;
			log->dw.wsk_buf-=10;
		}
		else if(log->dw.wsk_buf>=40 && log->dw.wsk_buf<50)
		{
			startbuf=10;
			log->dw.wsk_buf-=10;
		}
		else if(log->dw.wsk_buf>=50 && log->dw.wsk_buf<=60)
		{
			startbuf=15;
			log->dw.wsk_buf-=15;
		}
		else
		{
			startbuf=20;
			log->dw.wsk_buf-=20;
		}

    	sr1 = 0;
    	sr2 = 0;
    	sr3 = 0; 
    
		//wyliczenie sredniej wartosci pradow w oknie zwarcia
		for(i=0;i<(log->dw.wsk_buf - startbuf);i++)
		{
			float r = log->dw.dp[i+startbuf].IL1.r;
			float ii = log->dw.dp[i+startbuf].IL1.i;
			float esk = sqrt((r * r + ii * ii) * 0.5);
			sr1 +=esk;
      
			r = log->dw.dp[i+startbuf].IL2.r;
			ii = log->dw.dp[i+startbuf].IL2.i;
			esk = sqrt((r * r + ii * ii) * 0.5);
			sr2 +=esk;
      
			r = log->dw.dp[i+startbuf].IL3.r;
			ii = log->dw.dp[i+startbuf].IL3.i;
			esk = sqrt((r * r + ii * ii) * 0.5);
			sr3 +=esk;
		}
		sr1 = sr1 / (log->dw.wsk_buf - startbuf);  
		sr2 = sr2 / (log->dw.wsk_buf - startbuf);
		sr3 = sr3 / (log->dw.wsk_buf - startbuf);
  
		float procent = 1;
      
		#ifdef LMZ_DEBUG
		*log->debug1 = (float)czas_1f;
		*log->debug2 = (float)czas_4;
		*log->debug3 = (float)czas_5;
		*log->debug4 = (float)czas_6;
		#endif
  
		float d1 = 1;
		float d2 = 1;

		//gdy zwarcie jest miedzyfazowe i sa pobudzone jednoczesnie dwie petle, wybierz wlasciwa
		//na podstawie pradow
		if (czas_mf > 20/LOOP_CYCLE_MS) //zwarcie miedzyfazowe
		{
        	if ((czas_4 > 20/LOOP_CYCLE_MS) && (czas_5 > 20/LOOP_CYCLE_MS) && (czas_6 > 20/LOOP_CYCLE_MS))
        	{
        	//zwarcie 3 fazowe nic nie rob
			}
			else
			{
				if ((czas_4 > 20/LOOP_CYCLE_MS) && (czas_5 > 20/LOOP_CYCLE_MS))  //L1L2 i L2L3
				{
					if (czas_4 > czas_5)
	                	procent = (float)czas_5 / (float)czas_4;
					else
						procent = (float)czas_4 / (float)czas_5;
	            
	            	if (procent > 0.7)
					{
						float iloraz12 = 1;
						float iloraz23 = 1;                    
						d1 = sr1;
						d2 = sr2;                       
						if (d1 > d2)
							iloraz12 = d1 / d2;
						else
							iloraz12 = d2 / d1;
	                
						d1 = sr2;
						d2 = sr3;                                
	              		if (d1 > d2)
							iloraz23 = d1 / d2;
						else
							iloraz23 = d2 / d1;
	                
						if (iloraz12 > iloraz23)
						{
							temp = 5;
							zwarcie_mf = 5;
						}
						else
						{
							temp = 4;
							zwarcie_mf = 4;
						}
					}
				}
	               
				if ((czas_4 > 20/LOOP_CYCLE_MS) && (czas_6 > 20/LOOP_CYCLE_MS))  //L1L2 i L3L1
				{
					if (czas_4 > czas_6)
						procent = (float)czas_6 / (float)czas_4;
					else
						procent = (float)czas_4 / (float)czas_6;
	            
					if (procent > 0.7)
					{
						float iloraz12 = 1;
						float iloraz31 = 1;
		   				d1 = sr1;
						d2 = sr2;
		   				if (d1 > d2)
							iloraz12 = d1 / d2;
						else
							iloraz12 = d2 / d1;
		                
						d1 = sr3;
						d2 = sr1;                                
						if (d1 > d2)
							iloraz31 = d1 / d2;
						else
							iloraz31 = d2 / d1;
		                
						if (iloraz12 > iloraz31)
						{
							temp = 6;
							zwarcie_mf = 6;
						}
						else
						{
							temp = 4;
							zwarcie_mf = 4;
						}
					}
				}
	       
				if ((czas_5 > 20/LOOP_CYCLE_MS) && (czas_6 > 20/LOOP_CYCLE_MS))  //L2L3 i L3L1
				{
					if (czas_5 > czas_6)
						procent = (float)czas_6 / (float)czas_5;
					else
						procent = (float)czas_5 / (float)czas_6;
		            
					if (procent > 0.7)
					{
						float iloraz23 = 1;
						float iloraz31 = 1;
		                     
						d1 = sr2;
						d2 = sr3;
		                       
						if (d1 > d2)
							iloraz23 = d1 / d2;
						else
							iloraz23 = d2 / d1;
		                
						d1 = sr3;
						d2 = sr1;
		                                
						if (d1 > d2)
							iloraz31 = d1 / d2;
						else
							iloraz31 = d2 / d1;
		                
						if (iloraz23 > iloraz31)
						{
							temp = 6;
							zwarcie_mf = 6;
						}
						else
						{
							temp = 5;
							zwarcie_mf = 5; 
						}
					}
				}      
			}    
		}
           
		if (zwarcie_mf == 4)
		{
			d1 = sr1;
			d2 = sr2;
		}
		else if (zwarcie_mf == 5)
		{
			d1 = sr2;
			d2 = sr3;
		}
		if (zwarcie_mf == 6)
		{
			d1 = sr3;
			d2 = sr1;
		}
	     
		if (d1 > d2)
			iloraz = d1 / d2;
		else
			iloraz = d2 / d1; 
	      
		procent = 1;
	      
		if (czas_1f != 0)
			procent = (float)czas_mf / (float)czas_1f;
	           
		if ((czas_mf > 20/LOOP_CYCLE_MS) && (czas_1f > 20/LOOP_CYCLE_MS) && (procent > 0.5) && (zwarcie_mf != 0))
		{
			if (iloraz < 3)
				temp = zwarcie_mf;
			else
				temp = zwarcie_1f;    
		}
	  	
	  	#ifdef LMZ_DEBUG
		*log->debug = temp;
	  	#endif
	  	
		switch(temp)
		{
			case 1://zwarcie RE
				for(i=0;i<(log->dw.wsk_buf - startbuf);i++)
				{
					log->dw.zu = log->dw.dp[i+startbuf].UL1;
					log->dw.odl_b[i] = wylicz_dane_1f(log,i+startbuf,0);
				}
				break;
			case 2://zwarcie SE
				for(i=0;i<(log->dw.wsk_buf - startbuf);i++)
				{
					log->dw.zu = log->dw.dp[i+startbuf].UL2;
					log->dw.odl_b[i] = wylicz_dane_1f(log,i+startbuf,1);
				}
				break;
			case 3://zwarcie TE
				for(i=0;i<(log->dw.wsk_buf-startbuf);i++)
				{
					log->dw.zu = log->dw.dp[i+startbuf].UL3;
					log->dw.odl_b[i] = wylicz_dane_1f(log,i+startbuf,2);
				}
				break;
			case 4://zwarcie RS
				for(i=0;i<(log->dw.wsk_buf-startbuf);i++)
				{
					log->dw.zu = log->dw.dp[i+startbuf].UL12;
					log->dw.odl_b[i] = wylicz_dane_mf(log,i+startbuf,0,1);
				}
				break;
			case 5://zwarcie ST
				for(i=0;i<(log->dw.wsk_buf-startbuf);i++)
				{
					log->dw.zu = log->dw.dp[i+startbuf].UL23;
					log->dw.odl_b[i] = wylicz_dane_mf(log,i+startbuf,1,2);
				}
				break;
			case 6://zwarcie TE
				for(i=0;i<(log->dw.wsk_buf-startbuf);i++)
				{
					log->dw.zu = log->dw.dp[i+startbuf].UL31;
					log->dw.odl_b[i] = wylicz_dane_mf(log,i+startbuf,2,0);
				}
				break;
			case 0: //error
				break;
		}

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

	//szukanie wyniku - mediana z wyliczonych wartoci
	for(i=0;i<(log->dw.wsk_buf-startbuf);i++)
	{
		temp1 = i+1;
		while(temp1)
		{
			temp1--;
			if(log->dw.odl_b[temp1+1]>log->dw.odl_b[temp1])
			{
				local = log->dw.odl_b[temp1];
				log->dw.odl_b[temp1] = log->dw.odl_b[temp1+1];
				log->dw.odl_b[temp1+1] = local;
			}
			else
				{
					temp1 = 0;
				}
			}
		}

		if(temp)
		{
			set_struct(&log->P);
			if((log->dw.wsk_buf-startbuf) % 2)
			{
				*log->distance=log->dw.odl_b[(log->dw.wsk_buf-startbuf)/2 + 1];
			}
			else
			{
				*log->distance=(log->dw.odl_b[(log->dw.wsk_buf-startbuf)/2 + 1] + log->dw.odl_b[(log->dw.wsk_buf-startbuf)/2]) / 2;
			}
			//------------------------------------------------
		}

		log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia
    }

	if(check_struct(&log->clr))
	{
		*log->distance=0;
		clear_struct(&log->P);

	}
}
