/*
 * eth_ench.c
 *
 *  Created on: 17-05-2024
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "eth.h"
#include "eth_ench.h"
#include "mkstream.h"

#include <ti/ipc/Notify.h>

#include "comm.h"
#include "../ethernet/ports/am1808/include/lwiplib.h"
#include "../ethernet/emac.h"
#include "../ethernet/hw_types.h"
#include "ports/am1808/include/netif/sitaraif.h"
#include <ti/sysbios/hal/Hwi.h>
//#include <ti/sysbios/hal/Cache.h>


#define ETH_NO_TZ_CHECKSUM // don't check the checksum inside a TZ frame


int ethernet_ench_initlog(void *arguments, void *logic)
{
	struct eth_ench_args *args = (struct eth_ench_args *)arguments;
	struct eth_ench_logic *log = (struct eth_ench_logic *)logic;

	if(set_bit_ptr_struct(args->io.clr_in,&log->clr))
		return -1;

	if(set_pointer_out_ptr(args->io.stream3_ptr_out,(u32 *)&log->stream_out[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream3_act_out,&log->stream_act[0]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream4_ptr_out,(u32 *)&log->stream_out[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream4_act_out,&log->stream_act[1]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream5_ptr_out,(u32 *)&log->stream_out[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream5_act_out,&log->stream_act[2]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream6_ptr_out,(u32 *)&log->stream_out[3]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream6_act_out,&log->stream_act[3]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream7_ptr_out,(u32 *)&log->stream_out[4]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream7_act_out,&log->stream_act[4]))
		return -1;

	if(set_bit_ptr_struct(args->io.link3_out,&log->link[0]))
		return -1;
	if(set_float_ptr(args->io.stream3_errs_float_out,&log->stream_errs[0]))
		return -1;

	if(set_bit_ptr_struct(args->io.link4_out,&log->link[1]))
		return -1;
	if(set_float_ptr(args->io.stream4_errs_float_out,&log->stream_errs[1]))
		return -1;

	if(set_bit_ptr_struct(args->io.link5_out,&log->link[2]))
		return -1;
	if(set_float_ptr(args->io.stream5_errs_float_out,&log->stream_errs[2]))
		return -1;

	if(set_bit_ptr_struct(args->io.link6_out,&log->link[3]))
		return -1;
	if(set_float_ptr(args->io.stream6_errs_float_out,&log->stream_errs[3]))
		return -1;

	if(set_bit_ptr_struct(args->io.link7_out,&log->link[4]))
		return -1;
	if(set_float_ptr(args->io.stream7_errs_float_out,&log->stream_errs[4]))
		return -1;

	if(set_float_ptr(args->io.stream3_synt_errs_float_out,&log->stream_synt_errs[0]))
		return -1;
	if(set_float_ptr(args->io.stream4_synt_errs_float_out,&log->stream_synt_errs[1]))
		return -1;
	if(set_float_ptr(args->io.stream5_synt_errs_float_out,&log->stream_synt_errs[2]))
		return -1;
	if(set_float_ptr(args->io.stream6_synt_errs_float_out,&log->stream_synt_errs[3]))
		return -1;
	if(set_float_ptr(args->io.stream7_synt_errs_float_out,&log->stream_synt_errs[4]))
		return -1;

	log->s_delay=args->params.sample_delay/LOOP_CYCLE_MS;
	if(args->params.sample_delay%LOOP_CYCLE_MS)
		log->s_delay++;

	log->s_delay*=(SAMPLES_PER_MS*LOOP_CYCLE_MS);

	if(log->s_delay>=((ETH_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS)))
		log->s_delay=(ETH_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS);

	return 0;
}

void ethernet_ench(void *arguments, void *logic)
{
	struct eth_ench_logic *log = (struct eth_ench_logic *)logic;
	struct eth_ench_args *args = (struct eth_ench_args *)arguments;

	u8 i,k;
#ifndef ETH_NO_TZ_CHECKSUM
	u8 j,cksum;
#endif
	u16 find_stamp;
	u8 already_incremented[ETH_ENCH_STREAMS]={0,0,0,0,0};
    //struct str_eth_frame *eth_str = (struct str_eth_frame *)r87l_tx_packet;

	if(check_struct(&log->clr))
	{
		for(i=0;i<ETH_ENCH_STREAMS;i++)
		{
			*log->stream_errs[i]=0;
			*log->stream_synt_errs[i]=0;
		}
	}

	for(i=0;i<ETH_ENCH_STREAMS;i++)
	{
		if(log->prev_pos[i]!=str_pos[i+ETH_MAX_STREAMS])
		{
			if(log->link_up_cnt[i]>LINK_UP_TIME)
			{
				if(!check_struct(&log->link[i]))
					*log->stream_errs[i]=0;
				set_struct(&log->link[i]);
			}
			else
				log->link_up_cnt[i]+=LOOP_CYCLE_MS;

			while(log->prev_pos[i]!=str_pos[i+ETH_MAX_STREAMS])
			{
				u16 prev_tstamp=(!log->prev_pos[i]?str[i+ETH_MAX_STREAMS][ETH_STREAM_BUF_SIZE-1].sample_stamp:str[i+ETH_MAX_STREAMS][log->prev_pos[i]-1].sample_stamp) & 0x7FFF;
				u16 next_tstamp=(str[i+ETH_MAX_STREAMS][log->prev_pos[i]].sample_stamp)&0x7FFF;

				if((prev_tstamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=next_tstamp)
				{
					if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp)
						*log->stream_errs[i]+=1.0f;
					already_incremented[i]=1;
				}
				log->prev_pos[i]++;
				log->prev_pos[i]%=ETH_STREAM_BUF_SIZE;
			}

			log->link_timeout_cnt[i]=0;
			log->prev_pos[i]=str_pos[i+ETH_MAX_STREAMS];
		}
		else
		{
			if(log->link_timeout_cnt[i]>LINK_TIMEOUT_TIME)
			{
				clear_struct(&log->link[i]);
				log->link_up_cnt[i]=0;
			}
			else
			{
				if(log->link_timeout_cnt[i]>(1*LOOP_CYCLE_MS)) // at least 2ms between samples
				{
					if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp)
						*log->stream_errs[i]+=1.0f;
					already_incremented[i]=1;
				}
				log->link_timeout_cnt[i]+=LOOP_CYCLE_MS;
			}
		}
	}

	if(bus_an_cur_timestamp>=log->s_delay/*ETH_BUF_DELAY*/)
		find_stamp=bus_an_cur_timestamp-log->s_delay;//ETH_BUF_DELAY;
	else
		find_stamp=(SAMPLES_PER_MS*1000)+bus_an_cur_timestamp-log->s_delay;//ETH_BUF_DELAY;

	for(i=0;i<ETH_ENCH_STREAMS;i++)
	{
		if(!check_struct(&log->link[i]))
		{
			clear_struct(&log->stream_act[i]);
			continue;
		}
		for(k=0;k<ETH_STREAM_BUF_SIZE;k++)
		{
			if((args->params.bits & ETH_NO_SYNC || str[i+ETH_MAX_STREAMS][k].sample_stamp == find_stamp) && !(str_bits[i+ETH_MAX_STREAMS][k] & STATUS_FRAME_READ))
			{
#ifndef ETH_NO_TZ_CHECKSUM
				cksum=CKSUM_IV;
				for(j=0;j<sizeof(struct stream_frame) - 1;j++)
					cksum+=*((u8*)&str[i+ETH_MAX_STREAMS][k]+j);
#endif
				str_bits[i+ETH_MAX_STREAMS][k]|=STATUS_FRAME_READ;

#ifndef ETH_NO_TZ_CHECKSUM
				if(cksum == str[i+ETH_MAX_STREAMS][k].cksum)
#endif
				{
					memcpy((u8*)&log->stream_out[i],(u8*)&str[i+ETH_MAX_STREAMS][k],sizeof(struct stream_frame));
					break;
				}
			}
		}

		if(k<ETH_STREAM_BUF_SIZE)
			set_struct(&log->stream_act[i]);
		else
		{
			clear_struct(&log->stream_act[i]);
			if(!already_incremented[i]) // wyrzucono bledy od synT z licznikow
				*log->stream_synt_errs[i]+=1.0f;
		}
	}

	log->prev_timestamp=bus_an_cur_timestamp;
}
