/*
 * cap_comp.c
 *
 *  Created on: 19-08-2019
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "cap_comp.h"
#include "analog_in.h"

int cap_comp_initlog(void *arguments, void *logic)
{
	struct cap_comp_args *args = (struct cap_comp_args *)arguments;
	struct cap_comp_logic *log = (struct cap_comp_logic *)logic;
/*
	if(set_float_ptr(args->io.IL1_float_in,&log->I[0]))
		return -1;
	if(set_float_ptr(args->io.IL2_float_in,&log->I[1]))
		return -1;
	if(set_float_ptr(args->io.IL3_float_in,&log->I[2]))
		return -1;

	if(set_float_ptr(args->io.UL1_float_in,&log->U[0]))
		return -1;
	if(set_float_ptr(args->io.UL2_float_in,&log->U[1]))
		return -1;
	if(set_float_ptr(args->io.UL3_float_in,&log->U[2]))
		return -1;
*/
	if(set_pointer_in_ptr(args->io.IL1_buf_ptr_in,(u32*)&log->IL_buf[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.IL2_buf_ptr_in,(u32*)&log->IL_buf[1]))
		return -1;
	if(set_pointer_in_ptr(args->io.IL3_buf_ptr_in,(u32*)&log->IL_buf[2]))
		return -1;

	if(set_pointer_in_ptr(args->io.UL1_buf_ptr_in,(u32*)&log->UL_buf[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.UL2_buf_ptr_in,(u32*)&log->UL_buf[1]))
		return -1;
	if(set_pointer_in_ptr(args->io.UL3_buf_ptr_in,(u32*)&log->UL_buf[2]))
		return -1;

	if(set_float_ptr(args->io.IL1_comp_float_out,&log->I_comp[0]))
		return -1;
	if(set_float_ptr(args->io.IL2_comp_float_out,&log->I_comp[1]))
		return -1;
	if(set_float_ptr(args->io.IL3_comp_float_out,&log->I_comp[2]))
		return -1;

	if(set_float_ptr(args->io.IL1_comp_prev_float_out,&log->I_comp_prev[0]))
		return -1;
	if(set_float_ptr(args->io.IL2_comp_prev_float_out,&log->I_comp_prev[1]))
		return -1;
	if(set_float_ptr(args->io.IL3_comp_prev_float_out,&log->I_comp_prev[2]))
		return -1;

	if(set_pointer_in_ptr(args->io.IL1_params_ptr_in,(u32 *)&log->I_params[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.IL2_params_ptr_in,(u32 *)&log->I_params[1]))
		return -1;
	if(set_pointer_in_ptr(args->io.IL3_params_ptr_in,(u32 *)&log->I_params[2]))
		return -1;

	if(set_pointer_in_ptr(args->io.UL1_params_ptr_in,(u32 *)&log->U_params[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.UL2_params_ptr_in,(u32 *)&log->U_params[1]))
		return -1;
	if(set_pointer_in_ptr(args->io.UL3_params_ptr_in,(u32 *)&log->U_params[2]))
		return -1;

	//po staremu log->mul=args->params.k*args->params.c/2.0f*(1 / log->I_params[0]->znam_pierw);
	log->mul=args->params.k*args->params.c/2.0f*((log->U_params[0]->znam_pierw/log->U_params[0]->znam_wtor)*(log->I_params[0]->znam_wtor/log->I_params[0]->znam_pierw));
	return 0;
}

void cap_comp(void *arguments, void *logic)
{
	struct cap_comp_logic *log = (struct cap_comp_logic *)logic;
	struct cap_comp_args *args = (struct cap_comp_args *)arguments;
	u8 i;
	short snum_next = bus_an_cur_sample_num;
	short snum_cur,snum_prev;
	short snum_next2, snum_cur2,snum_prev2;

	snum_cur = snum_next - 1;
	if(snum_cur < 0)
		snum_cur = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1;

	snum_prev = snum_cur - 1;
	if(snum_prev < 0)
		snum_prev = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1;

	snum_next2 = snum_next-SAMPLES_PER_MS;
	if(snum_next2 < 0)
		snum_next2 = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-SAMPLES_PER_MS;

	snum_cur2 = snum_next2 - 1;
	if(snum_cur2 < 0)
		snum_cur2 = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1;

	snum_prev2 = snum_cur2 - 1;
	if(snum_prev2 < 0)
		snum_prev2 = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1;


	if(args->params.bits & 0x01)
	{
		for(i=0;i<3;i++)
		{
			float u_next = (((float)(log->UL_buf[i][snum_next] - 0x7FFF)) - log->U_params[i]->offset ) * log->U_params[i]->multiplier;
			float u_prev = (((float)(log->UL_buf[i][snum_prev] - 0x7FFF)) - log->U_params[i]->offset ) * log->U_params[i]->multiplier;
			float i_cur= (((float)(log->IL_buf[i][snum_cur] - 0x7FFF)) - log->I_params[i]->offset ) * log->I_params[i]->multiplier;
			*log->I_comp[i]=i_cur-((u_next-u_prev)*log->mul);

			u_next = (((float)(log->UL_buf[i][snum_next2] - 0x7FFF)) - log->U_params[i]->offset ) * log->U_params[i]->multiplier;
			u_prev = (((float)(log->UL_buf[i][snum_prev2] - 0x7FFF)) - log->U_params[i]->offset ) * log->U_params[i]->multiplier;
			i_cur= (((float)(log->IL_buf[i][snum_cur2] - 0x7FFF)) - log->I_params[i]->offset ) * log->I_params[i]->multiplier;
			*log->I_comp_prev[i]=i_cur-((u_next-u_prev)*log->mul);
		}
	}
	else
	{
		for(i=0;i<3;i++)
		{
			*log->I_comp[i]=(((float)(log->IL_buf[i][snum_cur] - 0x7FFF)) - log->I_params[i]->offset ) * log->I_params[i]->multiplier;
			*log->I_comp_prev[i]=(((float)(log->IL_buf[i][snum_cur2] - 0x7FFF)) - log->I_params[i]->offset ) * log->I_params[i]->multiplier;
		}
	}
}
