/*
 * ov_1ph2h.c
 *
 *  Created on: 08-06-2017
 *      Author: Krzysztof Jakubczyk
 */

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

#include "ov_1ph2h.h"

int ov_1ph2h_initlog(void *arguments, void *logic)
{
	struct ov_1ph2h_args *args = (struct ov_1ph2h_args *)arguments;
	struct ov_1ph2h_logic *log = (struct ov_1ph2h_logic *)logic;

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

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

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

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

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

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

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

	if(set_bit_ptr_struct(args->io.bl_2h_out,&log->bl_2h))
		return -1;

	if(set_bit_ptr_struct(args->io.bezwar_out,&log->bezwar))
		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_.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_.t_ = ((args->params.t_ - 0.005f) * 100); //czas dzialania

	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);
	log->nast_.I2h_powr = (0.99 * args->params.I2h) * (0.99 * args->params.I2h);
  
  log->nast_.I2h_min = log->nast_.I2h * log->nast_.limit1_;

	log->nast_.Ibezwar = args->params.Ibezwar * args->params.Ibezwar;
	log->nast_.Ibezwar_powr =  (args->params.kp_ * args->params.Ibezwar) * (args->params.kp_ * args->params.Ibezwar);
	log->nast_.full_period = args->params.filter_type;
	return 0;
}

void ov_1ph2h(void *arguments, void *logic)
{
//	struct ov_1ph_args *args = (struct ov_1ph_args *)arguments;
	struct ov_1ph2h_logic *log = (struct ov_1ph2h_logic *)logic;
	u8 jest_blokada = 0;

    if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {

		sprawdz_P(&log->dw.pob2h,
			(*log->I2h>(log->nast_.I2h * *log->I) && *log->I2h > log->nast_.I2h_min),
			(*log->I2h<(log->nast_.I2h_powr * *log->I) ||  *log->I2h < log->nast_.I2h_min),
			&log->dw.lp2h,
			2,10); //realizacja przekaznika

		sprawdz_P(&log->dw.pob1h,
			*log->I>log->nast_.limit1_,
			*log->I<log->nast_.limit2_,
			&log->dw.lp1h,
			log->nast_.full_period?20:3,3); //realizacja przekaznika

		sprawdz_P(&log->dw.pobbezwar,*log->I>log->nast_.Ibezwar,*log->I<log->nast_.Ibezwar_powr,
			&log->dw.lpbezwar,
			1,1); //realizacja przekaznika

		sprawdz_P(&log->dw.pob,
			log->dw.pob1h && (!log->dw.pob2h || log->dw.pobbezwar),
			!log->dw.pob1h || (log->dw.pob2h && !log->dw.pobbezwar),
			&log->dw.lp,
			1,1); //realizacja przekaznika

		//obsluga wejscia testu przekaznika
		if (czy_test_R())
		{
			log->dw.pob = (*log->test & (1<<log->test_bit_no))?1:0;
		}
        //

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

		if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego
			if (!log->dw.zad && log->dw.pob)
			{
				log->dw.zad = 1;
//				dw.czas_ = time;
			}

		jest_blokada = (log->dw.pob1h && log->dw.pob2h);

		check_and_set_struct(jest_blokada && !(*log->stan_bl & (1<<log->stan_bl_bit_no)),&log->bl_2h);
		check_and_set_struct(log->dw.pobbezwar && !(log->nast_.bl_ && (*log->stan_bl & (1<<log->stan_bl_bit_no))) && log->dw.zad,&log->bezwar);

		check_and_set(log->dw.pob,log->P,log->P_bit_no);
		check_and_set(log->dw.zad,log->Z,log->Z_bit_no);
		check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no);

	} else {
		clear_struct(&log->bl_2h);
		clear_struct(&log->bezwar);
		*log->W&=~(1<<log->W_bit_no);
		*log->Z&=~(1<<log->Z_bit_no);
		*log->P&=~(1<<log->P_bit_no);
	}
}

void ov_1ph2h_100hz(void *arguments, void *logic)
{
//	struct ov_1ph_args *args = (struct ov_1ph_args *)arguments;
	struct ov_1ph2h_logic *log = (struct ov_1ph2h_logic *)logic;

	if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {
		// realizacja przekaznika czasowego
		if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania
		if(log->dw.licz_t_ > log->nast_.t_)
		{ // sprawdzenie przekroczenia nastawionego czasu
			log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu
			log->dw.licz_t_--;
		} else log->dw.zad = 0;
		//
	}
}
