/*
 * CBR.c
 *
 *  Created on: 16-02-2017
 *      Author: PS
 */


#include "CBR.h"
#include "helper.h"
#include "config.h"

int CBR_initlog(void *arguments, void *logic)
{
	struct CBR_args *args = (struct CBR_args *)arguments;
	struct CBR_logic *log = (struct CBR_logic *)logic;

	if(set_bit_ptr_mask(args->io.we_W1_ON,&log->we_W1_ON,&log->we_W1_ON_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_W2_ON,&log->we_W2_ON,&log->we_W2_ON_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_W3_ON,&log->we_W3_ON,&log->we_W3_ON_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_W1_OFF,&log->we_W1_OFF,&log->we_W1_OFF_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_W2_OFF,&log->we_W2_OFF,&log->we_W2_OFF_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.we_W3_OFF,&log->we_W3_OFF,&log->we_W3_OFF_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.wy_W1_ON,&log->wy_W1_ON,&log->wy_W1_ON_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.wy_W2_ON,&log->wy_W2_ON,&log->wy_W2_ON_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.wy_W3_ON,&log->wy_W3_ON,&log->wy_W3_ON_bit_mask))
		return -1;

    if(set_bit_ptr_mask(args->io.zd_W_ON,&log->zd_W_ON,&log->zd_W_ON_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.zd_W_OFF,&log->zd_W_OFF,&log->zd_W_OFF_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.niezg_L1,&log->niezg_L1,&log->niezg_L1_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.niezg_L2,&log->niezg_L2,&log->niezg_L2_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.niezg_L3,&log->niezg_L3,&log->niezg_L3_bit_mask))
		return -1;

	log->nast_.licz_p = ((args->params.to) * 1000);
	log->nast_.rodzaj = args->params.rodzaj;

	log->licz_t1 = 0; // wyzerowanie czasu
    log->licz_t2 = 0; // wyzerowanie czasu
    log->licz_t3 = 0; // wyzerowanie czasu




	return 0;
}

