/*
 * 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_min_hyst = args->params.phi_min - args->params.kp_phi;

	if(log->phi_min_hyst<-180)
	{
		log->phi_min_hyst=180+(180+log->phi_min_hyst);
		log->phi_min_ov=1;
	}

	log->phi_max_hyst = args->params.phi_max + args->params.kp_phi;
	if(log->phi_max_hyst>180)
	{
		log->phi_max_hyst=-180+(log->phi_max_hyst-180);
		log->phi_max_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],
				12,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],
				12,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);

			sprawdz_P(&log->dw.pob_phi[i],
				phi>args->params.phi_min && phi<args->params.phi_max,
				//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);

			if(log->dw.pob_phi[i] && log->dw.pob_i[i] && log->dw.pob_u[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(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);
	}
}
