/*
 * 21g.c
 *
 *  Created on: 07-05-2018
 *      Author: Krzysztof Jakubczyk
 */

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

#include "21g.h"
#include "helper.h"
#include "analog_in.h"
#include "config.h"
#include <math.h>

int r21g_initlog(void *arguments, void *logic)
{
	struct r21g_args *args = (struct r21g_args *)arguments;
	struct r21g_logic *log = (struct r21g_logic *)logic;

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

	if(set_float_ptr(args->io.U_esk_float_in,&log->U_esk))
		return -1;
	if(set_float_ptr(args->io.U_orta_float_in,&log->U_orta))
		return -1;
	if(set_float_ptr(args->io.U_ortb_float_in,&log->U_ortb))
		return -1;

	if(set_float_ptr(args->io.I_esk_float_in,&log->I_esk))
		return -1;
	if(set_float_ptr(args->io.I_orta_float_in,&log->I_orta))
		return -1;
	if(set_float_ptr(args->io.I_ortb_float_in,&log->I_ortb))
		return -1;

	if(set_pointer_in_ptr(args->io.U_param_ptr_in,(u32 *)&log->U_param))
		return -1;

	if(set_pointer_in_ptr(args->io.I_param_ptr_in,(u32 *)&log->I_param))
		return -1;

	if(set_bit_ptr_struct(args->io.p_out,&log->P))
		return -1;
	if(set_bit_ptr_struct(args->io.z_out,&log->Z))
		return -1;
	if(set_bit_ptr_struct(args->io.w_out,&log->W))
		return -1;

	if(set_float_ptr(args->io.y_float_out,&log->y))
		return -1;
	if(set_float_ptr(args->io.g_float_out,&log->g))
		return -1;
	if(set_float_ptr(args->io.b_float_out,&log->b))
		return -1;

	log->przek = log->I_param->znam_wtor / log->U_param->znam_wtor;
	log->y_rozr = (float)0.001 * 0.001 * args->params.yr * args->params.yr;// * log->przek * log->przek;
	log->y_powr = log->y_rozr * args->params.kp * args->params.kp;

	log->g_rozr = (float)0.001 * args->params.gr;// * log->przek;
	log->g_powr = log->g_rozr * args->params.kp;

	log->b_rozr = (float)0.001 * args->params.br;// * log->przek;
	log->b_powr = log->b_rozr * args->params.kp;

	log->umin_rozr = args->params.umin * args->params.umin;
	log->umin_powr = log->umin_rozr * args->params.kp * args->params.kp;

	log->imin_rozr = args->params.imin * args->params.imin;
	log->imin_powr = log->imin_rozr * args->params.kp * args->params.kp;

	log->td=(u32)(args->params.td*1000);


	log->cos_shift = cosf(args->params.phi_corr*PI/180);
	log->sin_shift = sinf(args->params.phi_corr*PI/180);

	return 0;
}

void r21g(void *arguments, void *logic)
{
	struct r21g_logic *log = (struct r21g_logic *)logic;
	struct r21g_args *args = (struct r21g_args *)arguments;
	float y,g,b,m,g_new;

	if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked
	{
		if(!*log->U_esk)
			y=*log->I_esk / 0.000000001;
		else
			y=*log->I_esk / *log->U_esk;

		m=((*log->U_ortb * *log->U_ortb) + (*log->U_orta * *log->U_orta));

		if(!m)
			m=0.000000001;

		g=((*log->I_ortb * *log->U_ortb) + (*log->I_orta * *log->U_orta))/m;
		b=((*log->I_orta * *log->U_ortb) - (*log->I_ortb * *log->U_orta))/m;
//sprawdzic dzielenie przez zero

		y*=log->przek*log->przek;
		g*=log->przek;
		b*=log->przek;

		g_new=(log->cos_shift*g) - (log->sin_shift*b);
		b=(log->sin_shift*g) + (log->cos_shift*b);
		g=g_new;

		*log->y=y;
		*log->g=g;
		*log->b=b;
		//g=

		sprawdz_P(&log->pobi,
			*log->I_esk>log->imin_rozr,
			*log->I_esk<log->imin_powr,
			&log->lpi,
			15,3);
		sprawdz_P(&log->pobu,
			*log->U_esk>log->umin_rozr,
			*log->U_esk<log->umin_powr,
			&log->lpu,
			15,3);

		if(log->pobu && log->pobi)
		{
			sprawdz_P(&log->poby,
				y>log->y_rozr,
				y<log->y_powr,
				&log->lpy,
				20,3);

			switch(args->params.g_enum)
			{
				case S_FWD:
					sprawdz_P(&log->pobg,
						g>log->g_rozr,
						g<log->g_powr,
						&log->lpg,
						20,3);
					break;
				case S_BKW:
					g*=-1;
					sprawdz_P(&log->pobg,
						g>log->g_rozr,
						g<log->g_powr,
						&log->lpg,
						20,3);
					break;
				case S_BID:
					if(g<0)
						g*=-1;
					sprawdz_P(&log->pobg,
						g>log->g_rozr,
						g<log->g_powr,
						&log->lpg,
						20,3);
					break;
				default:
					log->pobg=0;
					break;
			}

			switch(args->params.b_enum)
			{
				case S_FWD:
					sprawdz_P(&log->pobb,
						b>log->b_rozr,
						b<log->b_powr,
						&log->lpb,
						20,3);
					break;
				case S_BKW:
					b*=-1;
					sprawdz_P(&log->pobb,
						b>log->b_rozr,
						b<log->b_powr,
						&log->lpb,
						20,3);
					break;
				case S_BID:
					if(b<0)
						b*=-1;
					sprawdz_P(&log->pobb,
						b>log->b_rozr,
						b<log->b_powr,
						&log->lpb,
						20,3);
					break;
				default:
					log->pobb=0;
					break;
			}
		}
		else
		{
			log->poby=0;
			log->pobg=0;
			log->pobb=0;
			log->lpy=0;
			log->lpg=0;
			log->lpb=0;
		}

		if(!args->params.log_enum)
		{
			log->p=1;

			if(args->params.y_enum)
				log->p&=log->poby;
			if(args->params.g_enum)
				log->p&=log->pobg;
			if(args->params.b_enum)
				log->p&=log->pobb;
		}
		else
		{
			log->p=0;

			if(args->params.y_enum)
				log->p|=log->poby;
			if(args->params.g_enum)
				log->p|=log->pobg;
			if(args->params.b_enum)
				log->p|=log->pobb;
		}

		if(!args->params.y_enum&&!args->params.g_enum&&!args->params.b_enum)
			log->p=0;

		if(log->p)
		{
			if(log->td_cnt>log->td)
				log->z=1;
			else
				log->td_cnt+=LOOP_CYCLE_MS;
		}
		else
		{
			log->td_cnt=0;
			log->z=0;
		}

		if(czy_test_R() && check_struct(&log->test))
		{
			log->p=1;
			log->z=1;
		}

		check_and_set_struct(log->p,&log->P);
		check_and_set_struct(log->z,&log->Z);

		if(args->params.bity & 0x00000004) // na wylacz
		{
			check_and_set_struct(log->z,&log->W);
		}
	}
	else
	{
		log->poby=0;
		log->pobg=0;
		log->pobb=0;
		log->lpy=0;
		log->lpg=0;
		log->lpb=0;

		clear_struct(&log->P);
		clear_struct(&log->Z);
		clear_struct(&log->W);
	}
}
