/*
 * e_sc_p.c
 *
 *  Created on: 14-06-2016
 *      Author: Krzysztof Jakubczyk
 */

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

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

int e_sc_p_initlog(void *arguments, void *logic)
{
	struct e_sc_p_args *args = (struct e_sc_p_args *)arguments;
	struct e_sc_p_logic *log = (struct e_sc_p_logic *)logic;

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

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

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

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

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

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

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

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

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

	return 0;
}

void e_sc_p(void *arguments, void *logic)
{
//	struct e_sc_p_args *args = (struct e_sc_p_args *)arguments;
	struct e_sc_p_logic *log = (struct e_sc_p_logic *)logic;

	float orta, ortb, ortaL1, ortbL1, ortaL2, ortbL2, ortaL3, ortbL3;

	ortaL1 = *log->orta_L1_ptr;
	ortbL1 = *log->ortb_L1_ptr;
	ortaL2 = *log->orta_L2_ptr;
	ortbL2 = *log->ortb_L2_ptr;
	ortaL3 = *log->orta_L3_ptr;
	ortbL3 = *log->ortb_L3_ptr;

    orta = ortaL1 - 0.5f * ortaL2 + 0.866f * ortbL2 - 0.5f * ortaL3 - 0.866f * ortbL3;
    orta *= 0.3333f; // wyliczenie skladowej ortogonalnej a skladowej symetrycznej zgodnej
    ortb = ortbL1 - 0.866f * ortaL2 - 0.5f * ortbL2 + 0.866f * ortaL3 - 0.5f * ortbL3;
    ortb *= 0.3333f; // wyliczenie skladowej ortogonalnej b skladowej symetrycznej zgodnej

	*log->esk_ptr=(orta*orta + ortb*ortb)*0.5f;
	*log->orta_ptr = orta;
	*log->ortb_ptr = ortb;
}
