#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <windows.h>
 
#include <C:\Program Files (x86)\libsbp\c\include\libsbp\sbp.h>
#include <C:\Program Files (x86)\libsbp\c\include\libsbp\navigation.h>
//#include <C:\Program Files (x86)\libsbp\c\include\libsbp\sbp.c>
//#include <C:\Program Files (x86)\libsbp\c\include\libsbp\edc.h>
//#include <C:\Program Files (x86)\libsbp\c\include\libsbp\edc.c>


#define _CRT_SECURE_NO_WARNINGS
#define _D_FILE_OFFSET_BITS = 64
#define List_Size 141843576
#define FILEINPUT  "I:\\Data\\GNSS\\Dynamic\\10.0.26.87_2020-05-27_15-02-36.sbp"
#define FILEOUTPUT "I:\\Data\\GNSS\\Dynamic\\10.0.26.87_2020-05-27_15-02-36.csv"
//#define Buff_Size 10000
//F:\\Jirka\\skola\\GPS-RTK\\Parse3\\Project1\\Project1\\
//I:\\Data\\GNSS\\

FILE *input, *output;
long i, count, input_count;
long head;
char list[List_Size] = { 0 };
//char buffer[Buff_Size];
int time;
char list_out[140000000];
long print_len = 0;
int output_pause;;


/* State of the SBP message parser.
* Must be statically allocated.
*/
sbp_state_t sbp_state;

/* SBP structs that messages from Piksi will feed. */
msg_pos_llh_t					pos_llh;
msg_baseline_ned_t				baseline_ned;
msg_vel_ned_t					vel_ned;
msg_dops_t						dops;
msg_gps_time_t					gps_time;
msg_baseline_heading_dep_a_t	baseline_heading_dep_a;

/*
* SBP callback nodes must be statically allocated. Each message ID / callback
* pair must have a unique sbp_msg_callbacks_node_t associated with it.
*/
sbp_msg_callbacks_node_t pos_llh_node;
sbp_msg_callbacks_node_t baseline_ned_node;
sbp_msg_callbacks_node_t vel_ned_node;
sbp_msg_callbacks_node_t dops_node;
sbp_msg_callbacks_node_t gps_time_node;
sbp_msg_callbacks_node_t baseline_heading_dep_a_node;

/*
* Callback functions to interpret SBP messages.
* Every message ID has a callback associated with it to
* receive and interpret the message payload.
*/
void sbp_pos_llh_callback(u16 sender_id, u8 len, u8 msg[], void *context)
{
	pos_llh = *(msg_pos_llh_t *)msg;
}
void sbp_baseline_ned_callback(u16 sender_id, u8 len, u8 msg[], void *context)
{
	baseline_ned = *(msg_baseline_ned_t *)msg;
}
void sbp_vel_ned_callback(u16 sender_id, u8 len, u8 msg[], void *context)
{
	vel_ned = *(msg_vel_ned_t *)msg;
}
void sbp_dops_callback(u16 sender_id, u8 len, u8 msg[], void *context)
{
	dops = *(msg_dops_t *)msg;
}
void sbp_gps_time_callback(u16 sender_id, u8 len, u8 msg[], void *context)
{
	gps_time = *(msg_gps_time_t *)msg;
}
void sbp_baseline_heading_dep_a_callback(u16 sender_id, u8 len, u8 msg[], void *context)
{
	baseline_heading_dep_a = *(msg_baseline_heading_dep_a_t *)msg;
}

/*
* Set up SwiftNav Binary Protocol (SBP) nodes; the sbp_process function will
* search through these to find the callback for a particular message ID.
*
* Example: sbp_pos_llh_callback is registered with sbp_state, and is associated
* with both a unique sbp_msg_callbacks_node_t and the message ID SBP_POS_LLH.
* When a valid SBP message with the ID SBP_POS_LLH comes through the UART, written
* to the FIFO, and then parsed by sbp_process, sbp_pos_llh_callback is called
* with the data carried by that message.
*/
void sbp_setup(void)
{
	/* SBP parser state must be initialized before sbp_process is called. */
	sbp_state_init(&sbp_state);

	/* Register a node and callback, and associate them with a specific message ID. */
	sbp_register_callback(&sbp_state, SBP_MSG_GPS_TIME, &sbp_gps_time_callback,
		NULL, &gps_time_node);
	sbp_register_callback(&sbp_state, SBP_MSG_POS_LLH, &sbp_pos_llh_callback,
		NULL, &pos_llh_node);
	sbp_register_callback(&sbp_state, SBP_MSG_BASELINE_NED, &sbp_baseline_ned_callback,
		NULL, &baseline_ned_node);
	sbp_register_callback(&sbp_state, SBP_MSG_VEL_NED, &sbp_vel_ned_callback,
		NULL, &vel_ned_node);
	sbp_register_callback(&sbp_state, SBP_MSG_DOPS, &sbp_dops_callback,
		NULL, &dops_node);
	sbp_register_callback(&sbp_state, SBP_MSG_BASELINE_HEADING_DEP_A, &sbp_baseline_heading_dep_a_callback,
		NULL, &baseline_heading_dep_a_node);
}

