/*
 * eth_demux.c
 *
 *  Created on: 21-05-2024
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>
#include <string.h>

#include "tdefs.h"
#include "misc.h"
#include "eth.h"
#include "helper.h"
#include "eth_demux.h"
#include "analog_in.h"
#include "R87L.h"

u16 eth_comm_bits_act[8] = {0,0,0,0,0,0,0,0};

int eth_demux_initlog(void *arguments, void *logic)
{
	struct eth_demux_args *args = (struct eth_demux_args *)arguments;
	struct eth_demux_logic *log = (struct eth_demux_logic *)logic;
//	u8 i;

	if(set_pointer_in_ptr(args->io.stream1_ptr_in,(u32*)&log->stream_in[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream2_ptr_in,(u32*)&log->stream_in[1]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream3_ptr_in,(u32*)&log->stream_in[2]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream4_ptr_in,(u32*)&log->stream_in[3]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream5_ptr_in,(u32*)&log->stream_in[4]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream6_ptr_in,(u32*)&log->stream_in[5]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream7_ptr_in,(u32*)&log->stream_in[6]))
		return -1;
	if(set_pointer_in_ptr(args->io.stream8_ptr_in,(u32*)&log->stream_in[7]))
		return -1;

	if(set_bit_ptr_struct(args->io.stream1_act_in,&log->stream_in_act[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream2_act_in,&log->stream_in_act[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream3_act_in,&log->stream_in_act[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream4_act_in,&log->stream_in_act[3]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream5_act_in,&log->stream_in_act[4]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream6_act_in,&log->stream_in_act[5]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream7_act_in,&log->stream_in_act[6]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream8_act_in,&log->stream_in_act[7]))
		return -1;


	if(set_pointer_out_ptr(args->io.stream1_ptr_out,(u32*)&log->stream_out[0]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream2_ptr_out,(u32*)&log->stream_out[1]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream3_ptr_out,(u32*)&log->stream_out[2]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream4_ptr_out,(u32*)&log->stream_out[3]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream5_ptr_out,(u32*)&log->stream_out[4]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream6_ptr_out,(u32*)&log->stream_out[5]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream7_ptr_out,(u32*)&log->stream_out[6]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream8_ptr_out,(u32*)&log->stream_out[7]))
		return -1;

	if(set_bit_ptr_struct(args->io.streams_ok_out,&log->streams_ok))
		return -1;

	if(set_bit_ptr_struct(args->io.przeglad1_out,&log->przeglad[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.przeglad2_out,&log->przeglad[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.przeglad3_out,&log->przeglad[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.przeglad4_out,&log->przeglad[3]))
		return -1;
	if(set_bit_ptr_struct(args->io.przeglad5_out,&log->przeglad[4]))
		return -1;
	if(set_bit_ptr_struct(args->io.przeglad6_out,&log->przeglad[5]))
		return -1;
	if(set_bit_ptr_struct(args->io.przeglad7_out,&log->przeglad[6]))
		return -1;
	if(set_bit_ptr_struct(args->io.przeglad8_out,&log->przeglad[7]))
		return -1;

	log->jednostki = args->params.liczba_jednostek+2;

	if(log->jednostki>8)
		log->jednostki=8;

	if(set_float_ptr(args->io.endpoints_float_out,&log->endpoints))
		return -1;

	*log->endpoints=log->jednostki;

	return 0;
}

void eth_demux(void *arguments, void *logic)
{
	struct eth_demux_logic *log = (struct eth_demux_logic *)logic;

	u8 i,j,stream_ok,str_ok[8],str_ok_ndelay[8];
	stream_ok=1;

	for(i=0;i<log->jednostki;i++)
	{
		str_ok[i]=1;
		str_ok_ndelay[i]=1;

		if(check_struct(&log->stream_in_act[i]))
		{
			log->snum_cur[i]=log->stream_in[i]->sample_stamp;

			if((log->snum_prev[i]+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=log->snum_cur[i])
			{
				log->sfault_delay_cnt[i]=20;
				if(!(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD))
					stream_ok=0;
				str_ok[i]=0;
				str_ok_ndelay[i]=0;
			}
			else if(log->sfault_delay_cnt[i])
			{
				if(log->sfault_delay_cnt[i]>=LOOP_CYCLE_MS)
					log->sfault_delay_cnt[i]-=LOOP_CYCLE_MS;
				else
					log->sfault_delay_cnt[i]=0;

				if(!(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD))
					stream_ok=0;
				str_ok[i]=0;
			}

			log->ostream_ptr[i]=log->stream_in[i];
			log->snum_prev[i]=log->snum_cur[i];
		}
		else
		{
			if(!(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD))
				stream_ok=0;
			str_ok_ndelay[i]=0;
			str_ok[i]=0;
		}

		if(i>0 && log->snum_cur[i]!=log->snum_cur[i-1] && !(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD) && !(eth_comm_bits_act[i-1] & R87L_COMM_PRZEGLAD))
		{
			log->sfault_delay_cnt[i]=20;
			stream_ok=0;
			str_ok[i]=0;
			str_ok_ndelay[i]=0;
		}
	}

	for(i=0;i<log->jednostki;i++)
	{
		if(str_ok[i])
			eth_comm_bits_act[i]=log->ostream_ptr[i]->bits;

		check_and_set_struct(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD,&log->przeglad[i]);
	}

	check_and_set_struct(stream_ok,&log->streams_ok);

	if(stream_ok)
	{
		for(i=0;i<log->jednostki;i++)
		{
			if(!(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD))
				memcpy((u8 *)&log->stream_out[i],(u8 *)log->ostream_ptr[i],sizeof(struct stream_frame));
			else
			{
				memset((u8*)&log->stream_out[i],0,sizeof(log->stream_out[0]));
				log->stream_out[i].bits=(eth_comm_bits_act[i] & (R87L_COMM_PRZEGLAD|0xFF00)); // przeglad bit i GPbits przepisuj w przegladzie
			}
		}
	}
	else
	{
		for(i=0;i<log->jednostki;i++)
			if(str_ok_ndelay[i])
				memcpy((u8 *)&log->stream_out[i],(u8 *)log->ostream_ptr[i],sizeof(struct stream_frame));
			else
			{
				for(j=0;j<3;j++)
					log->stream_out[i].A[j]=32768.0f;
			}
//		memset((u8*)log->stream_out,0,sizeof(log->stream_out)); // zostawic ostatnie dane?
//		for(i=0;i<3;i++)
//			log->stream_out[i].bits=(comm_bits_act[i] & R87L_COMM_PRZEGLAD);
	}
}