/*
 * kier_pr.c
 *
 *  Created on: 27-02-2018
 *      Author: Krzysztof Jakubczyk
 */

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

#include "kier_pr.h"
#include "e_phi.h"
#include "helper.h"
#include <math.h>

int kier_pr_initlog(void *arguments, void *logic)
{
	struct kier_pr_args *args = (struct kier_pr_args *)arguments;
	struct kier_pr_logic *log = (struct kier_pr_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_float_ptr(args->io.UL1_orta_float_in,&log->UL_orta[0]))
		return -1;
	if(set_float_ptr(args->io.UL1_ortb_float_in,&log->UL_ortb[0]))
		return -1;
	if(set_float_ptr(args->io.UL1_esk_float_in,&log->UL_esk[0]))
		return -1;
	if(set_float_ptr(args->io.UL2_orta_float_in,&log->UL_orta[1]))
		return -1;
	if(set_float_ptr(args->io.UL2_ortb_float_in,&log->UL_ortb[1]))
		return -1;
	if(set_float_ptr(args->io.UL2_esk_float_in,&log->UL_esk[1]))
		return -1;
	if(set_float_ptr(args->io.UL3_orta_float_in,&log->UL_orta[2]))
		return -1;
	if(set_float_ptr(args->io.UL3_ortb_float_in,&log->UL_ortb[2]))
		return -1;
	if(set_float_ptr(args->io.UL3_esk_float_in,&log->UL_esk[2]))
		return -1;

	if(set_float_ptr(args->io.IL1_orta_float_in,&log->IL_orta[0]))
		return -1;
	if(set_float_ptr(args->io.IL1_ortb_float_in,&log->IL_ortb[0]))
		return -1;
	if(set_float_ptr(args->io.IL1_esk_float_in,&log->IL_esk[0]))
		return -1;
	if(set_float_ptr(args->io.IL2_orta_float_in,&log->IL_orta[1]))
		return -1;
	if(set_float_ptr(args->io.IL2_ortb_float_in,&log->IL_ortb[1]))
		return -1;
	if(set_float_ptr(args->io.IL2_esk_float_in,&log->IL_esk[1]))
		return -1;
	if(set_float_ptr(args->io.IL3_orta_float_in,&log->IL_orta[2]))
		return -1;
	if(set_float_ptr(args->io.IL3_ortb_float_in,&log->IL_ortb[2]))
		return -1;
	if(set_float_ptr(args->io.IL3_esk_float_in,&log->IL_esk[2]))
		return -1;

	if(set_bit_ptr_struct(args->io.p3f_out,&log->P3F))
		return -1;
	if(set_bit_ptr_struct(args->io.pl1_out,&log->PL1))
		return -1;
	if(set_bit_ptr_struct(args->io.pl2_out,&log->PL2))
		return -1;
	if(set_bit_ptr_struct(args->io.pl3_out,&log->PL3))
		return -1;

	if(set_bit_ptr_struct(args->io.w3f_out,&log->W3F))
		return -1;
	if(set_bit_ptr_struct(args->io.wl1_out,&log->WL1))
		return -1;
	if(set_bit_ptr_struct(args->io.wl2_out,&log->WL2))
		return -1;
	if(set_bit_ptr_struct(args->io.wl3_out,&log->WL3))
		return -1;

	if(set_bit_ptr_struct(args->io.z3f_out,&log->Z3F))
		return -1;
	if(set_bit_ptr_struct(args->io.zl1_out,&log->ZL1))
		return -1;
	if(set_bit_ptr_struct(args->io.zl2_out,&log->ZL2))
		return -1;
	if(set_bit_ptr_struct(args->io.zl3_out,&log->ZL3))
		return -1;

	log->umin = args->params.U_min * args->params.U_min;
	log->umin_hyst = log->umin * args->params.kp_ui * args->params.kp_ui;

	log->imin = args->params.I_min * args->params.I_min;
	log->imin_hyst = log->imin * args->params.kp_ui * args->params.kp_ui;

	log->imax = args->params.I_max * args->params.I_max;
	log->imax_hyst = log->imax * (1/args->params.kp_ui) * (1/args->params.kp_ui);

	log->phi_char_360 = (args->params.phi_char<0)?360+args->params.phi_char:args->params.phi_char;
	log->phi_min = log->phi_char_360 - args->params.phi_dev;
	log->phi_max = log->phi_char_360 + args->params.phi_dev;
	log->phi_min=log->phi_min<0?(360+log->phi_min):log->phi_min;
	if(log->phi_min>log->phi_max)
		log->phi_ov=1;

	log->phi_min_hyst = log->phi_char_360 - args->params.phi_dev - args->params.kp_phi;
	log->phi_max_hyst = log->phi_char_360 + args->params.phi_dev + args->params.kp_phi;
	log->phi_min_hyst = log->phi_min_hyst<0?(360+log->phi_min_hyst):log->phi_min_hyst;

	if(log->phi_min_hyst>log->phi_max_hyst)
		log->phi_hyst_ov=1;

	log->td=(u32)(args->params.td*1000);
	if(!log->td)
		log->td=1;
	if(log->td>32767)
		log->td=32767;

	return 0;
}

