/*
 * e_filtr2h.c
 *
 *  Created on: 22-04-2016
 *      Author: Krzysztof Jakubczyk
 */

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

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

float wsp_filtr_sin_2h[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS];
float wsp_filtr_cos_2h[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS];
int wsp_filtr_2h_initialised=0;

int e_filtr2h_initlog(void *arguments, void *logic)
{
	struct e_filtr2h_args *args = (struct e_filtr2h_args *)arguments;
	struct e_filtr2h_logic *log = (struct e_filtr2h_logic *)logic;
	struct analog_in_params *an_params;
	u32 *an_params_ptr;
	u32 *buf_ptr;
	int i;

	an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3));
	if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;

	if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before)
		return -1;

	an_params=(struct analog_in_params *)*an_params_ptr;
	log->an_params=an_params;

	buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3));
	if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;

	if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before)
		return -1;

	log->buf_ptr=(u16*)*buf_ptr;

	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;

	if(!wsp_filtr_2h_initialised)
	{
		for(i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++)
		{
			wsp_filtr_sin_2h[i] =/* an_params->multiplier **/
						sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i)  / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2);

			wsp_filtr_cos_2h[i] =/* an_params->multiplier **/
						cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2);
		}

		wsp_filtr_2h_initialised=1;
	}

	return 0;
}

void e_filtr2h(void *arguments, void *logic)
{
//	struct e_filtr2h_args *args = (struct e_filtr2h_args *)arguments;
	struct e_filtr2h_logic *log = (struct e_filtr2h_logic *)logic;
    short i,i1,i2;
	float orta = 0;
	float ortb = 0;
	float pr;

	i2 = bus_an_cur_sample_num;

	for (i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++)
	{
		i1 = i2 - i;
	    if(i1<0)
	    	i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2);
		pr = (float)log->buf_ptr[i1];
		pr*=log->an_params->multiplier;
		orta += pr * wsp_filtr_sin_2h[i];
	    ortb += pr * wsp_filtr_cos_2h[i];
	}

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