/*
 * e_filtr2h_1khz.c
 *
 *  Created on: 01-03-2017
 *      Author: Krzysztof Jakubczyk
 */

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

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

int e_filtr2h_1khz_initlog(void *arguments, void *logic)
{
	struct e_filtr2h_1khz_args *args = (struct e_filtr2h_1khz_args *)arguments;
	struct e_filtr2h_1khz_logic *log = (struct e_filtr2h_1khz_logic *)logic;
	int i;

	if(set_float_ptr(args->io.sample_float_in,&log->sample_in))
		return -1;

	if(set_float_ptr(args->io.orta_float_out,&log->orta_ptr))
		return -1;

	if(set_float_ptr(args->io.ortb_float_out,&log->ortb_ptr))
		return -1;

	if(set_float_ptr(args->io.esk_float_out,&log->esk_ptr))
		return -1;

    for(i=0;i<(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);i++)
    {
		log->wsp_filtr_sin_[i] = sin(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/2)) * i) / (10/LOOP_CYCLE_MS);
        log->wsp_filtr_cos_[i] = cos(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/2)) * i) / (10/LOOP_CYCLE_MS);
    }

	return 0;
}

void e_filtr2h_1khz(void *arguments, void *logic)
{
//	struct e_filtr2h_1khz_args *args = (struct e_filtr2h_1khz_args *)arguments;
	struct e_filtr2h_1khz_logic *log = (struct e_filtr2h_1khz_logic *)logic;
    short i,i1,i2;
	float orta = 0;
	float ortb = 0;
	float pr;

	i2 = log->sample_num;

	log->buf[i2]=*log->sample_in;

	for (i=0;i<(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);i++)
	{
		i1 = i2 - i;
	    if(i1<0)
	    	i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);
		pr = log->buf[i1];
		orta += pr * log->wsp_filtr_sin_[i];
	    ortb += pr * log->wsp_filtr_cos_[i];
	}

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

	i2++;
	i2%=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);

	log->sample_num=i2;
}