void kier_pr(void *arguments, void *logic)
{
	struct kier_pr_logic *log = (struct kier_pr_logic *)logic;
	struct kier_pr_args *args = (struct kier_pr_args *)arguments;
	u8 i;
	float phi,phi1,phi2;

	if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked
	{
		for(i=0;i<3;i++)
		{
			sprawdz_P(&log->dw.pob_i[i],
				*log->IL_esk[i]>log->imin&&*log->IL_esk[i]<log->imax,
				*log->IL_esk[i]<log->imin_hyst||*log->IL_esk[i]>log->imax_hyst,
				&log->dw.lp_i[i],
				5,3);
			sprawdz_P(&log->dw.pob_u[i],
				*log->UL_esk[i]>log->umin,
				*log->UL_esk[i]<log->umin_hyst,
				&log->dw.lp_u[i],
				5,3);

			phi1=get_phase(*log->IL_ortb[i],*log->IL_orta[i]);
			phi2=get_phase(*log->UL_ortb[i],*log->UL_orta[i]);

			phi=get_phase_diff(phi1,phi2);

			phi=phi<0?(360+phi):phi;


			if(log->dw.pob_u[i]&&log->dw.pob_i[i])
			{
				if(log->phi_max_hyst>=360 && phi<=(log->phi_max_hyst-360))
					phi+=360;

				  if (args->params.bity & 0x00000010)
            log->dw.pob_phi[i] = 1;
          else
          {
       
        sprawdz_P(&log->dw.pob_phi[i],
					log->phi_ov?(phi>log->phi_min||phi<log->phi_max):(phi>log->phi_min && phi<log->phi_max),
					log->phi_hyst_ov?(phi<log->phi_min_hyst&&phi>log->phi_max_hyst):(phi<log->phi_min_hyst || phi>log->phi_max_hyst),
					//(log->phi_min_ov ? (phi<log->phi_min_hyst && phi>log->phi_max_hyst) : phi<log->phi_min_hyst) || (log->phi_max_ov ? phi<log->phi_max_hyst : log->phi_min_ov? (phi>log->phi_max_hyst && phi<log->phi_min_hyst): phi>log->phi_max_hyst ),
					&log->dw.lp_phi[i],
					20,10);
          }
			}
			else
			{
				log->dw.lp_phi[i]=0;
				log->dw.pob_phi[i]=0;
			}

			if(log->dw.pob_phi[i])
				log->pob[i]=1;
			else
				log->pob[i]=0;

			sprawdz_P(&log->dw.zadz[i],
					log->pob[i],
					!log->pob[i],
					&log->dw.lp[i],log->td,1);
		}

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

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

		check_and_set_struct(log->pob[0],&log->PL1);
		check_and_set_struct(log->pob[1],&log->PL2);
		check_and_set_struct(log->pob[2],&log->PL3);
		check_and_set_struct(log->pob[3],&log->P3F);
		check_and_set_struct(log->dw.zadz[0],&log->ZL1);
		check_and_set_struct(log->dw.zadz[1],&log->ZL2);
		check_and_set_struct(log->dw.zadz[2],&log->ZL3);
		check_and_set_struct(log->dw.zadz[3],&log->Z3F);

		if(args->params.bity & 0x00000008) // na wylacz
		{
			check_and_set_struct(log->dw.zadz[0],&log->WL1);
			check_and_set_struct(log->dw.zadz[1],&log->WL2);
			check_and_set_struct(log->dw.zadz[2],&log->WL3);
			check_and_set_struct(log->dw.zadz[3],&log->W3F);
		}

	}
	else
	{
		clear_struct(&log->P3F);
		clear_struct(&log->PL1);
		clear_struct(&log->PL2);
		clear_struct(&log->PL3);
		clear_struct(&log->Z3F);
		clear_struct(&log->ZL1);
		clear_struct(&log->ZL2);
		clear_struct(&log->ZL3);
		clear_struct(&log->W3F);
		clear_struct(&log->WL1);
		clear_struct(&log->WL2);
		clear_struct(&log->WL3);
	}
}
