/*
 * sck.c
 *
 *  Created on: 11-01-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "sck.h"

const u32 NaN = 0xFFFFFFFF;

int sck_initlog(void *arguments, void *logic)
{
	struct sck_args *args = (struct sck_args *)arguments;
	struct sck_logic *log = (struct sck_logic *)logic;

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

	if(set_float_ptr(args->io.U1_orta_float_in,&log->U1_orta))
		return -1;
	if(set_float_ptr(args->io.U1_ortb_float_in,&log->U1_ortb))
		return -1;

	if(set_float_ptr(args->io.U2_orta_float_in,&log->U2_orta))
		return -1;
	if(set_float_ptr(args->io.U2_ortb_float_in,&log->U2_ortb))
		return -1;

	if(set_float_ptr(args->io.U3_orta_float_in,&log->U3_orta))
		return -1;
	if(set_float_ptr(args->io.U3_ortb_float_in,&log->U3_ortb))
		return -1;

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

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

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

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

	if(set_bit_ptr_mask(args->io.fa_OK_in,&log->fa_OK,&log->fa_OK_bit_mask))
		return -1;

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

	if(set_bit_ptr_mask(args->io.fb_OK_in,&log->fb_OK,&log->fb_OK_bit_mask))
		return -1;

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

	if(set_bit_ptr_mask(args->io.Z_out,&log->Z,&log->Z_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.P_Uam_out,&log->P_Uam,&log->P_Uam_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.P_Uaw_out,&log->P_Uaw,&log->P_Uaw_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.P_Ubm_out,&log->P_Ubm,&log->P_Ubm_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.P_Ubw_out,&log->P_Ubw,&log->P_Ubw_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.P_DU_out,&log->P_DU,&log->P_DU_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.P_Df_out,&log->P_Df,&log->P_Df_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.P_Dfi_out,&log->P_Dfi,&log->P_Dfi_bit_mask))
		return -1;

	if(set_float_ptr(args->io.Df_float_out,&log->Df))
		return -1;
	if(set_float_ptr(args->io.Dfi_float_out,&log->Dfi))
		return -1;
	if(set_float_ptr(args->io.DU_float_out,&log->DU))
		return -1;

	if(set_bit_ptr_mask(args->io.f_OK_out,&log->f_OK,&log->f_OK_bit_mask))
		return -1;

	log->nast_.On = (args->params.bity & 0x0001)?1:0;
	log->nast_.bl = (args->params.bity & 0x0002)?1:0;
	log->nast_.N00 = (args->params.bity & 0x0004)?1:0;
	log->nast_.N01 = (args->params.bity & 0x0008)?1:0;
	log->nast_.N10 = (args->params.bity & 0x0010)?1:0;
	log->nast_.N11 = (args->params.bity & 0x0020)?1:0;
	log->nast_.delU = (args->params.bity & 0x0040)?1:0;

	log->nast_.Ub = args->params.Ub;

	//wartosci progowe dla kryterium fazowego
	log->nast_.Dfir = cos(args->params.Dfir * 3.14159f / 180);
	log->nast_.Dfir = log->nast_.Dfir * log->nast_.Dfir * 4;
	log->nast_.Dfip = cos((args->params.Dfir + 0.2) * 3.14159f / 180);
	log->nast_.Dfip = log->nast_.Dfip * log->nast_.Dfip * 4;
	//
	//wartosci progowe dla kryterium czestotliwosci
	log->nast_.Dfr = args->params.Dfr;
	log->nast_.Dfp = args->params.Dfr + 0.002;
	//
	//wartosci progowe dla kryterium roznicy napiecia
	log->nast_.DUr = args->params.DUr * args->params.DUr;
	log->nast_.DUp = log->nast_.DUr * 1.01;
	//
	//wartosci rozruchowe dla dedekcji obecnosci napiec
	log->nast_.Uarm = args->params.Uarm * args->params.Uarm;
	log->nast_.Uapm = log->nast_.Uarm * 1.04;
	log->nast_.Ubrm = args->params.Ubrm * args->params.Ubrm;
	log->nast_.Ubpm = log->nast_.Ubrm * 1.04;
	log->nast_.Uarw = args->params.Uarw * args->params.Uarw;
	log->nast_.Uapw = log->nast_.Uarw * 0.96;
	log->nast_.Ubrw = args->params.Ubrw * args->params.Ubrw;
	log->nast_.Ubpw = log->nast_.Ubrw * 0.96;
	//

	return 0;
}

void sck(void *arguments, void *logic)
{
	struct sck_logic *log = (struct sck_logic *)logic;

	float orta_a;
	float ortb_a;
	float cosfi2;

	if(log->nast_.On)
	{
		//wyliczenie napiecia wejsciowego w zalenosci od nastaw
		switch(log->nast_.Ub)
		{
			case 0: //faza L1
				orta_a = *log->U1_orta;
				ortb_a = *log->U1_ortb;
				break;
			case 1: //faza L2
				orta_a = *log->U2_orta;
				ortb_a = *log->U2_ortb;
				break;
			case 2: //faza L3
				orta_a = *log->U3_orta;
				ortb_a = *log->U3_ortb;
				break;
			case 3: //faza L12
				orta_a = *log->U1_orta - *log->U2_orta;
				ortb_a = *log->U1_ortb - *log->U2_ortb;
				break;
			case 4: //faza L23
				orta_a = *log->U2_orta - *log->U3_orta;
				ortb_a = *log->U2_ortb - *log->U3_ortb;
				break;
			case 5: //faza L31
				orta_a = *log->U3_orta - *log->U1_orta;
				ortb_a = *log->U3_ortb - *log->U1_ortb;
				break;
		}

		log->A = (orta_a * orta_a + ortb_a * ortb_a) / 2;

		if (log->nast_.delU)
		{
			//Wyliczenie roznicy wartosci skutecznych
			log->dw.DU = sqrtf(log->A) - sqrtf(*log->Ub);
			log->dw.DU*=log->dw.DU;
		}
		else
		{
			//wyliczenie ronicy wektorowej napiec
			log->dw.DUa = orta_a - *log->Ub_orta;
			log->dw.DUb = ortb_a - *log->Ub_ortb;
			log->dw.DU = (log->dw.DUa * log->dw.DUa + log->dw.DUb * log->dw.DUb) / 2;
		}
	
		//sprawdzenie kryteriow napieciowcyh fazowych
		sprawdz_P(
					&log->stan_.P_Uam,
					log->A < log->nast_.Uarm,
					log->A > log->nast_.Uapm,
					&log->dw.lUarm,
					5,5);

		sprawdz_P(
					&log->stan_.P_Ubm,
					*log->Ub < log->nast_.Ubrm,
					*log->Ub > log->nast_.Ubpm,
					&log->dw.lUbrm,
					5,5);

		sprawdz_P(
					&log->stan_.P_Uaw,
					log->A > log->nast_.Uarw,
					log->A < log->nast_.Uapw,
					&log->dw.lUarw,
					5,5);

		sprawdz_P(
					&log->stan_.P_Ubw,
					*log->Ub > log->nast_.Ubrw,
					*log->Ub < log->nast_.Ubpw,
					&log->dw.lUbrw,
					5,5);
		//



		//Sprawdzenie kryterium ronicy napiec
		sprawdz_P(
					&log->stan_.P_DU,
					log->dw.DU < log->nast_.DUr,
					log->dw.DU > log->nast_.DUp,
					&log->dw.lDU,
					5,5);

		//
		// sprawdzenie kryterium kata fazowego
		log->dw.cosfi = orta_a * *log->Ub_orta + ortb_a * *log->Ub_ortb;
		cosfi2 = log->dw.cosfi * log->dw.cosfi;//
		log->A = (log->A * *log->Ub) ;
		sprawdz_P(
					&log->stan_.P_Dfi,
					(cosfi2 > (log->nast_.Dfir * log->A)) && (log->dw.cosfi > 0),
					(cosfi2 < (log->nast_.Dfip * log->A)) || (log->dw.cosfi < 0),
					&log->dw.lDfi,
					5,5);

		//


		//wyprowadzenie wyjscia zezwolenia na zalaczenie
		if (czy_test_R())
		{
			log->stan_.Z = !(log->nast_.bl && (*log->bl & log->bl_bit_mask)) && (*log->test & log->test_bit_mask);
		}
		else
		{
			log->stan_.Z = !(log->nast_.bl && (*log->bl & log->bl_bit_mask)) && // jesli nastawiona blokada i jest blokada
			(
				(log->nast_.N00 && (log->stan_.P_Uam && log->stan_.P_Ubm)) ||
				(log->nast_.N01 && (log->stan_.P_Uam && log->stan_.P_Ubw)) ||
				(log->nast_.N10 && (log->stan_.P_Uaw && log->stan_.P_Ubm)) ||
				(log->nast_.N11 && (log->stan_.P_Df && log->stan_.P_Dfi && log->stan_.P_DU && log->stan_.P_Uaw && log->stan_.P_Ubw))
			);
		}
		//

		check_and_set_mask(log->stan_.Z,log->Z,log->Z_bit_mask);
		check_and_set_mask(log->stan_.P_Uam,log->P_Uam,log->P_Uam_bit_mask);
		check_and_set_mask(log->stan_.P_Uaw,log->P_Uaw,log->P_Uaw_bit_mask);
		check_and_set_mask(log->stan_.P_Ubm,log->P_Ubm,log->P_Ubm_bit_mask);
		check_and_set_mask(log->stan_.P_Ubw,log->P_Ubw,log->P_Ubw_bit_mask);
		check_and_set_mask(log->stan_.P_Dfi,log->P_Dfi,log->P_Dfi_bit_mask);
		check_and_set_mask(log->stan_.P_DU,log->P_DU,log->P_DU_bit_mask);
	}
	else
	{
		*log->Z&=~log->Z_bit_mask;
		*log->P_Uam&=~log->P_Uam_bit_mask;
		*log->P_Uaw&=~log->P_Uaw_bit_mask;
		*log->P_Ubm&=~log->P_Ubm_bit_mask;
		*log->P_Ubw&=~log->P_Ubw_bit_mask;
		*log->P_Dfi&=~log->P_Dfi_bit_mask;
		*log->P_DU&=~log->P_DU_bit_mask;
	}
}

void sck_100hz(void *arguments, void *logic)
{
	struct sck_logic *log = (struct sck_logic *)logic;

	float Df;
	u8 OK;

	if(log->nast_.On)
	{
		Df = fabs(*log->fa - *log->fb);
		OK = (*log->fa_OK & log->fa_OK_bit_mask) && (*log->fb_OK & log->fb_OK_bit_mask);

		sprawdz_P(
					&log->stan_.P_Df,
					Df < log->nast_.Dfr && OK,
					Df > log->nast_.Dfp || !OK,
					&log->dw.lDf,
					4,4);
			//wyprowadzenie aktualnych pomiarow

			//

		*log->Df = Df;
		check_and_set_mask(OK,log->f_OK,log->f_OK_bit_mask);
		check_and_set_mask(log->stan_.P_Df,log->P_Df,log->P_Df_bit_mask);
	}
	else
	{
		*log->f_OK&=~log->f_OK_bit_mask;
		*log->P_Df&=~log->P_Df_bit_mask;
	}
}

void sck_20hz(void *arguments, void *logic)
{
	struct sck_logic *log = (struct sck_logic *)logic;

	float loc;
	if(log->dw.licz_pom>9)
	{
		loc = sqrt(4 * log->A);
		log->dw.licz_pom=0;
		*log->DU=log->dw.DU;

		if (log->stan_.P_Uam || log->stan_.P_Ubm)
		{
			*log->Dfi = *(float *)&NaN;
		} else {
			*log->Dfi = acos(log->dw.cosfi / loc) * 57.2958f;// 180 / 3.14159f ;
		}
	}
	log->dw.licz_pom++;
}
