/*
 * goose.c
 *
 *  Created on: 16-07-2020
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "eth.h"
#include "goose.h"
#include "goose_drv.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>

//TODO: stnum i seqnum dokladniej analizowac?
// 		dorobic lub sprawdzac wyjscia QUALITY?

int goose_drv_initlog(void *arguments, void *logic)
{
	struct goose_drv_args *args = (struct goose_drv_args *)arguments;
	struct goose_drv_logic *log = (struct goose_drv_logic *)logic;
	u32 i;

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

	gqueue.lost=0;

	for(i=0;i<MAX_LOG_ELEMENTS;i++) /* find goose channels */
	{
		if(log_manager.log_element[i].flags & FUNCTION_IS_GOOSE)
		{
			if((u8 *)&log->element_num[log->gooses_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16)))
				return -1;

			log->element_num[log->gooses_count]=i;
			log->gooses_count++;
		}
	}

	return 0;
}


void goose_drv(void *arguments, void *logic)
{
	struct goose_drv_logic *log = (struct goose_drv_logic *)logic;
//	struct goose_drv_args *args = (struct goose_drv_args *)arguments;
	struct goose_args *g_args;
	struct goose_logic *g_log;

	struct goose_frame *gfr;
	u8 *pdu;
//	u8 *pdu_prev;
	u8 matched_crit;
	int len,pdu_len;
	int bytes_left;//, bytes_left_prev;
	int err;
	u8 tag;
	int alldata_len;
	int bytes_left_alldata;
	u8 *pdu_alldata;
	u8 data_cnt;
	u8 i;
	u8 j;
	u32 /*sqnum,stnum,ttl*/cfg_rev,ndscom,test;
	uint64_t sqnum,stnum,ttl;
	u8 ndscom_valid,test_valid;

	*log->lost=gqueue.lost;

	for(j=0;j<log->gooses_count;j++)
	{
		g_args = (struct goose_args *)log_manager.log_element[log->element_num[j]].fun_args_ptr;
		g_log = (struct goose_logic *)log_manager.log_element[log->element_num[j]].fun_log_ptr;
		if(g_log->ttl>=LOOP_CYCLE_MS)
			g_log->ttl-=LOOP_CYCLE_MS;
		else
			g_log->ttl=0;

		check_and_set_struct(!g_log->ttl,&g_log->expired);
	}

	while(gqueue.status[gqueue.cur] & (GOOSE_TO_PROCESS))
	{
		gfr = (struct goose_frame *)gqueue.buf[gqueue.cur];
		*log->recv=(*log->recv)+1;

		for(j=0;j<log->gooses_count;j++)
		{
			g_args = (struct goose_args *)log_manager.log_element[log->element_num[j]].fun_args_ptr;
			g_log = (struct goose_logic *)log_manager.log_element[log->element_num[j]].fun_log_ptr;

			err = 0;
			ndscom_valid=0;
			test_valid=0;

			if(htons(gfr->appid)==g_args->params.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==GOOSE_TAG_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 GOOSE_TAG_GOCBREF:
									// process data (compare LEN bytes with STR)
									if(len!=strlen(g_log->goose_cbref) || memcmp(g_log->goose_cbref,pdu,len))
										err=1;
									else
										matched_crit++;

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

								case GOOSE_TAG_DATSET:
									// process data (compare LEN bytes with STR)
									if(len!=strlen(g_log->goose_dataset) || memcmp(g_log->goose_dataset,pdu,len))
										err=1;
									else
										matched_crit++;

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

								case GOOSE_TAG_CONFREV: //dodac bytes left-=len ?!
									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(g_args->params.cfg_rev==cfg_rev)
											matched_crit++;
										else
											err=1;
									}
									break;

								case GOOSE_TAG_TTL: //dodac bytes left-=len ?!
									if(len>5)
										err=1;
									else
									{
										ttl=0;

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

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

										matched_crit++;
									}

									break;

								case GOOSE_TAG_STNUM: //dodac bytes left-=len ?!
									if(len>5)
										err=1;
									else
									{
										stnum=0;

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

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

										matched_crit++;
									}
									break;

								case GOOSE_TAG_SQNUM: //dodac bytes left-=len ?!
									if(len>5)
										err=1;
									else
									{
										sqnum=0;

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

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

										matched_crit++;
									}
									break;

								case GOOSE_TAG_SIMULATION:
									if(len>4)
										err=1;
									else
									{
										test=0;

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

											if((i+1)<len)
												test<<=8;
										}
										test_valid=1;
									}
									break;

								case GOOSE_TAG_NDSCOM:
									if(len>4)
										err=1;
									else
									{
										ndscom=0;

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

											if((i+1)<len)
												ndscom<<=8;
										}
										ndscom_valid=1;
									}
									break;

								case GOOSE_TAG_ID:
									// process data (compare LEN bytes with STR)
									if(len!=strlen(g_log->goose_id) || memcmp(g_log->goose_id,pdu,len))
										err=1;
									else
										matched_crit++;

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

								case GOOSE_TAG_ALLDATA:
									data_cnt = 0;
									alldata_len = len;
									bytes_left_alldata = bytes_left;
									pdu_alldata = pdu;
									bytes_left=alldata_len;

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

										switch(tag)
										{
											case GOOSE_DATA_STRUCT:
												break;
											default:
												for(i=0;i<8;i++)
													if(data_cnt==g_args->params.data_index[i] && tag==GOOSE_DATA_BOOLEAN)
													{
														g_log->values&=~(1<<i);
														g_log->values|=*pdu?(1<<i):0;
														matched_crit++;
													}

												pdu+=len;
												bytes_left-=len;
												data_cnt++;
												break;
										}
									}

									pdu=pdu_alldata+alldata_len;
									bytes_left=bytes_left_alldata-alldata_len;
									break;

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

							if(!err && matched_crit==15) // matched goose cfgrev,cbref,datset,ttl,sqnum,stnum,goid+8 states
							{

								//check sqnum/stnum here
								if((sqnum>=(g_log->sqnum+1) && stnum==g_log->stnum)
									|| stnum>=g_log->stnum+1
									|| !g_log->ttl)
								{
									*g_log->recv_cnt=(*g_log->recv_cnt)+1;
									g_log->stnum=stnum;
									g_log->sqnum=sqnum;
									g_log->ttl=ttl;
									clear_struct(&g_log->expired);

									for(i=0;i<8;i++)
										check_and_set_struct(g_log->values & (1<<i),&g_log->state[i]);

									check_and_set_struct(ndscom,&g_log->ndscom);
									check_and_set_struct(test,&g_log->test);
								}
								break;
							}

							if(err) //do nastepnego goosa
							{
								break;
							}

						}

						break;
					}
					else
					{
						pdu+=len;
						bytes_left-=len;
					}
				}
			}
		}
		gqueue.status[gqueue.cur]=0;
		gqueue.cur=((gqueue.cur+1)%GOOSE_QUEUE_DEPTH);
	}
}
