/*
 * e_pq_3f.c
 *
 *  Created on: 06-07-2016
 *      Author: Krzysztof Jakubczyk
 */

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

#include "e_pq_3f.h"
#include "analog_in.h"
#include "helper.h"
#include <math.h>

#define _1_PRZEZ_2_PIERW_Z_3	0.288675135
#define PIERW_Z_3				1.732050808

int e_pq_3f_initlog(void *arguments, void *logic)
{
	struct e_pq_3f_args *args = (struct e_pq_3f_args *)arguments;
	struct e_pq_3f_logic *log = (struct e_pq_3f_logic *)logic;

	log->i1_orta = (float *)(log_manager.nets_data + (args->io.i1_orta_float_in >> 3));
	if((u8 *)log->i1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->i1_ortb = (float *)(log_manager.nets_data + (args->io.i1_ortb_float_in >> 3));
	if((u8 *)log->i1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->i2_orta = (float *)(log_manager.nets_data + (args->io.i2_orta_float_in >> 3));
	if((u8 *)log->i2_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->i2_ortb = (float *)(log_manager.nets_data + (args->io.i2_ortb_float_in >> 3));
	if((u8 *)log->i2_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->i3_orta = (float *)(log_manager.nets_data + (args->io.i3_orta_float_in >> 3));
	if((u8 *)log->i3_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->i3_ortb = (float *)(log_manager.nets_data + (args->io.i3_ortb_float_in >> 3));
	if((u8 *)log->i3_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->u1_orta = (float *)(log_manager.nets_data + (args->io.u1_orta_float_in >> 3));
	if((u8 *)log->u1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->u1_ortb = (float *)(log_manager.nets_data + (args->io.u1_ortb_float_in >> 3));
	if((u8 *)log->u1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->u2_orta = (float *)(log_manager.nets_data + (args->io.u2_orta_float_in >> 3));
	if((u8 *)log->u2_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->u2_ortb = (float *)(log_manager.nets_data + (args->io.u2_ortb_float_in >> 3));
	if((u8 *)log->u2_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->u3_orta = (float *)(log_manager.nets_data + (args->io.u3_orta_float_in >> 3));
	if((u8 *)log->u3_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->u3_ortb = (float *)(log_manager.nets_data + (args->io.u3_ortb_float_in >> 3));
	if((u8 *)log->u3_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->p = (float *)(log_manager.nets_data + (args->io.p_float_out >> 3));
	if((u8 *)log->p > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->q = (float *)(log_manager.nets_data + (args->io.q_float_out >> 3));
	if((u8 *)log->q > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

// dodatkowe we 2019.06.03
	if(set_pointer_in_ptr(args->io.I_params_ptr_in,(u32 *)&log->I_params))
		return -1;
	if(set_pointer_in_ptr(args->io.U_params_ptr_in,(u32 *)&log->U_params))
		return -1;
	if(set_pointer_out_ptr(args->io.P_params_ptr_out,(u32*)&log->P_params))
		return -1;

	log->P_params.znam_pierw = PIERW_Z_3 * log->I_params->znam_pierw * log->U_params->znam_pierw;
	log->P_params.znam_wtor = PIERW_Z_3 * log->I_params->znam_wtor * log->U_params->znam_wtor;

	return 0;
}

void e_pq_3f_100hz(void *arguments, void *logic)
{
	struct e_pq_3f_logic *log = (struct e_pq_3f_logic *)logic;

	float P,Q;

		P =
			*log->i1_orta * *log->u1_orta +
			*log->i1_ortb * *log->u1_ortb +
			*log->i2_orta * *log->u2_orta +
			*log->i2_ortb * *log->u2_ortb +
			*log->i3_orta * *log->u3_orta +
			*log->i3_ortb * *log->u3_ortb;
		Q =
			*log->i1_ortb * *log->u1_orta -
			*log->i1_orta * *log->u1_ortb +
			*log->i2_ortb * *log->u2_orta -
			*log->i2_orta * *log->u2_ortb +
			*log->i3_ortb * *log->u3_orta -
			*log->i3_orta * *log->u3_ortb;

		if ((fabs(P) > 0.001f) || (fabs(Q) > 0.001f))
		{
			*log->p=P * _1_PRZEZ_2_PIERW_Z_3;
			*log->q=Q * _1_PRZEZ_2_PIERW_Z_3;
		} else {
			*log->p=0;
			*log->q=0;
		}
}
