/*
 * sv.c
 *
 *  Created on: 21-01-2021
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "eth.h"
#include "goose.h"
#include "sv.h"
#include "analog_in.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>
#include <string.h>
#include <stdio.h>

struct sv_queue svqueue = { .cur = 0, .next = 0, .lost = 0 };

void iec61850_sv_recv(struct goose_frame *gfr,int len,char type)
{
	if(!(svqueue.status[svqueue.next] & (SV_TO_PROCESS)))
	{
		if(htons(gfr->len)==len && len<=SV_MAX_FR_SIZE)
		{
			memcpy(svqueue.buf[svqueue.next],(char*)gfr,len);
			svqueue.status[svqueue.next]|=SV_TO_PROCESS;
			svqueue.next++;
			svqueue.next%=SV_QUEUE_DEPTH;
		}
	}
	else
		svqueue.lost++;
}

int sv_initlog(void *arguments, void *logic)
{
	struct sv_args *args = (struct sv_args *)arguments;
	struct sv_logic *log = (struct sv_logic *)logic;

	u8 i;
	int values[6];
	u8 dstaddr[6];

	if(set_float_ptr(args->io.sv_cnt_float_out,&log->sv_cnt))
		return -1;

	if(set_float_ptr(args->io.lost_float_out,&log->lost))
		return -1;

	if(set_float_ptr(args->io.IL1_float_out,&log->IL[0]))
		return -1;
	if(set_float_ptr(args->io.IL2_float_out,&log->IL[1]))
		return -1;
	if(set_float_ptr(args->io.IL3_float_out,&log->IL[2]))
		return -1;
	if(set_float_ptr(args->io.IL4_float_out,&log->IL[3]))
		return -1;
	if(set_float_ptr(args->io.UL1_float_out,&log->UL[0]))
		return -1;
	if(set_float_ptr(args->io.UL2_float_out,&log->UL[1]))
		return -1;
	if(set_float_ptr(args->io.UL3_float_out,&log->UL[2]))
		return -1;
	if(set_float_ptr(args->io.UL4_float_out,&log->UL[3]))
		return -1;

	if(set_pointer_out_ptr(args->io.IL1_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_I_CARD_ADDR][0][0]))
		return -1;
	if(set_pointer_out_ptr(args->io.IL2_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_I_CARD_ADDR][1][0]))
		return -1;
	if(set_pointer_out_ptr(args->io.IL3_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_I_CARD_ADDR][2][0]))
		return -1;
	if(set_pointer_out_ptr(args->io.IL4_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_I_CARD_ADDR][3][0]))
		return -1;

	if(set_pointer_out_ptr(args->io.UL1_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_U_CARD_ADDR][0][0]))
		return -1;
	if(set_pointer_out_ptr(args->io.UL2_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_U_CARD_ADDR][1][0]))
		return -1;
	if(set_pointer_out_ptr(args->io.UL3_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_U_CARD_ADDR][2][0]))
		return -1;
	if(set_pointer_out_ptr(args->io.UL4_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_U_CARD_ADDR][3][0]))
		return -1;

	if(set_pointer_out_ptr(args->io.I_params_ptr_out,(u32*)&log->I_params))
		return -1;

	if(set_pointer_out_ptr(args->io.U_params_ptr_out,(u32*)&log->U_params))
		return -1;

	if(set_bit_ptr_struct(args->io.kob_out,&log->kob))
		return -1;
	if(set_bit_ptr_struct(args->io.no_sync_out,&log->no_sync))
		return -1;

	if(set_bit_ptr_struct(args->io.IL1_qual_good_out,&log->qual_good[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.IL2_qual_good_out,&log->qual_good[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.IL3_qual_good_out,&log->qual_good[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.IL4_qual_good_out,&log->qual_good[3]))
		return -1;
	if(set_bit_ptr_struct(args->io.UL1_qual_good_out,&log->qual_good[4]))
		return -1;
	if(set_bit_ptr_struct(args->io.UL2_qual_good_out,&log->qual_good[5]))
		return -1;
	if(set_bit_ptr_struct(args->io.UL3_qual_good_out,&log->qual_good[6]))
		return -1;
	if(set_bit_ptr_struct(args->io.UL4_qual_good_out,&log->qual_good[7]))
		return -1;

	if(set_bit_ptr_struct(args->io.IL1_test_out,&log->test[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.IL2_test_out,&log->test[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.IL3_test_out,&log->test[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.IL4_test_out,&log->test[3]))
		return -1;
	if(set_bit_ptr_struct(args->io.UL1_test_out,&log->test[4]))
		return -1;
	if(set_bit_ptr_struct(args->io.UL2_test_out,&log->test[5]))
		return -1;
	if(set_bit_ptr_struct(args->io.UL3_test_out,&log->test[6]))
		return -1;
	if(set_bit_ptr_struct(args->io.UL4_test_out,&log->test[7]))
		return -1;


	svqueue.lost=0;


	log->sv_id=(char*)&args->params.strings_start;
	u8 len = strlen(log->sv_id) + 1;
	if(len%4)
		len+=4-(len%4);

	log->sv_dstaddr=log->sv_id+len;

	if( 6 == sscanf( log->sv_dstaddr, "%x-%x-%x-%x-%x-%x%*c",
	    &values[0], &values[1], &values[2],
	    &values[3], &values[4], &values[5] ) )
	{
	    for( i = 0; i < 6; ++i )
	        dstaddr[i] = (u8) values[i];
	}
	else
		return -1;

	emac_hash_add(EMAC_0_BASE,dstaddr);

	log->U_params.znam_wtor=args->params.U_znam_wtor;
	log->U_params.znam_pierw=args->params.U_znam_pierw;
	log->U_params.jednostka=0; //V

	log->I_params.znam_wtor=args->params.I_znam_wtor;
	log->I_params.znam_pierw=args->params.I_znam_pierw;
	log->I_params.jednostka=1; //A

	log->i_mul = 0.001f / log->I_params.znam_pierw;
	log->i_mul2 = 32767.0f / (/*log->I_params.znam_wtor**/args->params.I_zakr*sqrtf(2));
	log->I_params.multiplier=1.0/log->i_mul2;
	log->I_params.nast.mnoznik=log->I_params.multiplier;

	log->u_mul = 0.01f / log->U_params.znam_pierw;
	log->u_mul2 = 32767.0f / (/*log->U_params.znam_wtor**/args->params.U_zakr*sqrtf(2));
	log->U_params.multiplier=1.0/log->u_mul2;
	log->U_params.nast.mnoznik=log->U_params.multiplier;

	return 0;
}


