/*
 * s_demux.c
 *
 *  Created on: 29-10-2019
 *      Author: Krzysztof Jakubczyk
 */

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

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

u16 comm_bits_act[3] = {0,0,0};

int s_demux_initlog(void *arguments, void *logic)
{
	struct s_demux_args *args = (struct s_demux_args *)arguments;
	struct s_demux_logic *log = (struct s_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.astream1_ptr_in,(u32*)&log->astream_in[0]))
		return -1;
	if(set_pointer_in_ptr(args->io.astream2_ptr_in,(u32*)&log->astream_in[1]))
		return -1;
	if(set_pointer_in_ptr(args->io.astream3_ptr_in,(u32*)&log->astream_in[2]))
		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.astream1_act_in,&log->astream_in_act[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.astream2_act_in,&log->astream_in_act[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.astream3_act_in,&log->astream_in_act[2]))
		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_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;

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

	if(log->jednostki!=2 && log->jednostki !=3)
		log->jednostki=2;

	if(set_bit_ptr_struct(args->io.three_endpoints_out,&log->three_endpoints))
		return -1;

	check_and_set_struct(log->jednostki==3,&log->three_endpoints);

	return 0;
}

void s_demux(void *arguments, void *logic)
{
	struct s_demux_logic *log = (struct s_demux_logic *)logic;

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

	alt_stream=0;

	if(log->jednostki==2)
	{
		if((!check_struct(&log->stream_in_act[0]) && !(comm_bits_act[0] & R87L_COMM_PRZEGLAD)) || (!check_struct(&log->stream_in_act[1]) && !(comm_bits_act[1] & R87L_COMM_PRZEGLAD)))
			alt_stream=1;
	}
	else if(log->jednostki==3)
	{
		if((!check_struct(&log->stream_in_act[0]) && !(comm_bits_act[0] & R87L_COMM_PRZEGLAD)) || (!check_struct(&log->stream_in_act[1]) && !(comm_bits_act[1] & R87L_COMM_PRZEGLAD))|| (!check_struct(&log->stream_in_act[2]) && !(comm_bits_act[2] & R87L_COMM_PRZEGLAD)))
			alt_stream=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]) && !alt_stream)
		{
			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(!(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(!(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(check_struct(&log->astream_in_act[i]))
		{
			log->snum_cur[i]=log->astream_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(!(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(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD))
					stream_ok=0;
				str_ok[i]=0;
			}

			log->ostream_ptr[i]=log->astream_in[i];
			log->snum_prev[i]=log->snum_cur[i];
		}
		else
		{
			if(!(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] && !(comm_bits_act[i] & R87L_COMM_PRZEGLAD) && !(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])
			comm_bits_act[i]=log->ostream_ptr[i]->bits;

		check_and_set_struct(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(!(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=(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);
	}
}
