/*
 * demux8.c
 *
 *  Created on: 22-05-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "nr_zacz.h"

int nr_zacz_initlog(void *arguments, void *logic)
{
	struct nr_zacz_args *args = (struct nr_zacz_args *)arguments;
	struct nr_zacz_logic *log = (struct nr_zacz_logic *)logic;

	if(set_bit_ptr_struct(args->io.b0_in,&log->b0))
		return -1;
	if(set_bit_ptr_struct(args->io.b1_in,&log->b1))
		return -1;
	if(set_bit_ptr_struct(args->io.b2_in,&log->b2))
		return -1;
	if(set_bit_ptr_struct(args->io.b3_in,&log->b3))
		return -1;
	if(set_bit_ptr_struct(args->io.b4_in,&log->b4))
		return -1;
	if(set_bit_ptr_struct(args->io.b5_in,&log->b5))
		return -1;

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

	if(set_bit_ptr_struct(args->io.err_out,&log->err))
		return -1;
	if(set_bit_ptr_struct(args->io.min_out,&log->min))
		return -1;
	if(set_bit_ptr_struct(args->io.max_out,&log->max))
		return -1;
	if(set_bit_ptr_struct(args->io.zo_zacz_out,&log->zo_zacz))
		return -1;

	log->t_odczytu = 0;

	return 0;
}

void nr_zacz(void *arguments, void *logic)
{
	struct nr_zacz_args *args = (struct nr_zacz_args *)arguments;
	struct nr_zacz_logic *log = (struct nr_zacz_logic *)logic;
	u8 choice,bcd1;

	switch(args->params.typ_numer) ///< nastawa trybu
		{
			case BCD:
				//BCD
				choice = check_struct(&log->b0)?1:0;
				choice |= check_struct(&log->b1)?2:0;
				choice |= check_struct(&log->b2)?4:0;
				choice |= check_struct(&log->b3)?8:0;
				bcd1 = choice;//tylko do ustalenia czy BCD jednosci > 9
				choice += check_struct(&log->b4)?10:0;
				choice += check_struct(&log->b5)?20:0;
				break;
			case BIN:
				//BIN
				choice = check_struct(&log->b0)?1:0;
				choice |= check_struct(&log->b1)?2:0;
				choice |= check_struct(&log->b2)?4:0;
				choice |= check_struct(&log->b3)?8:0;
				bcd1 = 0;//tylko do ustalenia czy BCD jednosci > 9
				choice |= check_struct(&log->b4)?16:0;
				choice |= check_struct(&log->b5)?32:0;
				break;
			case GRAY:
				//GRAY
				bcd1 = 0;//tylko do ustalenia czy BCD jednosci > 9
				choice = check_struct(&log->b5)?0x20:0;
				choice |= check_struct(&log->b4)?((~choice & (1<<5))>>1):((choice & (1<<5))>>1);
				choice |= check_struct(&log->b3)?((~choice & (1<<4))>>1):((choice & (1<<4))>>1);
				choice |= check_struct(&log->b2)?((~choice & (1<<3))>>1):((choice & (1<<3))>>1);
				choice |= check_struct(&log->b1)?((~choice & (1<<2))>>1):((choice & (1<<2))>>1);
				choice |= check_struct(&log->b0)?((~choice & (1<<1))>>1):((choice & (1<<1))>>1);
				break;
			default:
				choice = 0;
				break;
		}
	//

	
	//if (!check_struct(&log->err))
		{
		//if *log->nr_zacz_out-(float)choice  //jak różnica o 1 to przepisz na log->nr_zacz_out
		//if(fabs(*log->nr_zacz_out-choice)<1.5)
		//if (log->last_choice == choice)
/*
		if ((u8)*log->nr_zacz_out == choice)
			{
			*log->nr_zacz_out = (float)choice;
			log->t_odczytu = 0;
			clear_struct(&log->zo_zacz);
			check_and_set_struct((choice <= args->params.nr_min_zaczep),&log->min);
			check_and_set_struct((choice >= args->params.nr_max_zaczep),&log->max);
			check_and_set_struct(((choice < args->params.nr_min_zaczep)||(choice > args->params.nr_max_zaczep)||(bcd1>9)) ,&log->err);
			}
			else
			{
				set_struct(&log->zo_zacz);
				if (log->last_choice == choice)
					log->t_odczytu+=LOOP_CYCLE_MS;
					else
					log->t_odczytu = 0;
				log->last_choice = choice;
				if (log->t_odczytu > ((args->params.t_op_zacz)*1000))
					*log->nr_zacz_out = (float)choice;
			}
*/
		if (log->last_choice == choice)
			{
			log->t_odczytu+=LOOP_CYCLE_MS;
			if (log->t_odczytu > ((args->params.t_op_zacz)*1000))
				{
				check_and_set_struct(((choice < args->params.nr_min_zaczep)||(choice > args->params.nr_max_zaczep)||(bcd1>9)) ,&log->err);
				if (!check_struct(&log->err))
					{
						*log->nr_zacz_out = (float)choice;
						check_and_set_struct((choice <= args->params.nr_min_zaczep),&log->min);
						check_and_set_struct((choice >= args->params.nr_max_zaczep),&log->max);
					}
				log->t_odczytu=LOOP_CYCLE_MS;
				clear_struct(&log->zo_zacz);
				}
			}
			else
			{
				set_struct(&log->zo_zacz);
				log->t_odczytu = 0;
				log->last_choice = choice;
			}



		}
		

}
