/*
 * e_rms.c
 *
 *  Created on: 07-06-2016
 *      Author: Krzysztof Jakubczyk
 */

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

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

int e_rms_initlog(void *arguments, void *logic)
{
	struct e_rms_args *args = (struct e_rms_args *)arguments;
	struct e_rms_logic *log = (struct e_rms_logic *)logic;
	struct analog_in_params *an_params;
	u32 *an_params_ptr;
	u32 *buf_ptr;

	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;

	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->rms_ptr = (float *)(log_manager.nets_data + (args->io.rms_float_out >> 3));
	if((u8 *)log->rms_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

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

	log->multiplier = an_params->multiplier; /* (float)1.1107 */;
	log->multiplier *= log->multiplier;
	log->multiplier /= (MAIN_FREQ_PERIOD_MS*SAMPLES_PER_MS);//60;
	log->offset = 32767 + an_params->offset;

	return 0;
}

void e_rms(void *arguments, void *logic)
{
	struct e_rms_args *args = (struct e_rms_args *)arguments;
	struct e_rms_logic *log = (struct e_rms_logic *)logic;
	int16_t i,i1;
	float pr,skut;

	i1 = bus_an_cur_sample_num;

	skut = 0;
	for (i=0;i<(MAIN_FREQ_PERIOD_MS*SAMPLES_PER_MS);i++)
	{
		if(i1<0) i1 += (MAIN_FREQ_PERIOD_MS*SAMPLES_PER_MS)*2;

		pr = (float)log->buf_ptr[i1] - log->offset;
		pr *= pr;
		skut += pr;
		i1--;
	}

	if(args->io.rms_kw_float_out)
		*log->rms_kw_ptr=skut*log->multiplier;

	if(args->io.rms_float_out)
		*log->rms_ptr=sqrtf(skut*log->multiplier);
}
