/*
 * Diff_2.c
 *
 *  Created on: 08-05-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "Diff.h"
#include "Diff_2.h"

int diff_2_initlog(void *arguments, void *logic)
{
	struct diff_2_args *args = (struct diff_2_args *)arguments;
	struct diff_logic *log = (struct diff_logic *)logic;

	if(diff_init_io(&args->io,log))
		return -1;

    log->nast_.on =  (args->params.bity & 0x0001)?1:0;
    log->nast_.w =  (args->params.bity & 0x0002)?1:0;
    log->nast_.bl =  (args->params.bity & 0x0004)?1:0;
    log->nast_.bl2h =  (args->params.bity & 0x0008)?1:0;
    log->nast_.bl5h =  (args->params.bity & 0x0010)?1:0;
    log->nast_.sbc =  (args->params.bity & 0x0020)?1:0;
    log->nast_.h =  (args->params.bity & 0x0040)?1:0;
    log->nast_.r =  (args->params.bity & 0x0080)?1:0;

	//ustawienie wartosci rozruchowych
	log->nast_.khr_2h = args->params.k_2h * args->params.k_2h;
	log->nast_.khp_2h = log->nast_.khr_2h * args->params.kp *args->params.kp;
	log->nast_.khr_5h = args->params.k_5h * args->params.k_5h;
	log->nast_.khp_5h = log->nast_.khr_5h * args->params.kp *args->params.kp;
	log->nast_.Irr = args->params.Ir * 1.3577f;
	log->nast_.Ipr = args->params.Ir * args->params.kp;
	log->nast_.Ihdr = args->params.Ihd;
	log->nast_.Irdr = args->params.rd;
	log->nast_.kh1r = args->params.kh1;
	log->nast_.kh1p = log->nast_.kh1r * args->params.kp;

	log->nast_.kh1r2 = args->params.kh1 * args->params.kh1;
	log->nast_.kh1p2 = log->nast_.kh1r2 * args->params.kp * args->params.kp;
	log->nast_.Ir4 = args->params.Ihg * args->params.kh1 * args->params.Ihg * args->params.kh1 + args->params.Iro * args->params.Iro;
	log->nast_.Ip4 = log->nast_.Ir4 * args->params.kp * args->params.kp;
	log->nast_.Iror = args->params.Iro * args->params.Iro;
	log->nast_.Irop = log->nast_.Iror * args->params.kp * args->params.kp;
	log->nast_.t_sbc = args->params.tbld * 1000;
	log->nast_.Ihg = args->params.Ihg * args->params.Ihg;

	log->nast_.tz = args->params.t * 1000;


	//--------

	struct nast_trafo n1;
	n1.GrP1 = args->params.GrP1;
	n1.GrP2 = args->params.GrP2;
	n1.GrP3 = args->params.GrP3;
	n1.PT12 = args->params.PT12;
	n1.PT13 = args->params.PT13;
	//n1.Tr_zas = args->params.Tr_zas;

	przelicz_nastawy1(log,n1);

	return 0;
}

void diff_2(void *arguments, void *logic)
{
	struct diff_logic *log = (struct diff_logic *)logic;

    if(log->nast_.on)   // jesli zabezpieczenie aktywne
    {
		przelicz_1200Hz_1(log);

		//Sprawdzenie pobudzenia w stabilizowanej strefie dzialania


		kryterium3(log,log->Ir1_,log->Ih1_,&log->P_.Pb1);
		kryterium3(log,log->Ir2_,log->Ih2_,&log->P_.Pb2);
		kryterium3(log,log->Ir3_,log->Ih3_,&log->P_.Pb3);

		przelicz_1200Hz_2(log);
	} else {
		*log->Ir1=0;
		*log->Ir2=0;
		*log->Ir3=0;
		*log->Ih1=0;
		*log->Ih2=0;
		*log->Ih3=0;
		*log->Ir_L1_ortb=0;
		*log->Ir_L2_ortb=0;
		*log->Ir_L3_ortb=0;
		//*log->Ih_L1_ortb=0;
		//*log->Ih_L2_ortb=0;
		//*log->Ih_L3_ortb=0;
		*log->I2h1=0;
		*log->I2h2=0;
		*log->I2h3=0;
		*log->I5h1=0;
		*log->I5h2=0;
		*log->I5h3=0;
		clear_struct(&log->P);
		clear_struct(&log->Pf1);
		clear_struct(&log->Pf2);
		clear_struct(&log->Pf3);
		clear_struct(&log->W);
		clear_struct(&log->Z);
		clear_struct(&log->Zw1);
		clear_struct(&log->Zw2);
		clear_struct(&log->Zw3);
		clear_struct(&log->BL2h1);
		clear_struct(&log->BL2h2);
		clear_struct(&log->BL2h3);
		clear_struct(&log->BL5h1);
		clear_struct(&log->BL5h2);
		clear_struct(&log->BL5h3);
	}
}
