/*
 * under_1ph.c
 *
 *  Created on: 08-05-2014
 *      Author: Krzysztof Jakubczyk
 */

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

#include "under_1ph.h"

int under_1ph_initlog(void *arguments, void *logic)
{
	struct under_1ph_args *args = (struct under_1ph_args *)arguments;
	struct under_1ph_logic *log = (struct under_1ph_logic *)logic;

	if(set_bit_ptr_mask(args->io.stan_bl_in,&log->stan_bl,&log->stan_bl_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.W_out,&log->W,&log->W_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_out,&log->P,&log->P_bit_mask))
		return -1;

	if(set_float_ptr(args->io.I_float_in,&log->I))
		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_.full_period = args->params.filter_type;
	return 0;
}

void under_1ph(void *arguments, void *logic)
{
//	struct ov_1ph_args *args = (struct ov_1ph_args *)arguments;
	struct under_1ph_logic *log = (struct under_1ph_logic *)logic;
    if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {
		sprawdz_P(&log->dw.pob,
			*log->I<log->nast_.limit1_,
			*log->I>log->nast_.limit2_,
			&log->dw.lp,
			log->nast_.full_period?20:3,3); //realizacja przekaznika


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


        // realizacja blokady dzialania przekaznika
		if((log->nast_.bl_ && (*log->stan_bl & log->stan_bl_bit_mask)) /*|| 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;
			}

		check_and_set_mask(log->dw.pob,log->P,log->P_bit_mask);
		check_and_set_mask(log->dw.zad,log->Z,log->Z_bit_mask);
		check_and_set_mask(log->dw.zad && log->nast_.w_,log->W,log->W_bit_mask);
	} else {
		*log->W&=~log->W_bit_mask;
		*log->Z&=~log->Z_bit_mask;
		*log->P&=~log->P_bit_mask;
	}
}

void under_1ph_100hz(void *arguments, void *logic)
{
//	struct ov_1ph_args *args = (struct ov_1ph_args *)arguments;
	struct under_1ph_logic *log = (struct under_1ph_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;
		//
	}
}
