/*
 * LRW.c
 *
 *  Created on: 15-03-2022
 *      Author: 
 *      Pawe Skoczykoda
 */


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


#include "LRW.h"

void set_default(void *logic)
{
	struct LRW_logic *log = (struct LRW_logic *)logic;	
	
	log->ret_I_L1 = 0;
	log->ret_I_L2 = 0;
	log->ret_I_L3 = 0;
	log->ret_W_L1 = 0;
	log->ret_W_L2 = 0;
	log->ret_W_L3 = 0;
	log->ret_I_3f = 0;
	log->ret_W_3f = 0;
	log->wy_T1 = 0;
	log->wy_T2 = 0;
	log->wy_T2_I = 0;
	log->wy_T2_W = 0;
	
	log->pob_I_L1 = 0;
	log->pob_I_L2 = 0;
	log->pob_I_L3 = 0;
	log->pob_W_L1 = 0;
	log->pob_W_L2 = 0;
	log->pob_W_L3 = 0;
	log->pob_I_3f = 0;
	log->pob_W_3f = 0;		
	log->pob_T2_I = 0;
	log->pob_T2_W = 0;
	log->wy_pob = 0;
	
	log->t_I_L1 = 0;
	log->t_I_L2 = 0;
	log->t_I_L3 = 0;
	log->t_W_L1 = 0;
	log->t_W_L2 = 0;
	log->t_W_L3 = 0;	
	log->t_I_3f = 0;
	log->t_W_3f = 0;
	log->t_I_2 = 0;
	log->t_W_2 = 0;
	
}	

int LRW_initlog(void *arguments, void *logic)
{
	struct LRW_args *args = (struct LRW_args *)arguments;
	struct LRW_logic *log = (struct LRW_logic *)logic;

	if(set_bit_ptr_struct(args->io.stan_bl,&log->stan_bl))
		return -1;

	if(set_bit_ptr_struct(args->io.w_on_L1_in,&log->w_on_L1))
		return -1;
	if(set_bit_ptr_struct(args->io.w_on_L2_in,&log->w_on_L2))
		return -1;
	if(set_bit_ptr_struct(args->io.w_on_L3_in,&log->w_on_L3))
		return -1;
	if(set_bit_ptr_struct(args->io.wyl_L1_in,&log->wyl_L1))
		return -1;
	if(set_bit_ptr_struct(args->io.wyl_L2_in,&log->wyl_L2))
		return -1;
	if(set_bit_ptr_struct(args->io.wyl_L3_in,&log->wyl_L3))
		return -1;
	if(set_bit_ptr_struct(args->io.test_in,&log->test))
		return -1;
		
	if(set_bit_ptr_struct(args->io.retrip_I_L1_out,&log->retrip_I_L1))
		return -1;
	if(set_bit_ptr_struct(args->io.retrip_I_L2_out,&log->retrip_I_L2))
		return -1;
	if(set_bit_ptr_struct(args->io.retrip_I_L3_out,&log->retrip_I_L3))
		return -1;	
	if(set_bit_ptr_struct(args->io.retrip_W_L1_out,&log->retrip_W_L1))
		return -1;
	if(set_bit_ptr_struct(args->io.retrip_W_L2_out,&log->retrip_W_L2))
		return -1;
	if(set_bit_ptr_struct(args->io.retrip_W_L3_out,&log->retrip_W_L3))
		return -1;

	if(set_bit_ptr_struct(args->io.retrip_I_3f_out,&log->retrip_I_3f))
		return -1;
	if(set_bit_ptr_struct(args->io.retrip_W_3f_out,&log->retrip_W_3f))
		return -1;

	if(set_bit_ptr_struct(args->io.T2_I_out,&log->T2_I))
		return -1;
	if(set_bit_ptr_struct(args->io.T2_W_out,&log->T2_W))
		return -1;

	if(set_bit_ptr_struct(args->io.T1_out,&log->T1))
		return -1;
	if(set_bit_ptr_struct(args->io.T2_out,&log->T2))
		return -1;

	if(set_bit_ptr_struct(args->io.pob_out,&log->pob))
		return -1;

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

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

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

	log->nast_.on_ = (args->params.bity & 0x0001)?1:0;
	log->kryt_pr = (args->params.bity & 0x0002)?1:0;
	log->kryt_wyl = (args->params.bity & 0x0004)?1:0;
	log->nast_.full_period = (args->params.bity & 0x0008)?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->t1_limit = (u32)((args->params.t1_) * 1000 / LOOP_CYCLE_MS);
	log->t2_limit = (u32)((args->params.t2_) * 1000 / LOOP_CYCLE_MS);
		
	set_default(log);
		
	return 0;
}

