/*
 * hr_3ph2h.c
 *
 *  Created on: 20-04-2018
 *      Author: Krzysztof Jakubczyk
 */

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

#include "helper.h"
#include "hr_3ph2h.h"


int hr_3ph2h_initlog(void *arguments, void *logic)
{
	struct hr_3ph2h_args *args = (struct hr_3ph2h_args *)arguments;
	struct hr_3ph2h_logic *log = (struct hr_3ph2h_logic *)logic;

	if(set_bit_ptr(args->io.stan_bl,&log->stan_bl,&log->stan_bl_bit_no))
		return -1;

	if(set_bit_ptr(args->io.BL_out,&log->BL,&log->BL_bit_no))
		return -1;

	if(set_bit_ptr(args->io.BL1_out,&log->BL1,&log->BL1_bit_no))
		return -1;

	if(set_bit_ptr(args->io.BL2_out,&log->BL2,&log->BL2_bit_no))
		return -1;

	if(set_bit_ptr(args->io.BL3_out,&log->BL3,&log->BL3_bit_no))
		return -1;

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

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

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

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

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

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

	log->nast_.on_ = (args->params.bity & 0x0001)?1:0;
	log->nast_.bl_ = (args->params.bity & 0x0002)?1:0;
	log->nast_.or_ = (args->params.bity & 0x0004)?1:0;
    log->nast_.limit1_ = args->params.Wr_ *  args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu
    log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu

	log->nast_.I2h = args->params.I2h * args->params.I2h;
	log->nast_.I2h_powr = (args->params.kp_ * args->params.I2h) * (args->params.kp_ * args->params.I2h);

	return 0;
}

void hr_3ph2h(void *arguments, void *logic)
{
	struct hr_3ph2h_logic *log = (struct hr_3ph2h_logic *)logic;

    if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {
		sprawdz_P(&log->dw.pob12h,
			(*log->I12h>(log->nast_.I2h * *log->I1)),
			(*log->I12h<(log->nast_.I2h_powr * *log->I1)),
			&log->dw.lp12h,
			2,10); //realizacja przekaznika

		sprawdz_P(&log->dw.pob11h,
			*log->I1>log->nast_.limit1_,
			*log->I1<log->nast_.limit2_,
			&log->dw.lp11h,
			3,3); //realizacja przekaznika faza 1

		sprawdz_P(&log->dw.pob1,
			log->dw.pob11h && log->dw.pob12h,
			!log->dw.pob11h || !log->dw.pob12h,
			&log->dw.lp1,
			1,1); //realizacja przekaznika

//
		sprawdz_P(&log->dw.pob22h,
			(*log->I22h>(log->nast_.I2h * *log->I2)),
			(*log->I22h<(log->nast_.I2h_powr * *log->I2)),
			&log->dw.lp22h,
			2,10); //realizacja przekaznika

		sprawdz_P(&log->dw.pob21h,
			*log->I2>log->nast_.limit1_,
			*log->I2<log->nast_.limit2_,
			&log->dw.lp21h,
			3,3); //realizacja przekaznika faza 1

		sprawdz_P(&log->dw.pob2,
			log->dw.pob21h && log->dw.pob22h,
			!log->dw.pob21h || !log->dw.pob22h,
			&log->dw.lp2,
			1,1); //realizacja przekaznika

//
		sprawdz_P(&log->dw.pob32h,
			(*log->I32h>(log->nast_.I2h * *log->I3)),
			(*log->I32h<(log->nast_.I2h_powr * *log->I3)),
			&log->dw.lp32h,
			2,10); //realizacja przekaznika

		sprawdz_P(&log->dw.pob31h,
			*log->I3>log->nast_.limit1_,
			*log->I3<log->nast_.limit2_,
			&log->dw.lp31h,
			3,3); //realizacja przekaznika faza 1

		sprawdz_P(&log->dw.pob3,
			log->dw.pob31h && log->dw.pob32h,
			!log->dw.pob31h || !log->dw.pob32h,
			&log->dw.lp3,
			1,1); //realizacja przekaznika

        // realizacja blokady dzialania przekaznika
		if((log->nast_.bl_ && (*log->stan_bl & (1<<log->stan_bl_bit_no))) /*| czy_start()*/)
		{
			log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0;
		}
        //

        if(log->nast_.or_)  // zaleznie od nastawienia logiki pobudzen "and" lub "or"
			log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "or"
        else
			log->dw.pob = log->dw.pob1 && log->dw.pob2 && log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "and"

		check_and_set(log->dw.pob,log->BL,log->BL_bit_no);
		check_and_set(log->dw.pob1,log->BL1,log->BL1_bit_no);
		check_and_set(log->dw.pob2,log->BL2,log->BL2_bit_no);
		check_and_set(log->dw.pob3,log->BL3,log->BL3_bit_no);
    }
    else
    {
		*log->BL&=~(1<<log->BL_bit_no);
		*log->BL1&=~(1<<log->BL1_bit_no);
		*log->BL2&=~(1<<log->BL2_bit_no);
		*log->BL3&=~(1<<log->BL3_bit_no);
	}
}