void CBR(void *arguments, void *logic)
{
	struct CBR_logic *log = (struct CBR_logic *)logic;

	u8 t_W1_ON = ((*log->we_W1_ON & log->we_W1_ON_bit_mask) != 0) ? 1: 0;
	u8 t_W2_ON = ((*log->we_W2_ON & log->we_W2_ON_bit_mask) != 0) ? 1: 0;
	u8 t_W3_ON = ((*log->we_W3_ON & log->we_W3_ON_bit_mask) != 0) ? 1: 0;
	u8 t_W1_OFF = ((*log->we_W1_OFF & log->we_W1_OFF_bit_mask) != 0) ? 1: 0;
	u8 t_W2_OFF = ((*log->we_W2_OFF & log->we_W2_OFF_bit_mask) != 0) ? 1: 0;
	u8 t_W3_OFF = ((*log->we_W3_OFF & log->we_W3_OFF_bit_mask) != 0) ? 1: 0;

	u8 o_W1_ON = ((*log->wy_W1_ON & log->wy_W1_ON_bit_mask) != 0) ? 1: 0;
	u8 o_W2_ON = ((*log->wy_W2_ON & log->wy_W2_ON_bit_mask) != 0) ? 1: 0;
	u8 o_W3_ON = ((*log->wy_W3_ON & log->wy_W3_ON_bit_mask) != 0) ? 1: 0;

	u8 o_zd_ON = ((*log->zd_W_ON & log->zd_W_ON_bit_mask) != 0) ? 1: 0;
	u8 o_zd_OFF = ((*log->zd_W_OFF & log->zd_W_OFF_bit_mask) != 0) ? 1: 0;
	u8 o_n_L1 = ((*log->niezg_L1 & log->niezg_L1_bit_mask) != 0) ? 1: 0;
	u8 o_n_L2 = ((*log->niezg_L2 & log->niezg_L2_bit_mask) != 0) ? 1: 0;
	u8 o_n_L3 = ((*log->niezg_L3 & log->niezg_L3_bit_mask) != 0) ? 1: 0;


	switch(log->nast_.rodzaj)
	{
		case 0: //jednobitowo

			o_n_L1 = 0;
			o_n_L2 = 0;
			o_n_L3 = 0;

			if (t_W1_ON)
			{
				o_W1_ON = 1;
				o_W2_ON = 1;
				o_W3_ON = 1;
				o_zd_ON = 1;
				o_zd_OFF = 0;
			}
			else
			{
				o_W1_ON = 0;
				o_W2_ON = 0;
				o_W3_ON = 0;
				o_zd_ON = 0;
				o_zd_OFF = 1;
			}
			break;

//*****************************************************************

		case 1: //jednobitowo pofazowo

			o_n_L1 = 0;
			o_n_L2 = 0;
			o_n_L3 = 0;

			if (t_W1_ON)
				o_W1_ON = 1;
			else
				o_W1_ON = 0;

			if (t_W2_ON)
				o_W2_ON = 1;
			else
				o_W2_ON = 0;

			if (t_W3_ON)
				o_W3_ON = 1;
			else
				o_W3_ON = 0;

			if (t_W1_ON && t_W2_ON && t_W3_ON)
			{
				o_zd_ON = 1;
				o_zd_OFF = 0;
			}
			else if (!t_W1_ON && !t_W2_ON && !t_W3_ON)
			{
				o_zd_OFF = 1;
				o_zd_ON = 0;
			}
			else
			{
				o_zd_ON = 0;
				o_zd_OFF = 0;
			}
			break;

//*****************************************************************

		case 2: //dwubitowo

			if (t_W1_ON && !t_W1_OFF)
			{
				o_W1_ON = 1;
				o_W2_ON = 1;
				o_W3_ON = 1;
				o_zd_ON = 1;
				o_zd_OFF = 0;
				o_n_L1 = 0;
				o_n_L2 = 0;
				o_n_L3 = 0;
				log->licz_t1  = 0;
			}
			else if (!t_W1_ON && t_W1_OFF)
			{
				o_W1_ON = 0;
				o_W2_ON = 0;
				o_W3_ON = 0;
				o_zd_ON = 0;
				o_zd_OFF = 1;
				o_n_L1 = 0;
				o_n_L2 = 0;
				o_n_L3 = 0;
				log->licz_t1  = 0;
			}
			else  if (!t_W1_ON && !t_W1_OFF)
			{
				//pamieta stan - nie zmienia
				log->licz_t1+=LOOP_CYCLE_MS;
			}
			else  if (t_W1_ON && t_W1_OFF)
			{
				o_W1_ON = 1;
				o_W2_ON = 1;
				o_W3_ON = 1;
				o_zd_ON = 1;
				o_zd_OFF = 0;
				log->licz_t1+=LOOP_CYCLE_MS;
			}

			if (log->licz_t1 > log->nast_.licz_p)
			{
				o_n_L1 = 1;
				o_n_L2 = 1;
				o_n_L3 = 1;
				log->licz_t1 = log->nast_.licz_p + (5*LOOP_CYCLE_MS);
			}

			break;

//*****************************************************************

		case 3: //dwubitowo pofazowo

		// FAZA L1
			if (t_W1_ON && !t_W1_OFF)
			{
				o_W1_ON = 1;
				o_n_L1 = 0;
				log->licz_t1  = 0;
			}
			else if (!t_W1_ON && t_W1_OFF)
			{
				o_W1_ON = 0;
				o_n_L1 = 0;
				log->licz_t1  = 0;
			}
			else  if (!t_W1_ON && !t_W1_OFF)
			{
				//pamieta stan - nie zmienia
				log->licz_t1+=LOOP_CYCLE_MS;
			}
			else  if (t_W1_ON && t_W1_OFF)
			{
				o_W1_ON = 1;
				log->licz_t1+=LOOP_CYCLE_MS;
			}

			if (log->licz_t1 > log->nast_.licz_p)
			{
				o_n_L1 = 1;
				log->licz_t1 = log->nast_.licz_p + (5*LOOP_CYCLE_MS);
			}

		// FAZA L2
			if (t_W2_ON && !t_W2_OFF)
			{
				o_W2_ON = 1;
				o_n_L2 = 0;
				log->licz_t2  = 0;
			}
			else if (!t_W2_ON && t_W2_OFF)
			{
				o_W2_ON = 0;
				o_n_L2 = 0;
				log->licz_t2  = 0;
			}
			else  if (!t_W2_ON && !t_W2_OFF)
			{
				//pamieta stan - nie zmienia
				log->licz_t2+=LOOP_CYCLE_MS;
			}
			else  if (t_W2_ON && t_W2_OFF)
			{
				o_W2_ON = 1;
				log->licz_t2+=LOOP_CYCLE_MS;
			}

			if (log->licz_t2 > log->nast_.licz_p)
			{
				o_n_L2 = 1;
				log->licz_t2 = log->nast_.licz_p + (5*LOOP_CYCLE_MS);
			}

		// FAZA L3
			if (t_W3_ON && !t_W3_OFF)
			{
				o_W3_ON  = 1;
				o_n_L3 = 0;
				log->licz_t3  = 0;
			}
			else if (!t_W3_ON && t_W3_OFF)
			{
				o_W3_ON  = 0;
				o_n_L3 = 0;
				log->licz_t3  = 0;
			}
			else  if (!t_W3_ON && !t_W3_OFF)
			{
				//pamieta stan - nie zmienia
				log->licz_t3+=LOOP_CYCLE_MS;
			}
			else  if (t_W3_ON && t_W3_OFF)
			{
				o_W3_ON  = 1;
				log->licz_t3+=LOOP_CYCLE_MS;
			}

			if (log->licz_t3 > log->nast_.licz_p)
			{
				o_n_L3 = 1;
				log->licz_t3 = log->nast_.licz_p + (5*LOOP_CYCLE_MS);
			}

			if (o_W1_ON && o_W2_ON && o_W3_ON)
			{
				o_zd_ON = 1;
				o_zd_OFF = 0;
			}
			else if (!o_W1_ON && !o_W2_ON && !o_W3_ON)
			{
				o_zd_ON = 0;
				o_zd_OFF = 1;
			}
			else
			{
				o_zd_ON = 0;
				o_zd_OFF = 0;
			}

			break;
	default:
		break;

	}

	if (o_W1_ON)
		*log->wy_W1_ON |= log->wy_W1_ON_bit_mask;
	else
		*log->wy_W1_ON &= ~log->wy_W1_ON_bit_mask;

	if (o_W2_ON)
		*log->wy_W2_ON |= log->wy_W2_ON_bit_mask;
	else
		*log->wy_W2_ON &= ~log->wy_W2_ON_bit_mask;

	if (o_W3_ON)
		*log->wy_W3_ON |= log->wy_W3_ON_bit_mask;
	else
		*log->wy_W3_ON &= ~log->wy_W3_ON_bit_mask;

	if (o_zd_ON)
		*log->zd_W_ON |= log->zd_W_ON_bit_mask;
	else
		*log->zd_W_ON &= ~log->zd_W_ON_bit_mask;

	if (o_zd_OFF)
		*log->zd_W_OFF |= log->zd_W_OFF_bit_mask;
	else
		*log->zd_W_OFF &= ~log->zd_W_OFF_bit_mask;

	if (o_n_L1)
		*log->niezg_L1 |= log->niezg_L1_bit_mask;
	else
		*log->niezg_L1 &= ~log->niezg_L1_bit_mask;

	if (o_n_L2)
		*log->niezg_L2 |= log->niezg_L2_bit_mask;
	else
		*log->niezg_L2 &= ~log->niezg_L2_bit_mask;

	if (o_n_L3)
		*log->niezg_L3 |= log->niezg_L3_bit_mask;
	else
		*log->niezg_L3 &= ~log->niezg_L3_bit_mask;


}
