/*
 * chka_ieee.c
 *
 *  Created on: 08-08-2018
 *      Author: Krzysztof Jakubczyk
 */

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

#include "chka_ieee.h"
#include <math.h>

int chka_ieee_initlog(void *arguments, void *logic)
{
	struct chka_ieee_args *args = (struct chka_ieee_args *)arguments;
	struct chka_ieee_logic *log = (struct chka_ieee_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_bit_ptr_struct(args->io.relax_out,&log->relax))
		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.kp * args->params.kp;

	/// predefined curves
	if(args->params.typ_chki == CHKA_INVERSE)
	{
		args->params.typ_chki=CHKA_SIL_ZAL;
		args->params.k=0.14;
		args->params.c=0;
		args->params.alfa=0.02;
		args->params.tr=0;
	}
	else if(args->params.typ_chki == CHKA_VERY_INVERSE)
	{
		args->params.typ_chki=CHKA_SIL_ZAL;
		args->params.k=13.5;
		args->params.c=0;
		args->params.alfa=1;
		args->params.tr=0;
	}
	else if(args->params.typ_chki == CHKA_EXTREMELY_INVERSE)
	{
		args->params.typ_chki=CHKA_SIL_ZAL;
		args->params.k=80;
		args->params.c=0;
		args->params.alfa=2;
		args->params.tr=0;
	}
	else if(args->params.typ_chki == CHKA_IEEE_MODERATELY_INVERSE)
	{
		args->params.typ_chki=CHKA_IDMT;
		args->params.k=0.0515;
		args->params.c=0.1140;
		args->params.alfa=0.02;
		args->params.tr=4.85;
		log->ieee_mi=1;
	}
	else if(args->params.typ_chki == CHKA_IEEE_VERY_INVERSE)
	{
		args->params.typ_chki=CHKA_IDMT;
		args->params.k=19.61;
		args->params.c=0.491;
		args->params.alfa=2;
		args->params.tr=21.6;
	}
	else if(args->params.typ_chki == CHKA_IEEE_EXTREMELY_INVERSE)
	{
		args->params.typ_chki=CHKA_IDMT;
		args->params.k=28.2;
		args->params.c=0.1217;
		args->params.alfa=2;
		args->params.tr=29.1;
	}
	///

	if(args->params.typ_chki == CHKA_STALA)
		log->t0=(u32)(args->params.c*1000);
	else
		log->t0=(u32)(args->params.c*1000*args->params.TMS);

	log->kTMS=args->params.TMS*args->params.k;
	log->trTMS=args->params.TMS*args->params.tr;
	
	log->t_blok_max = (u32)(args->params.t_blok * 1000);
	log->t_blok = 0;
	
	return 0;
}

void chka_ieee(void *arguments, void *logic)
{
	struct chka_ieee_logic *log = (struct chka_ieee_logic *)logic;
	struct chka_ieee_args *args = (struct chka_ieee_args *)arguments;
	float tplus_value;
	u8 w,i;

	if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked
	{
		log->t_blok = 0;
		
		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.alfa);
					tplus_value-=1;

					if(!tplus_value || tplus_value<0)
						tplus_value=0.0000001;
					tplus_value=log->kTMS/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.alfa);
					tplus_value-=1;

					if(!tplus_value || tplus_value<0)
						tplus_value=0.0000001;
					tplus_value=log->kTMS/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;
					if(log->ieee_mi)
						tplus_value=powf(tplus_value,(float)2.00);
					else
						tplus_value=powf(tplus_value,(float)args->params.alfa);

					tplus_value=(1-tplus_value);

					if(!tplus_value || tplus_value<0)
						tplus_value=0.0000001;
					tplus_value=log->trTMS/tplus_value;
					tplus_value*=(1000/LOOP_CYCLE_MS);

					if(tplus_value)
					{
						tplus_value=1000000000/tplus_value;
						if(tplus_value>=(float)log->dw.t[i])
							log->dw.t[i]=0;
						else
							log->dw.t[i]-=tplus_value;
					}
					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(czy_test_R() && 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
	{
		if (log->t_blok < log->t_blok_max)
		{
			log->t_blok += LOOP_CYCLE_MS;				
		}
		else
		{
			for(i=0;i<3;i++)
			{
			log->dw.p[i]=0;
			log->dw.w[i]=0;
			log->dw.t[i]=0;
			log->dw.lp2[i]=0;
			}			
		}
		
 
		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);
	}

	check_and_set_struct(!log->dw.t[0]&&!log->dw.t[1]&&!log->dw.t[2] && !((args->params.typ_chki == CHKA_STALA)&&w),&log->relax);
}