/*
 * chka.c
 *
 *  Created on: 07-03-2018
 *      Author: Krzysztof Jakubczyk
 */

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

#include "chka.h"
#include "helper.h"
#include <math.h>

int chka_initlog(void *arguments, void *logic)
{
	struct chka_args *args = (struct chka_args *)arguments;
	struct chka_logic *log = (struct chka_logic *)logic;

	if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl))
		return -1;
	if(set_bit_ptr_struct(args->io.test_in,&log->test))
		return -1;
	if(set_bit_ptr_struct(args->io.zezw_l1_in,&log->zezw[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.zezw_l2_in,&log->zezw[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.zezw_l3_in,&log->zezw[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.pl1_out,&log->P[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.pl2_out,&log->P[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.pl3_out,&log->P[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.p3f_out,&log->P[3]))
		return -1;
	if(set_bit_ptr_struct(args->io.w3f_out,&log->W))
		return -1;
	if(set_bit_ptr_struct(args->io.z3f_out,&log->Z))
		return -1;

	if(set_float_ptr(args->io.wel1_float_in,&log->We[0]))
		return -1;
	if(set_float_ptr(args->io.wel2_float_in,&log->We[1]))
		return -1;
	if(set_float_ptr(args->io.wel3_float_in,&log->We[2]))
		return -1;

	log->Wn = args->params.Wn * args->params.Wn;
	log->Wn_hyst = log->Wn * args->params.k * args->params.k;

	log->t0=(u32)(args->params.t0*1000);
	return 0;
}

void chka(void *arguments, void *logic)
{
	struct chka_logic *log = (struct chka_logic *)logic;
	struct chka_args *args = (struct chka_args *)arguments;
	float tplus_value;
	u8 w,i;

	if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked
	{
		for(i=0;i<3;i++)
		{
			if(!check_struct(&log->zezw[i]))
			{
				log->dw.p[i]=0;
				log->dw.w[i]=0;
				log->dw.t[i]=0;
				log->dw.lp[i]=0;
				log->dw.lp2[i]=0;
				continue;
			}

			w=0;

			sprawdz_P(&log->dw.p[i],
				*log->We[i]>log->Wn,
				*log->We[i]<log->Wn_hyst,
				&log->dw.lp[i],
				3,3);

			if(log->dw.p[i])
			{
				if(args->params.typ_chki == CHKA_SIL_ZAL)
				{
					tplus_value=sqrtf(*log->We[i]);
					tplus_value/=args->params.Wn;
					tplus_value=powf(tplus_value,(float)args->params.n);
					tplus_value-=1;

					if(!tplus_value || tplus_value<0)
						tplus_value=0.0000001;
					tplus_value=args->params.tm/tplus_value;
					tplus_value*=(1000/LOOP_CYCLE_MS);

					if(tplus_value)
					{
						tplus_value=1000000000/tplus_value;
						log->dw.t[i]+=tplus_value;
					}
					else
						log->dw.t[i]=1000000000;


					if(log->dw.t[i]>=1000000000)
					{
						log->dw.t[i]=1000000000;
						w=1;
					}
					else
						w=0;
				}
				else if(args->params.typ_chki == CHKA_STALA)
				{
					w=1;
 				}
				else if(args->params.typ_chki == CHKA_IDMT)
				{
					tplus_value=sqrtf(*log->We[i]);
					tplus_value/=args->params.Wn;
					tplus_value=powf(tplus_value,(float)args->params.n);
					tplus_value-=1;

					if(!tplus_value || tplus_value<0)
						tplus_value=0.0000001;
					tplus_value=args->params.tm/tplus_value;
					tplus_value*=(1000/LOOP_CYCLE_MS);

					if(tplus_value)
					{
						tplus_value=1000000000/tplus_value;
						log->dw.t[i]+=tplus_value;
					}
					else
						log->dw.t[i]=1000000000;


					if(log->dw.t[i]>=1000000000)
					{
						log->dw.t[i]=1000000000;
						w=1;
					}
					else
						w=0;
				}
			}
			else
			{
				if(args->params.typ_chki == CHKA_IDMT)
				{
					tplus_value=sqrtf(*log->We[i]);
					tplus_value/=args->params.Wn;
					tplus_value=powf(tplus_value,(float)args->params.n);
					tplus_value=(1-tplus_value);

					if(!tplus_value || tplus_value<0)
						tplus_value=0.0000001;
					tplus_value=args->params.tm/tplus_value;
					tplus_value*=(1000/LOOP_CYCLE_MS);

					if(tplus_value)
					{
						tplus_value=1000000000/tplus_value;
						if(tplus_value<=log->dw.t[i])
							log->dw.t[i]-=tplus_value;
						else
							log->dw.t[i]=0;
					}
					else
						log->dw.t[i]=0;
				}
				else
					log->dw.t[i]=0;
			}

			if(w)
			{
				if(log->dw.lp2[i]>log->t0)
					log->dw.w[i]=1;
				else
					log->dw.lp2[i]+=LOOP_CYCLE_MS;
			}
			else
			{
				log->dw.lp2[i]=0;
				log->dw.w[i]=0;
			}
		}

		if(args->params.bity & 0x00000004) // OR logic
		{
			log->dw.w[3]=log->dw.w[0]||log->dw.w[1]||log->dw.w[2];
			log->dw.p[3]=log->dw.p[0]||log->dw.p[1]||log->dw.p[2];
		}
		else
		{
			log->dw.w[3]=log->dw.w[0]&&log->dw.w[1]&&log->dw.w[2];
			log->dw.p[3]=log->dw.p[0]&&log->dw.p[1]&&log->dw.p[2];
		}


		if(check_struct(&log->test))
		{
			log->dw.p[0]=1;
			log->dw.p[1]=1;
			log->dw.p[2]=1;
			log->dw.p[3]=1;
			log->dw.w[0]=1;
			log->dw.w[1]=1;
			log->dw.w[2]=1;
			log->dw.w[3]=1;
		}


		check_and_set_struct(log->dw.p[0],&log->P[0]);
		check_and_set_struct(log->dw.p[1],&log->P[1]);
		check_and_set_struct(log->dw.p[2],&log->P[2]);
		check_and_set_struct(log->dw.p[3],&log->P[3]);
		check_and_set_struct(log->dw.w[3],&log->Z);

		if(args->params.bity & 0x00000008) // na wylacz
		{
			check_and_set_struct(log->dw.w[3],&log->W);
		}
	}
	else
	{
		clear_struct(&log->P[0]);
		clear_struct(&log->P[1]);
		clear_struct(&log->P[2]);
		clear_struct(&log->P[3]);
		clear_struct(&log->W);
		clear_struct(&log->Z);
	}
}