/*
 * under_1ph.c
 *
 *  Created on: 08-06-2021
 *      Author: Pawel Skoczykloda
 */

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

#include "r_freq.h"

#define CZAS_ODPADU 3

int r_freq_initlog(void *arguments, void *logic)
{
	struct r_freq_args *args = (struct r_freq_args *)arguments;
	struct r_freq_logic *log = (struct r_freq_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.f_ok_in, &log->f_ok, &log->f_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.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_.t_ = ((args->params.t_ - 0.005f) * 100); //czas dzialania
	log->nast_.nad_pod = args->params.tryb;
	
	
	log->nast_.rozruch = args->params.Wr_;
	if (log->nast_.nad_pod)
		log->nast_.odpad = log->nast_.rozruch - args->params.delta_;
	else
		log->nast_.odpad = log->nast_.rozruch + args->params.delta_;

	log->dw.lp_max = (short)(args->params.czas_wlasny_ / 10);
	if (log->dw.lp_max < 3)
		log->dw.lp_max = 3;
	log->dw.lp = 0;
	
	return 0;
}


void r_freq_100hz(void *arguments, void *logic)
{
	struct r_freq_logic *log = (struct r_freq_logic *)logic;

	if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {		
		if (log->nast_.nad_pod) //tryb nadczestotliwosciowy
		{
			u8 P = (*log->I >= log->nast_.rozruch) && (*log->f_ok & log->f_ok_bit_mask);
			u8 O = (*log->I <= log->nast_.odpad) || ((*log->f_ok & log->f_ok_bit_mask)== 0);
			
			sprawdz_P_100(&log->dw.pob, P, O, &log->dw.lp, log->dw.lp_max, CZAS_ODPADU);
									
		}
		else //tryb podczestotliwosciowy
		{
			u8 P = (*log->I <= log->nast_.rozruch) && (*log->f_ok & log->f_ok_bit_mask);
			u8 O = (*log->I >= log->nast_.odpad) || ((*log->f_ok & log->f_ok_bit_mask)== 0);
			
			sprawdz_P_100(&log->dw.pob, P, O, &log->dw.lp, log->dw.lp_max, CZAS_ODPADU);
		}

		//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;
				
		// 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;
				
		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;
	}
}