void sv(void *arguments, void *logic)
{
	struct sv_logic *log = (struct sv_logic *)logic;
	struct sv_args *args = (struct sv_args *)arguments;
	struct goose_frame *gfr;
	u8 *pdu;
	struct sv_seqdata *sv_data_ptr;
	u8 *pdu_prev;
	u8 matched_crit;
	int len,pdu_len;
	int bytes_left, bytes_left_prev;
	int err;
	u8 tag;
	u8 i;
	u32 cfg_rev;
	u32 smp_cnt;
	u32 smp_synch;
	u32 snum;

	*log->sv_cnt=log->sv_processed;
	*log->lost=svqueue.lost + log->lost_smp_cnt;

	while(svqueue.status[svqueue.cur] & SV_TO_PROCESS)
	{
		err = 0;
		gfr = (struct goose_frame *)svqueue.buf[svqueue.cur];

		if(htons(gfr->appid)==args->params.sv_appid)
		{
			matched_crit = 0;
			bytes_left=htons(gfr->len)-8;
			pdu = (u8 *)&gfr->pdu_tag;

			while(bytes_left>=2 && !err)
			{
				len = asn_parse(&pdu,&bytes_left,&tag);

				if(len<0)
				{
					err=1;
					break;
				}

				if(tag==SV_PDU)
				{
					pdu_len = len;
					pdu_prev = pdu;
					bytes_left_prev = bytes_left;
					bytes_left = pdu_len;

					while(bytes_left>=2 && !err)
					{
						len = asn_parse(&pdu,&bytes_left,&tag);
						if(len<0)
						{
							err=1;
							break;
						}

						switch(tag)
						{
							case SV_ASDU:
								while(bytes_left>=2 && !err)
								{
									len = asn_parse(&pdu,&bytes_left,&tag);
									if(len<0)
									{
										err=1;
										break;
									}
									switch(tag)
									{
										case SV_ASDU_DATA:
											break;

										case SV_TAG_ID:

											// process data (compare LEN bytes with STR)
											if(len!=strlen(log->sv_id) || memcmp(log->sv_id,pdu,len))
												err=1;
											else
												matched_crit++;
											pdu+=len;
											bytes_left-=len;
											break;

										case SV_TAG_CONFREF:
											if(len>4)
												err=1;
											else
											{
												cfg_rev=0;

												for(i=0;i<len;i++)
												{
													cfg_rev|=*pdu;
													pdu++;

													if((i+1)<len)
														cfg_rev<<=8;
												}

												if(args->params.sv_cfg_rev==cfg_rev)
													matched_crit++;
												else
													err=1;
											}

											bytes_left-=len;
											break;

										case SV_TAG_SMPCNT:
											if(len>4)
												err=1;
											else
											{
												smp_cnt=0;

												for(i=0;i<len;i++)
												{
													smp_cnt|=*pdu;
													pdu++;

													if((i+1)<len)
														smp_cnt<<=8;
												}

												matched_crit++;
											}

											bytes_left-=len;
											break;

										case SV_TAG_SMPSYNCH:
											if(len>4)
												err=1;
											else
											{
												smp_synch=0;

												for(i=0;i<len;i++)
												{
													smp_synch|=*pdu;
													pdu++;

													if((i+1)<len)
														smp_synch<<=8;
												}

												matched_crit++;
											}

											bytes_left-=len;
											break;

										case SV_TAG_SEQDATA:
											sv_data_ptr=(struct sv_seqdata *)pdu;
											matched_crit++;
											pdu+=len;
											bytes_left-=len;

											break;

										default:
											pdu+=len;
											bytes_left-=len;
											break;
									}
								}
								break;

							default:
								pdu+=len;
								bytes_left-=len;
								break;

						}
					}

					if(!err && matched_crit==5) // matched sv_id and cfgrev, smpsynch, smpcnt and seqdata
					{
						int32_t pr;
						u32 qual;

						float pr_fl;

						if(smp_synch)
							clear_struct(&log->no_sync);
						else
							set_struct(&log->no_sync);

						u8 *ptr;
						ptr=(u8*)sv_data_ptr;

						snum = (log->smp_cnt_prev+1) % (SAMPLES_PER_MS*1000);

						while(snum!=smp_cnt)
						{
							for(i=0;i<4;i++)
							{
								bus_an_samples_buf[SV_I_CARD_ADDR][i][snum%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=0+32767;
								bus_an_samples_buf[SV_U_CARD_ADDR][i][snum%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=0+32767;
							}

							snum++;
							snum%=(SAMPLES_PER_MS*1000);
							log->lost_smp_cnt++;
						}

						log->smp_cnt_prev = smp_cnt;

						for(i=0;i<8;i++)
						{
							pr=(int)ptr[0]<<24;
							pr|=(int)ptr[1]<<16;
							pr|=(int)ptr[2]<<8;
							pr|=(int)ptr[3];

							qual=(int)ptr[4]<<24;
							qual|=(int)ptr[5]<<16;
							qual|=(int)ptr[6]<<8;
							qual|=(int)ptr[7];

							check_and_set_struct(!(qual & SV_QUALITY_VALIDITY_MASK),&log->qual_good[i]);
							check_and_set_struct(qual & SV_QUALITY_TEST,&log->test[i]);

							if(i<4)
							{
								pr_fl=pr*log->i_mul;
								*log->IL[i]=pr_fl;

								pr_fl*=log->i_mul2;

								if(pr_fl>32767)
									pr_fl=32767;
								else if(pr_fl<-32767)
									pr_fl=-32767;

								bus_an_samples_buf[SV_I_CARD_ADDR][i][smp_cnt%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=(u16)(pr_fl+32767);
							}
							else
							{
								pr_fl=pr*log->u_mul;
								*log->UL[i-4]=pr_fl;

								pr_fl*=log->u_mul2;

								if(pr_fl>32767)
									pr_fl=32767;
								else if(pr_fl<-32767)
									pr_fl=-32767;

								bus_an_samples_buf[SV_U_CARD_ADDR][i-4][smp_cnt%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=(u16)(pr_fl+32767);
							}
							ptr+=8;
						}

						log->sv_processed++;
						break;
					}

					pdu=pdu_prev+pdu_len;
					bytes_left=bytes_left_prev-pdu_len;
					break; // only one PDU checked
				}
				else
				{
					pdu+=len;
					bytes_left-=len;
				}
			}
		}

		svqueue.status[svqueue.cur]=0;
		svqueue.cur=((svqueue.cur+1)%SV_QUEUE_DEPTH);
	}

	if(err || matched_crit!=5) // no valid sv in this cycle
	{
		if(log->sv_timeout_cnt>=SV_WAIT_LOOP_CYCLES)
		{
			clear_struct(&log->kob);
			set_struct(&log->no_sync);
			for(i=0;i<8;i++)
			{
				clear_struct(&log->qual_good[i]);
				clear_struct(&log->test[i]);
			}

			if(bus_an_cur_sample_num<(SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES))
				snum=SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2-((SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES)-bus_an_cur_sample_num);
			else
				snum=bus_an_cur_sample_num-(SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES);

			while(snum!=bus_an_cur_sample_num)
			{
				for(i=0;i<4;i++)
				{
					bus_an_samples_buf[SV_I_CARD_ADDR][i][snum%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=0+32767;
					bus_an_samples_buf[SV_U_CARD_ADDR][i][snum%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=0+32767;
				}

				snum++;
				snum%=(SAMPLES_PER_MS*1000);
			}

			for(i=0;i<4;i++)
			{
				bus_an_samples_buf[SV_I_CARD_ADDR][i][bus_an_cur_sample_num]=0+32767;
				bus_an_samples_buf[SV_U_CARD_ADDR][i][bus_an_cur_sample_num]=0+32767;
			}
			log->smp_cnt_prev = bus_an_cur_sample_num;
		}
		else
			log->sv_timeout_cnt++;
	}
	else
	{
		log->sv_timeout_cnt=0;
		set_struct(&log->kob);
	}

//
// TODO: sprawdzac dodatkowo czy jest probka z danym tstamp przed obiegiem? A co gdy brak synchro TZX lub MU?
//

}
