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

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

#include "e_pq_1f.h"
#include "analog_in.h"
#include <math.h>

int e_pq_1f_initlog(void *arguments, void *logic)
{
	struct e_pq_1f_args *args = (struct e_pq_1f_args *)arguments;
	struct e_pq_1f_logic *log = (struct e_pq_1f_logic *)logic;

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

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

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

	log->u_ortb = (float *)(log_manager.nets_data + (args->io.u_ortb_float_in >> 3));
	if((u8 *)log->u_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;

	return 0;
}

void e_pq_1f_100hz(void *arguments, void *logic)
{
	struct e_pq_1f_logic *log = (struct e_pq_1f_logic *)logic;

	float P,Q;
		P = *log->i_orta * *log->u_orta +
			*log->i_ortb * *log->u_ortb;
		Q =
			*log->i_ortb * *log->u_orta -
			*log->i_orta * *log->u_ortb;


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