/*
 * comp_s.c
 *
 *  Created on: 08-05-2014
 *      Author: Krzysztof Jakubczyk
 */

#include "tdefs.h"
#include "misc.h"
#include "helper.h"
#include "analog_in.h"

#include "comp_s.h"

int comp_s_initlog(void *arguments, void *logic)
{
	struct comp_s_args *args = (struct comp_s_args *)arguments;
	struct comp_s_logic *log = (struct comp_s_logic *)logic;
	struct analog_in_params *an_params;
	
	u32 *an_params_ptr;
	u32 *buf_ptr;

	if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no))
		return -1;

	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->nast_.limit1_ = args->params.Wr_ *  args->params.mnoz_; // wyliczenie wartosci rozruchowej algorytmu
	log->nast_.l_powt_pob = args->params.l_powt_pob;

	return 0;
}

void comp_s(void *arguments, void *logic)
{
//	struct comp_s_args *args = (struct comp_s_args *)arguments;
	struct comp_s_logic *log = (struct comp_s_logic *)logic;

	short i,i1,i2;
	float pr;

	short pobud = 0;
	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] - 0x7FFF);
		pr*=log->an_params->multiplier;

		if (pr < 0) pr = 0 - pr;
		
		if (pr > log->nast_.limit1_)
			pobud ++;		
		
	}
		if (pobud >=  log->nast_.l_powt_pob)
			log->pob = 1;
		else
			log->pob = 0;

		check_and_set(log->pob,log->P,log->P_bit_no);
}