/*
 * zzr.c
 *
 *  Created on: 11-07-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "zzr.h"

int zzr_initlog(void *arguments, void *logic)
{
	struct zzr_args *args = (struct zzr_args *)arguments;
	struct zzr_logic *log = (struct zzr_logic *)logic;

	if(set_float_ptr(args->io.InL1_orta,&log->InL1_orta))
		return -1;

	if(set_float_ptr(args->io.InL1_ortb,&log->InL1_ortb))
		return -1;

	if(set_float_ptr(args->io.InL1_esk,&log->InL1_esk))
		return -1;

	if(set_float_ptr(args->io.InL2_orta,&log->InL2_orta))
		return -1;

	if(set_float_ptr(args->io.InL2_ortb,&log->InL2_ortb))
		return -1;

	if(set_float_ptr(args->io.InL2_esk,&log->InL2_esk))
		return -1;

	if(set_float_ptr(args->io.InL3_orta,&log->InL3_orta))
		return -1;

	if(set_float_ptr(args->io.InL3_ortb,&log->InL3_ortb))
		return -1;

	if(set_float_ptr(args->io.InL3_esk,&log->InL3_esk))
		return -1;

	if(set_float_ptr(args->io.In2L1_orta,&log->In2L1_orta))
		return -1;

	if(set_float_ptr(args->io.In2L1_ortb,&log->In2L1_ortb))
		return -1;

	if(set_float_ptr(args->io.In2L1_esk,&log->In2L1_esk))
		return -1;

	if(set_float_ptr(args->io.In2L2_orta,&log->In2L2_orta))
		return -1;

	if(set_float_ptr(args->io.In2L2_ortb,&log->In2L2_ortb))
		return -1;

	if(set_float_ptr(args->io.In2L2_esk,&log->In2L2_esk))
		return -1;

	if(set_float_ptr(args->io.In2L3_orta,&log->In2L3_orta))
		return -1;

	if(set_float_ptr(args->io.In2L3_ortb,&log->In2L3_ortb))
		return -1;

	if(set_float_ptr(args->io.In2L3_esk,&log->In2L3_esk))
		return -1;

	if(set_float_ptr(args->io.Iy_orta,&log->Iy_orta))
		return -1;

	if(set_float_ptr(args->io.Iy_ortb,&log->Iy_ortb))
		return -1;

	if(set_float_ptr(args->io.Iy_esk,&log->Iy_esk))
		return -1;

	if(set_bit_ptr_struct(args->io.bl_in,&log->bl))
		return -1;

	if(set_bit_ptr_struct(args->io.test_in,&log->test))
		return -1;

	if(set_bit_ptr_struct(args->io.P_out,&log->P))
		return -1;

	if(set_bit_ptr_struct(args->io.W_out,&log->W))
		return -1;

	if(set_float_ptr(args->io.Id_float_out,&log->Id_out))
		return -1;

	if(set_float_ptr(args->io.Ir_float_out,&log->Ir_out))
		return -1;

	log->kamNa=args->params.kamN;
	log->kamN2a=args->params.kamN2;
	log->kamYa=args->params.kamY;
	log->td_ms=(u32)(args->params.td * 1000);

	return 0;
}

void zzr(void *arguments, void *logic)
{
	struct zzr_logic *log = (struct zzr_logic *)logic;
	struct zzr_args *args = (struct zzr_args *)arguments;
	float maxf,maxf2,Id_orta,Id_ortb,Isum_am_orta,Isum_am_ortb,Id,Ir,Isum_am2_orta,Isum_am2_ortb,Isum_am1_ortb,Isum_am1_orta,Ir1,Ir2;
	u8 trig, untrig;

	if(!check_struct(&log->bl) && (args->params.bits & ZZR_AKT))
	{
		Isum_am_orta=log->kamNa*(*log->InL1_orta + *log->InL2_orta + *log->InL3_orta);
		Isum_am_ortb=log->kamNa*(*log->InL1_ortb + *log->InL2_ortb + *log->InL3_ortb);
		Isum_am1_orta=Isum_am_orta;
		Isum_am1_ortb=Isum_am_ortb;

		if (args->params.bits & ZZR_AT)
		{
			Isum_am2_orta=log->kamN2a*(*log->In2L1_orta + *log->In2L2_orta + *log->In2L3_orta);
			Isum_am2_ortb=log->kamN2a*(*log->In2L1_ortb + *log->In2L2_ortb + *log->In2L3_ortb);
			Isum_am_orta+=Isum_am2_orta;
			Isum_am_ortb+=Isum_am2_ortb;
		}

		Id_orta = Isum_am_orta + (log->kamYa * *log->Iy_orta);
		Id_ortb = Isum_am_ortb  + (log->kamYa * *log->Iy_ortb);
		Id = ((Id_orta * Id_orta) + (Id_ortb*Id_ortb)) / 2;
		Id = sqrtf(Id);

		switch((enum zzr_mode)args->params.mode)
		{
			case low_imped_ip_max: // 1
				maxf=*log->InL1_esk;
				if(*log->InL2_esk>maxf)
					maxf=*log->InL2_esk;
				if(*log->InL3_esk>maxf)
					maxf=*log->InL3_esk;

				maxf=sqrtf(maxf);
				maxf*=log->kamNa;

				maxf2=0;

				if(args->params.bits & ZZR_AT)
				{
					maxf2=*log->In2L1_esk;
					if(*log->In2L2_esk>maxf2)
						maxf2=*log->In2L2_esk;
					if(*log->In2L3_esk>maxf2)
						maxf2=*log->In2L3_esk;

					maxf2=sqrtf(maxf2);
					maxf2*=log->kamN2a;
				}

				if(maxf2>maxf)
					maxf=maxf2;


				Ir=(sqrtf(*log->Iy_esk)*log->kamYa + maxf)*0.5;

				break;
			case low_imped_sum_ip: // 0
			default:

				if (args->params.bits & ZZR_AT)
				{
					Ir1 = ((Isum_am1_orta * Isum_am1_orta) + (Isum_am1_ortb * Isum_am1_ortb));
					Ir2 = ((Isum_am2_orta * Isum_am2_orta) + (Isum_am2_ortb * Isum_am2_ortb));
					if(Ir2>Ir1)
						Ir = Ir2 / 2;
					else
						Ir = Ir1 / 2;
				}
				else
					Ir = ((Isum_am_orta * Isum_am_orta) + (Isum_am_ortb * Isum_am_ortb)) / 2;

				Ir = sqrtf(Ir);
				break;
		}

		*log->Ir_out=Ir;
		*log->Id_out=Id;

		switch((enum zzr_characteristic)args->params.characteristic)
		{
			case linear_advanced:
				if(Ir<args->params.Ir_m2)
				{
					trig = (Id > args->params.Id_max) ? 1 : (Id > (args->params.Id_start + args->params.m1*Ir)) ? 1 : 0;
					untrig = (Id <= (0.95*args->params.Id_max) && Id < 0.95*(args->params.Id_start + args->params.m1*Ir))?1:0;
				}
				else
				{
					float tmp;
					tmp = args->params.Id_start + args->params.m1*args->params.Ir_m2;
					trig = (Id > args->params.Id_max) ? 1 : (Id > (tmp + args->params.m2*(Ir-args->params.Ir_m2))) ? 1 : 0;
					untrig = (Id <= (0.95*args->params.Id_max) && Id < 0.95*(tmp + args->params.m2*(Ir-args->params.Ir_m2)))?1:0;
				}

				sprawdz_P(
					&log->P_ch,
					trig,
					untrig,
					&log->P_ch_cnt,
					20,3
					);

				if (czy_test_R())
				{
					log->P_ch = check_struct(&log->test)?1:0;
				}

				if(log->P_ch)
				{
					set_struct(&log->P);
					log->t_cnt+=LOOP_CYCLE_MS;
					if(log->t_cnt>log->td_ms)
					{
						set_struct(&log->W);
						log->t_cnt-=LOOP_CYCLE_MS;
					}
				}
				else
				{
					clear_struct(&log->P);
					clear_struct(&log->W);
					log->t_cnt=20;
				}
				break;

			case linear_simple:
			default:
				trig = (Id > args->params.Id_max) ? 1 : (Id > (args->params.Id_start + args->params.m1*Ir)) ? 1 : 0;
				untrig = (Id <= (0.95*args->params.Id_max) && Id < 0.95*(args->params.Id_start + args->params.m1*Ir))?1:0;
				sprawdz_P(
					&log->P_ch,
					trig,
					untrig,
					&log->P_ch_cnt,
					3,3
					);

				if (czy_test_R())
				{
					log->P_ch = check_struct(&log->test)?1:0;
				}

				if(log->P_ch)
				{
					set_struct(&log->P);
					log->t_cnt+=LOOP_CYCLE_MS;
					if(log->t_cnt>log->td_ms)
					{
						set_struct(&log->W);
						log->t_cnt-=LOOP_CYCLE_MS;
					}
				}
				else
				{
					clear_struct(&log->P);
					clear_struct(&log->W);
					log->t_cnt=0;
				}

				break;
		}
	}
	else
	{
		*log->Ir_out=0;
		*log->Id_out=0;
		log->t_cnt=0;
		clear_struct(&log->P);
		clear_struct(&log->W);
	}

}