void LRW(void *arguments, void *logic)
{
	struct LRW_logic *log = (struct LRW_logic *)logic;


    if(log->nast_.on_)   // jesli zabezpieczenie aktywne
    {
		u8 blokada = check_struct(&log->stan_bl)?1:0;
		u8 w_on_L1 = check_struct(&log->w_on_L1)?1:0;
		u8 w_on_L2 = check_struct(&log->w_on_L2)?1:0;
		u8 w_on_L3 = check_struct(&log->w_on_L3)?1:0;
		u8 wyl_L1 = check_struct(&log->wyl_L1)?1:0;
		u8 wyl_L2 =  check_struct(&log->wyl_L2)?1:0;
		u8 wyl_L3 =  check_struct(&log->wyl_L3)?1:0;
		u8 test =  check_struct(&log->test)?1:0;


		sprawdz_P(&log->dw.pob1,
			*log->I1>log->nast_.limit1_,
			*log->I1<log->nast_.limit2_,
			&log->dw.lp1,
			log->nast_.full_period?20:3,3); //realizacja przekaznika faza 1

		sprawdz_P(&log->dw.pob2,
			*log->I2>log->nast_.limit1_,
			*log->I2<log->nast_.limit2_,
			&log->dw.lp2,
			log->nast_.full_period?20:3,3); //realizacja przekaznika faza 2

		sprawdz_P(&log->dw.pob3,
			*log->I3>log->nast_.limit1_,
			*log->I3<log->nast_.limit2_,
			&log->dw.lp3,
			log->nast_.full_period?20:3,3); //realizacja przekaznika faza 3


		log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;

		log->pob_I_L1 = wyl_L1 && !wyl_L2 && !wyl_L3 && log->dw.pob1 && log->kryt_pr;
		log->pob_I_L2 = !wyl_L1 && wyl_L2 && !wyl_L3 && log->dw.pob2 && log->kryt_pr;
		log->pob_I_L3 = !wyl_L1 && !wyl_L2 && wyl_L3 && log->dw.pob3 && log->kryt_pr;

		log->pob_W_L1 = wyl_L1 && !wyl_L2 && !wyl_L3 && w_on_L1 && log->kryt_wyl;
		log->pob_W_L2 = !wyl_L1 && wyl_L2 && !wyl_L3 && w_on_L2 && log->kryt_wyl;
		log->pob_W_L3 = !wyl_L1 && !wyl_L2 && wyl_L3 && w_on_L3 && log->kryt_wyl;

		u8 min_2faz =  (wyl_L1 && wyl_L2) || (wyl_L2 && wyl_L3) || (wyl_L1 && wyl_L3);
		u8 suma_on = w_on_L1 || w_on_L2 || w_on_L3;
		
		log->pob_I_3f = min_2faz && log->dw.pob && log->kryt_pr;
		log->pob_W_3f = min_2faz && suma_on && log->kryt_wyl;

		log->pob_T2_I = log->pob_I_L1 || log->pob_I_L2 || log->pob_I_L3 || log->pob_I_3f;
		log->pob_T2_W = log->pob_W_L1 || log->pob_W_L2 || log->pob_W_L3 || log->pob_W_3f;

		//obsluga wejscia testu przekaznika
		if (czy_test_R() && test)
		{
			log->pob_I_L1 = 1;
			log->pob_I_L2 = 1;
			log->pob_I_L3 = 1;
			log->pob_W_L1 = 1;
			log->pob_W_L2 = 1;
			log->pob_W_L3 = 1;
			
			log->pob_I_3f = 1;
			log->pob_W_3f = 1;
			
			log->pob_T2_I = 1;
			log->pob_T2_W = 1;
		}

         //realizacja blokady dzialania przekaznika
		if(blokada)
		{
			log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0;
			log->pob_I_L1 = 0;
			log->pob_I_L2 = 0;
			log->pob_I_L3 = 0;
			log->pob_W_L1 = 0;
			log->pob_W_L2 = 0;
			log->pob_W_L3 = 0;
			
			log->pob_I_3f = 0;
			log->pob_W_3f = 0;
			
			log->pob_T2_I = 0;
			log->pob_T2_W = 0;
		}
        
        log->wy_pob = log->pob_T2_I || log->pob_T2_W;
		//sekcja timerow
       
       if (log->pob_I_L1)
       {
       		if (++log->t_I_L1 > log->t1_limit)
       		{
       			log->ret_I_L1 = 1;
       			log->t_I_L1 = log->t1_limit + 10;
       		}     	
       }
       else
       {	
			log->t_I_L1 = 0;
			log->ret_I_L1 = 0;
       }
       
       if (log->pob_I_L2)
       {
       		if (++log->t_I_L2 > log->t1_limit)
       		{
       			log->ret_I_L2 = 1;
       			log->t_I_L2 = log->t1_limit + 10;
       		}     	
       }
       else
       {	
			log->t_I_L2 = 0;
			log->ret_I_L2 = 0;
       }
       
       if (log->pob_I_L3)
       {
       		if (++log->t_I_L3 > log->t1_limit)
       		{
       			log->ret_I_L3 = 1;
       			log->t_I_L3 = log->t1_limit + 10;
       		}     	
       }
       else
       {	
			log->t_I_L3 = 0;
			log->ret_I_L3 = 0;
       }
       
       if (log->pob_W_L1)
       {
       		if (++log->t_W_L1 > log->t1_limit)
       		{
       			log->ret_W_L1 = 1;
       			log->t_W_L1 = log->t1_limit + 10;
       		}     	
       }
       else
       {	
			log->t_W_L1 = 0;
			log->ret_W_L1 = 0;
       }
       
       if (log->pob_W_L2)
       {
       		if (++log->t_W_L2 > log->t1_limit)
       		{
       			log->ret_W_L2 = 1;
       			log->t_W_L2 = log->t1_limit + 10;
       		}     	
       }
       else
       {	
			log->t_W_L2 = 0;
			log->ret_W_L2 = 0;
       }      
       
       if (log->pob_W_L3)
       {
       		if (++log->t_W_L3 > log->t1_limit)
       		{
       			log->ret_W_L3 = 1;
       			log->t_W_L3 = log->t1_limit + 10;
       		}     	
       }
       else
       {	
			log->t_W_L3 = 0;
			log->ret_W_L3 = 0;
       }
       
       if (log->pob_I_3f)
       {
       		if (++log->t_I_3f > log->t1_limit)
       		{
       			log->ret_I_3f = 1;
       			log->t_I_3f = log->t1_limit + 10;
       		}     	
       }
       else
       {	
			log->t_I_3f = 0;
			log->ret_I_3f = 0;
       }
       
       if (log->pob_W_3f)
       {
       		if (++log->t_W_3f > log->t1_limit)
       		{
       			log->ret_W_3f = 1;
       			log->t_W_3f = log->t1_limit + 10;
       		}     	
       }
       else
       {	
			log->t_W_3f = 0;
			log->ret_W_3f = 0;
       }      
       
       if (log->pob_T2_I)
       {
       		if (++log->t_I_2 > log->t2_limit)
       		{
       			log->wy_T2_I = 1;
       			log->t_I_2  = log->t2_limit + 10;
       		}     	
       }
       else
       {	
			log->wy_T2_I = 0;
			log->t_I_2  = 0;
       }     
       
       if (log->pob_T2_W)
       {
       		if (++log->t_W_2 > log->t2_limit)
       		{
       			log->wy_T2_W = 1;
       			log->t_W_2  = log->t2_limit + 10;
       		}     	
       }
       else
       {	
			log->wy_T2_W = 0;
			log->t_W_2  = 0;
       }       
               
        log->wy_T1 = log->ret_I_L1 || log->ret_I_L2 || log->ret_I_L3 || log->ret_W_L1 || log->ret_W_L2 ||
        log->ret_W_L3 || log->ret_I_3f || log->ret_W_3f;
        
        log->wy_T2 = log->wy_T2_I || log->wy_T2_W;
		
		check_and_set_struct(log->ret_I_L1,&log->retrip_I_L1);
		check_and_set_struct(log->ret_I_L2,&log->retrip_I_L2);
		check_and_set_struct(log->ret_I_L3,&log->retrip_I_L3);
		check_and_set_struct(log->ret_W_L1,&log->retrip_W_L1);
		check_and_set_struct(log->ret_W_L2,&log->retrip_W_L2);
		check_and_set_struct(log->ret_W_L3,&log->retrip_W_L3);				
		check_and_set_struct(log->ret_I_3f,&log->retrip_I_3f);	
		check_and_set_struct(log->ret_W_3f,&log->retrip_W_3f);		
		check_and_set_struct(log->wy_T2_I,&log->T2_I);
		check_and_set_struct(log->wy_T2_W,&log->T2_W);
		check_and_set_struct(log->wy_T1,&log->T1);	
		check_and_set_struct(log->wy_T2,&log->T2);
		check_and_set_struct(log->wy_pob,&log->pob);	

    } 
    else
    {
		set_default(log);
		
		clear_struct(&log->retrip_I_L1);
		clear_struct(&log->retrip_I_L2);
		clear_struct(&log->retrip_I_L3);
		clear_struct(&log->retrip_W_L1);
		clear_struct(&log->retrip_W_L2);
		clear_struct(&log->retrip_W_L3);
		clear_struct(&log->retrip_I_3f);
		clear_struct(&log->retrip_W_3f);
		clear_struct(&log->T1);
		clear_struct(&log->T2);
		clear_struct(&log->T2_I);
		clear_struct(&log->T2_W);
	}
}



