/*
 * l_comm.c
 *
 *  Created on: 28-11-2016
 *      Author: Krzysztof Jakubczyk
 */

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

#include "l_comm.h"
#include "helper.h"
#include "ZDistL.h"
#include <math.h>


int l_comm_initlog(void *arguments, void *logic)
{
	struct l_comm_args *args = (struct l_comm_args *)arguments;
	struct l_comm_logic *log = (struct l_comm_logic *)logic;
	u32 *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_no = (args->io.stan_bl_in & 0x07);

	log->ok = (u8 *)(log_manager.nets_data + (args->io.ok_in >> 3));
	if((u8 *)log->ok > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;
	log->ok_bit_no = (args->io.ok_in & 0x07);

	log->odb = (u8 *)(log_manager.nets_data + (args->io.odb_in >> 3));
	if((u8 *)log->odb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;
	log->odb_bit_no = (args->io.odb_in & 0x07);

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

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

	log->P_Io1 = (u8 *)(log_manager.nets_data + (args->io.P_Io1_in >> 3));
	if((u8 *)log->P_Io1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;
	log->P_Io1_bit_no = (args->io.P_Io1_in & 0x07);

	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_no = (args->io.test_in & 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_no = (args->io.W_out & 0x07);

	log->NRL = (u8 *)(log_manager.nets_data + (args->io.NRL_out >> 3));
	if((u8 *)log->NRL > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;
	log->NRL_bit_no = (args->io.NRL_out & 0x07);

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

	log->We_ZdistL_ptr=(void *)*ptr;

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

	*ptr = (u32)&log->dw.stan;

	log->nast_.On = (args->params.bity & 0x0001)?1:0;
	log->nast_.bl = (args->params.bity & 0x0002)?1:0;
	log->nast_.CRL_ON = (args->params.bity & 0x0004)?1:0;
	log->nast_.ECHO_ON = (args->params.bity & 0x0008)?1:0;
	log->nast_.WD_SPZ_ON = (args->params.bity & 0x0010)?1:0;
	log->nast_.Echo_SPZ_ON = (args->params.bity & 0x0020)?1:0;;


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


	log->nast_.t_Nad = (int)((args->params.t_Nad + 0.005f) * 100);
	log->nast_.t_CRL = (int)(args->params.t_CRL * 1000);
	log->nast_.t_W = (int)((args->params.t_W - 0.005f) * 100);
	if(log->nast_.t_W)
		log->nast_.Wbz = 0;
	else
		log->nast_.Wbz = 1;
	log->nast_.Ur = args->params.Ur * args->params.Ur;
	log->nast_.Ur = (1.03 * args->params.Ur) * (1.03 * args->params.Ur);

//	log->nast_.WeZ0.Zdist_dw = &nast_.dw;
//	if (nast_.adr.WE_Zdist) nast_.WeZ = (R_ZdistA::Wyjscia *)wskazniki[nast_.adr.WE_Zdist];
//						else nast_.WeZ = &nast_.WeZ0;

	log->nast_.ECHO_ON &= (args->params.Typ != 1); // funkcja echa nie dziaa kiedy jest typ blokujcy


	if (args->params.Typ==2) args->params.Odb = 10;
	log->nast_.Com_ON[0] = (args->params.Odb != 0);
	log->nast_.Com_ON[1] = (args->params.Odb != 1);
	log->nast_.Com_ON[2] = (args->params.Odb != 2);
	log->nast_.Com_ON[3] = (args->params.Odb != 3);
	log->nast_.Com_ON[4] = (args->params.Odb != 4);
	log->nast_.Com_ON[5] = (args->params.Odb != 5);
	log->nast_.Com_ON[6] = (args->params.Odb != 6);

	//log->nast_.nad = &dane_binarne[0];

	switch(args->params.Nad)
	{
		case 0:
//			if (nast_.adr.WE_Zdist)	nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[0]);
			break;
		case 1:
//			if (nast_.adr.WE_Zdist)	nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[1]);
			break;
		case 2:
//			if (nast_.adr.WE_Zdist)	nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[2]);
			break;
		case 3:
//			if (nast_.adr.WE_Zdist)	nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[3]);
			break;
		case 4:
//			if (nast_.adr.WE_Zdist)	nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[4]);
			break;
		case 5:
//			if (nast_.adr.WE_Zdist)	nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[5]);
			break;
		case 6:
			/*if (log->nast_.adr.P_Io1)*/ log->nast_.nad = log->P_Io1;//&dane_binarne[nast_.adr.P_Io1];
			break;
	}


	//nast_.Str_rev = &dane_binarne[0];

	switch(args->params.Str_rev)
	{
		case 0:
			//if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[0]);
			break;
		case 1:
			//if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[1]);
			break;
		case 2:
			//if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[2]);
			break;
		case 3:
			//if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[3]);
			break;
		case 4:
			//if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[4]);
			break;
		case 5:
			//if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[5]);
			break;
	}

	log->zero=0;
	log->nast_.Pz = &log->zero;

	//if (nast_.adr.WE_Zdist) nast_.Pz = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->P);
		//				else nast_.Pz = &dane_binarne[0];


	return 0;
}

void l_comm(void *arguments, void *logic)
{
	struct l_comm_logic *log = (struct l_comm_logic *)logic;
	float local;
	u8 localb,localc;
	int i;

	if (log->nast_.On)
	{

		switch(log->nast_.Typ)
		{
			case 0:
				log->dw.dbl = (*log->odb & (1<<log->odb_bit_no))?1:0;
				break;
			case 1:
				log->dw.dbl = !(*log->odb & (1<<log->odb_bit_no))?1:0;
				break;
			case 2://typ bezwarunkowy
				log->dw.W_I = (((*log->odb & (1<<log->odb_bit_no)) && (*log->ok & (1<<log->ok_bit_no)))) ? 1:0;
				break;
		}

		if(log->nast_.CRL_ON) //jesli wczona automatyka prdu wstecznego
		{
			if (*log->nast_.Str_rev)
			{
				log->dw.licztIrev = log->nast_.t_CRL;
				log->dw.dbl = 0;
			} else {
				if (log->dw.licztIrev)
				{
					log->dw.licztIrev--;
					log->dw.dbl = 0;
				}
			}

		}

		if (log->nast_.ECHO_ON)//jeli wczona automatyka echa
		{
			local = *log->U1;
			if (*log->U2 < local) local = *log->U2;
			if (*log->U3 < local) local = *log->U3;
			localb = local < log->nast_.Ur;
			localc = local > log->nast_.Ur_powr;
			sprawdz_P(&log->dw.Um,localb,localc,&log->dw.liczU,3,3);
			log->dw.W_E = (log->dw.Um && !(*log->nast_.Pz) && (*log->odb & (1<<log->odb_bit_no)) && (*log->ok & (1<<log->ok_bit_no)))?1:0;
		}

		localb = log->dw.W_E || (*log->nast_.nad & (1<<log->P_Io1_bit_no));
		log->dw.Nad |=  localb && !log->dw.Nadp;
		log->dw.Nadp = localb;


		log->dw.W |= (log->dw.W_E | log->dw.W_I) && log->nast_.Wbz;


		log->dw.dbl |= !(*log->ok & (1<<log->ok_bit_no))?1:0; // jeli cze niesprawne to zdjta blokada


		//obsluga wejscia testu przekaznika
//		if (czy_test_R())
	//	{
		//	dw.W = dw.Nad = dw.dbl = dane_binarne[nast_.adr.test];
		//}
        //

        // realizacja blokady dzialania przekaznika
		if((log->nast_.bl && (*log->stan_bl & (1<<log->stan_bl_bit_no))) /*| czy_start()*/)
		{
			log->dw.W = log->dw.Nad = 0;
			log->dw.dbl = 1;
		}
		//


		for(i=0;i<7;i++) //ustawienie blokad stref od cza
		{
			log->dw.stan.dbl_Com[i] = log->nast_.Com_ON[i] || log->dw.dbl;
		}


		if(log->dw.W)
			*log->W|=(1<<log->W_bit_no);
		else
			*log->W&=~(1<<log->W_bit_no);

		if(log->dw.Nad)
			*log->NRL|=(1<<log->NRL_bit_no);
		else
			*log->NRL&=~(1<<log->NRL_bit_no);

	} else {

		for(i=0;i<7;i++) //ustawienie blokad stref od cza
		{
			log->dw.stan.dbl_Com[i] = 1;
		}

		*log->W&=~(1<<log->W_bit_no);
		*log->NRL&=~(1<<log->NRL_bit_no);
	}
}


void l_comm_100hz(void *arguments, void *logic)
{
	struct l_comm_logic *log = (struct l_comm_logic *)logic;
	u8 lokalW;

		if (log->nast_.On)
		{
			//generowanie impulsu nadawania i wycz
			lokalW = log->dw.W_E || log->dw.W_I;
			if (lokalW) log->dw.licztW++; else log->dw.licztW = 0;

			if(log->dw.licztW>log->nast_.t_W)
			{
				log->dw.W |=1;
				log->dw.licztW--;
			}

			log->dw.W &= lokalW?1:0;

			log->dw.stan.PSPZ = log->dw.W && ((log->dw.W_E && log->nast_.Echo_SPZ_ON) || (log->dw.W_I && log->nast_.WD_SPZ_ON));


			if (log->dw.Nad)
			{
				log->dw.licztNad--;
				if(!log->dw.licztNad) log->dw.Nad = 0;
			} else {
				log->dw.licztNad = log->nast_.t_Nad;
			}
		}
}