u8 fifo_read_char(char *c) {

	*c = list[head];
	head = head + 1;
	return 1;
}

u32 fifo_read(u8 *buff, u32 n, void *context) {
	unsigned int i;
	for (i = 0; i<n; i++)
		if (!fifo_read_char((char *)(buff + i)))
			break;
	return i;
}



int main(void){

	count = 0;
	time = 0;
	input_count = 0;
	output_pause = 1;
	long filesize = 0;
	long fileposition = 0;
	bool fileinit = 0;
	sbp_setup();
	

	if (fopen_s(&input, FILEINPUT, "r+t") == 0){
		fseek(input, 0, SEEK_END);
		filesize = ftell(input);
		fseek(input, 0, SEEK_SET);
		fileposition = fseek(input, 0, SEEK_CUR);
		printf("Velikost souboru v bitech = %d\n", filesize);
		printf("Zaciname cist na pozici %d\n", fileposition);
		fclose(input);
	}
	else
		printf("File could not be opened\n");


	while (fileposition < filesize){
		
		if (fopen_s(&input, FILEINPUT, "r+t") == 0)
		{
			fseek(input, fileposition, SEEK_SET);
			
			//printf("Opening file 0004-00020.sbp\n");
			while (!feof(input)){
				fread(list, sizeof(char), List_Size, input);
				//input_count += fread(list, sizeof(char), List_Size, input);
			}
			
			fileposition = ftell(input);
			printf("Posunuli jsme se na %d\n", fileposition);
			fclose(input);
		}
		else
			printf("File could not be opened\n");

		for (int tmp = 0; tmp < List_Size; tmp++){
			if (list[tmp] != 0)
				input_count++;
		}
		
		printf("Nenulovych znaku : %d\n", input_count);
		
		if ((fopen_s(&output, FILEOUTPUT, "w+t") == 0) && (fileinit == 0))
		{

			print_len = sprintf_s(list_out, 10000, "GPS;;Absolute Position;;;Baseline;;;Velocity;;;;;;;Dilution of Precision\n");
			print_len += sprintf_s(list_out + print_len, 10000, "GPS Week;GPS Time;Latitute;Longitude;Height;Pos_rms_h;Pos_rms_v;North;East;Down;Pos_rms_h;Pos_rms_v;North;East;Down;GDoP;HDoP;PDoP;TDoP;VDoP;Flags;Heading\n");

			fwrite(list_out, sizeof(char), print_len, output);
			fclose(output);
			fileinit++;
			printf("Soubor zalozen\n");
		}


		while (count < input_count)
		{

			s8 ret = sbp_process(&sbp_state, &fifo_read);
			
			//if (ret < 0) break;
			
			if (time < (int)baseline_ned.tow){

				time = (int)baseline_ned.tow;

				print_len += sprintf_s(list_out + print_len, 801000, "%d;%d;%.15f;%.15f;%.14f;%d;%d;", (int)gps_time.wn, (int)gps_time.tow, pos_llh.lat, pos_llh.lon, pos_llh.height, pos_llh.h_accuracy, pos_llh.v_accuracy);
				print_len += sprintf_s(list_out + print_len, 801000, "%d;%d;%d;%d;%d;%d;", (int)baseline_ned.n, (int)baseline_ned.e, (int)baseline_ned.d, baseline_ned.h_accuracy, baseline_ned.v_accuracy, (int)vel_ned.n, (int)vel_ned.e, (int)vel_ned.d);
				print_len += sprintf_s(list_out + print_len, 801000, "%.2f;%.2f;%.2f;%.2f;%.2f;", (float)dops.gdop / 100, (float)dops.hdop / 100, (float)dops.pdop / 100, (float)dops.tdop / 100, (float)dops.vdop / 100);
				print_len += sprintf_s(list_out + print_len, 801000, "%d; %f\n", pos_llh.flags, (float)baseline_heading_dep_a.heading/1000);
			}

			if (print_len > (75000 * output_pause)){
				if (fopen_s(&output, FILEOUTPUT, "w+t") == 0)
				{
					fwrite(list_out, sizeof(char), print_len, output);
					++output_pause;
					printf("#");
					fclose(output);

				}
				else{
					printf("File could not be opened\n");
					return (-1);
				}
			}

			
			if (head > List_Size)
			{
				/*if (fopen_s(&output, FILEOUTPUT, "w+t") == 0){
					print_len += sprintf_s(list_out + print_len, 1000, "Soubor jeste vede dal");
					fwrite(list_out, sizeof(char), print_len, output);
					fclose(output);
					
				}*/
				if (fopen_s(&output, FILEOUTPUT, "w+t") == 0)
				{
					fwrite(list_out, sizeof(char), print_len, output);
					++output_pause;
					printf(":#");
					//fclose(output);

				}
				
				break;
			}
			else
			{
				//fclose(output);
			}
			count++;
			fclose(output);
		}
		fclose(output);
		count = 0;
		head = 0;
		input_count = 0;
	
		//output_pause = 1;
		//print_len = 0;
		printf("\n");
	}
	//fclose(output);
}