/*
 * e_phi.c
 *
 *  Created on: 11-02-2017
 *      Author: Krzysztof Jakubczyk
 */

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

#include "e_phi.h"
#include "helper.h"
#include <math.h>

int e_phi_initlog(void *arguments, void *logic)
{
	struct e_phi_args *args = (struct e_phi_args *)arguments;
	struct e_phi_logic *log = (struct e_phi_logic *)logic;

	if(set_float_ptr(args->io.orta_a_float_in,&log->orta_a))
		return -1;
	if(set_float_ptr(args->io.ortb_a_float_in,&log->ortb_a))
		return -1;
	if(set_float_ptr(args->io.orta_b_float_in,&log->orta_b))
		return -1;
	if(set_float_ptr(args->io.ortb_b_float_in,&log->ortb_b))
		return -1;
	if(set_float_ptr(args->io.phi_float_out,&log->phi))
		return -1;

	return 0;
}

#define PI 3.14159265

float get_phase(float cos, float sin);
float get_phase_diff(float phi1, float phi2);

void e_phi_20hz(void *arguments, void *logic)
{
	struct e_phi_logic *log = (struct e_phi_logic *)logic;

	float phi1,phi2;

	phi1=get_phase(*log->ortb_a,*log->orta_a);
	phi2=get_phase(*log->ortb_b,*log->orta_b);

	*log->phi=get_phase_diff(phi1,phi2);
}
