/*
 * e_mulcon.c // Mnozenie wektora przez stala
 *
 *  Created on: 10-06-2016
 *      Author: Krzysztof Jakubczyk
 */

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

#include "e_mulcon.h"
#include <math.h>

int e_mulcon_initlog(void *arguments, void *logic)
{
	struct e_mulcon_args *args = (struct e_mulcon_args *)arguments;
	struct e_mulcon_logic *log = (struct e_mulcon_logic *)logic;

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

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

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

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

	log->orta_ = args->params.value * cos(args->params.phase * (float)3.1416/180);
	log->ortb_ = args->params.value * sin(args->params.phase * (float)3.1416/180);

	return 0;
}

void e_mulcon(void *arguments, void *logic)
{
	struct e_mulcon_args *args = (struct e_mulcon_args *)arguments;
	struct e_mulcon_logic *log = (struct e_mulcon_logic *)logic;

	float orta, ortb;

	orta = *log->orta_in_ptr * log->orta_ + *log->ortb_in_ptr * log->ortb_;
	ortb = *log->ortb_in_ptr * log->orta_ - *log->orta_in_ptr * log->ortb_;

	if(args->io.esk_float_out)
		*log->esk_ptr=(orta*orta + ortb*ortb)/2;

	if(args->io.orta_float_out)
		*log->orta_out_ptr = orta;

	if(args->io.ortb_float_out)
		*log->ortb_out_ptr = ortb;
}
