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

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

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

int e_rms_1t_initlog(void *arguments, void *logic)
{
	struct e_rms_1t_args *args = (struct e_rms_1t_args *)arguments;
	struct e_rms_1t_logic *log = (struct e_rms_1t_logic *)logic;

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

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

	return 0;
}

void e_rms_1t(void *arguments, void *logic)
{
	struct e_rms_1t_args *args = (struct e_rms_1t_args *)arguments;
	struct e_rms_1t_logic *log = (struct e_rms_1t_logic *)logic;

	float probka_ = *log->probka_ptr;
	float local = probka_ * probka_;
	float kwskut;

	log->dw.sumam += local;
	log->dw.sumap += local;
	log->dw.wskm++;
	log->dw.wskp++;

	if ( (probka_ * log->dw.last) < 0)
	{
		log->dw.liczz = 0;
		if (probka_<0)
		{
			if (log->dw.wskp>10)
			{
				kwskut = log->dw.sumap / log->dw.wskp;
				if(args->io.rms_1t_kw_float_out)
					*log->rms_kw_ptr = kwskut;
				log->dw.sumap = 0;
				log->dw.wskp = 0;

				if(args->io.rms_1t_float_out)
					*log->rms_ptr = sqrtf(kwskut);
			}
		}
		else
		{
			if (log->dw.wskm>10)
			{
				kwskut = log->dw.sumam / log->dw.wskm;
				if(args->io.rms_1t_kw_float_out)
					*log->rms_kw_ptr = kwskut;
				log->dw.sumam = 0;
				log->dw.wskm = 0;


				if(args->io.rms_1t_float_out)
					*log->rms_ptr = sqrtf(kwskut);
			}
		}
	}

	if (local < (float)0.000625)
	{
		log->dw.liczz++;
	}

	if(log->dw.liczz > 60)
	{
		log->dw.sumap = 0;
		log->dw.wskp = 0;
		log->dw.sumam = 0;
		log->dw.wskm = 0;
		log->dw.liczz = 0;
		kwskut=0;
		if(args->io.rms_1t_kw_float_out)
			*log->rms_kw_ptr = kwskut;

		if(args->io.rms_1t_float_out)
			*log->rms_ptr = sqrtf(kwskut);
	}

	if (log->dw.wskp>500)
	{
		kwskut = log->dw.sumap / log->dw.wskp;
		if(args->io.rms_1t_kw_float_out)
			*log->rms_kw_ptr = kwskut;

		if(args->io.rms_1t_float_out)
			*log->rms_ptr = sqrtf(kwskut);
		log->dw.sumap = 0;
		log->dw.wskp = 0;
		log->dw.liczz = 0;
	}

	if (log->dw.wskm>500)
	{
		kwskut = log->dw.sumam / log->dw.wskm;
		if(args->io.rms_1t_kw_float_out)
			*log->rms_kw_ptr = kwskut;

		if(args->io.rms_1t_float_out)
			*log->rms_ptr = sqrtf(kwskut);
		log->dw.sumam = 0;
		log->dw.wskm = 0;
		log->dw.liczz = 0;
	}

	log->dw.last = probka_;
}