#include "lacz.h"
#include "helper.h"
#include "config.h"

int lacz_initlog(void *arguments, void *logic)
{
	struct lacz_args *args = (struct lacz_args *)arguments;
	struct lacz_logic *log = (struct lacz_logic *)logic;

	if(set_bit_ptr_mask(args->io.we_ON,&log->we_ON,&log->we_ON_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_OFF,&log->we_OFF,&log->we_OFF_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_zamk1,&log->we_zamk1,&log->we_zamk1_bit_mask))
		return -1;
	
	if(set_bit_ptr_mask(args->io.we_zamk2,&log->we_zamk2,&log->we_zamk2_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_otw1,&log->we_otw1,&log->we_otw1_bit_mask))
		return -1;
	
	if(set_bit_ptr_mask(args->io.we_otw2,&log->we_otw2,&log->we_otw2_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_blk_otw,&log->we_blk_otw,&log->we_blk_otw_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_blk_zamk,&log->we_blk_zamk,&log->we_blk_zamk_bit_mask))
		return -1;
		
	if(set_bit_ptr_mask(args->io.wy_ON,&log->wy_ON,&log->wy_ON_bit_mask))
		return -1;	
	
	if(set_bit_ptr_mask(args->io.wy_NZG,&log->wy_NZG,&log->wy_NZG_bit_mask))
		return -1;	
	
	if(set_bit_ptr_mask(args->io.wy_imp_zamk,&log->wy_imp_zamk,&log->wy_imp_zamk_bit_mask))
		return -1;	
	
	if(set_bit_ptr_mask(args->io.wy_imp_otw,&log->wy_imp_otw,&log->wy_imp_otw_bit_mask))
		return -1;	
	
	log->licz_nast_on = (u16)((args->params.t_on) * 1000/ LOOP_CYCLE_MS);
	log->licz_nast_off = (u16)((args->params.t_off) * 1000/ LOOP_CYCLE_MS);
	log->licz_nast_nzg = (u16)((args->params.t_nzg) * 1000/ LOOP_CYCLE_MS);
	
	log->licz_t_on = 0;
	log->licz_t_off = 0;
	log->licz_t_nzg = 0;
	
	log->we_zamk_pop = 0;
	log->we_otw_pop = 0;
	return 0;
}

void lacz(void *arguments, void *logic)
{
	
	struct lacz_logic *log = (struct lacz_logic *)logic;
	
	u8 we_on = ((*log->we_ON & log->we_ON_bit_mask) != 0) ? 1 : 0;
	u8 we_off = ((*log->we_OFF & log->we_OFF_bit_mask) != 0) ? 1 : 0;
	
	u8 we_zamk = (((*log->we_zamk1 & log->we_zamk1_bit_mask) != 0) ? 1 : 0)
					|| (((*log->we_zamk2 & log->we_zamk2_bit_mask) != 0) ? 1 : 0);

	u8 we_otw = (((*log->we_otw1 & log->we_otw1_bit_mask) != 0) ? 1 : 0)
					|| (((*log->we_otw2 & log->we_otw2_bit_mask) != 0) ? 1 : 0);

	u8 blk_zamk = ((*log->we_blk_zamk & log->we_blk_zamk_bit_mask) != 0) ? 1 : 0;
	u8 blk_otw = ((*log->we_blk_otw & log->we_blk_otw_bit_mask) != 0) ? 1 : 0;

	if (we_on && !we_off)
	{
		log->licz_t_nzg = 0;
		*log->wy_ON |= log->wy_ON_bit_mask;
		*log->wy_NZG &= ~log->wy_NZG_bit_mask;
	}
	else if (!we_on && we_off)
	{
		log->licz_t_nzg = 0;
		*log->wy_ON &= ~log->wy_ON_bit_mask;
		*log->wy_NZG &= ~log->wy_NZG_bit_mask;
		
	} 
	else //niezgodnosc
	{
		//nie zmieniac stanu
		log->licz_t_nzg++;
		
	}
	
	if (log->licz_t_nzg > log->licz_nast_nzg)
	{
		log->licz_t_nzg = log->licz_nast_nzg + 5;
		*log->wy_NZG |= log->wy_NZG_bit_mask;			
	}


	if (!blk_zamk)
	{
		if (log->licz_t_on != 0)
		{
			if (++log->licz_t_on > log->licz_nast_on)
			{
				log->licz_t_on = 0;
				*log->wy_imp_zamk &= ~log->wy_imp_zamk_bit_mask;
			}
		}
		else if (we_zamk && (!log->we_zamk_pop))
		{
			*log->wy_imp_zamk |= log->wy_imp_zamk_bit_mask;
			log->licz_t_on = 1;	
		}			
	
		log->we_zamk_pop = we_zamk;
	}
	else
	{
		log->licz_t_on = 0;
		*log->wy_imp_zamk &= ~log->wy_imp_zamk_bit_mask;
		log->we_zamk_pop = 0;
	}

	

	if (!blk_otw)
	{
		if (log->licz_t_off != 0)
		{
			if (++log->licz_t_off > log->licz_nast_off)
			{
				log->licz_t_off = 0;
				*log->wy_imp_otw &= ~log->wy_imp_otw_bit_mask;
			}
		}
		else if (we_otw && (!log->we_otw_pop))
		{
			*log->wy_imp_otw |= log->wy_imp_otw_bit_mask;
			log->licz_t_off = 1;	
		}			
	
		log->we_otw_pop = we_otw;
	}
	else
	{
		log->licz_t_off = 0;
		*log->wy_imp_otw &= ~log->wy_imp_otw_bit_mask;
		log->we_otw_pop = 0;
	}

}