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

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

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

int e_pq_a_initlog(void *arguments, void *logic)
{
	struct e_pq_a_args *args = (struct e_pq_a_args *)arguments;
	struct e_pq_a_logic *log = (struct e_pq_a_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->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->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_a_100hz(void *arguments, void *logic)
{
	struct e_pq_a_logic *log = (struct e_pq_a_logic *)logic;

	float P,Q;
		P = 1.73205f *
		   (*log->i1_orta * *log->u1_orta +
			*log->i1_ortb * *log->u1_ortb +
			*log->i2_orta * *log->u2_orta +
			*log->i2_ortb * *log->u2_ortb)
		;
		Q = 1.73205f *
		   (*log->i1_ortb * *log->u1_orta -
			*log->i1_orta * *log->u1_ortb +
			*log->i2_ortb * *log->u2_orta -
			*log->i2_orta * *log->u2_ortb)
		;

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