import socket
import datetime, time
import pprint

import csv
import sys
sys.path.insert(0,'..')

from functions import *

pp = pprint.PrettyPrinter(indent=4)
port = 1111
count = 0

cs = None

dumpFile = open("logs/{}_{}.txt".format("binary", datetime.datetime.now().strftime('%m_%d-%H_%M_%S')), "bw")
logFile = open("logs/{}_{}.txt".format("flight", datetime.datetime.now().strftime('%m_%d-%H_%M_%S')), "w", newline='')

dataset = """
	struct vector_Uint accel_raw;
	struct vector_Uint gyro_raw; 
	struct vector_Uint mag_raw;
	uint32_t time;
	uint32_t count;
"""

master = PktParser(dataset)
slave = PktParser(dataset, master.end)

udpServer = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udpServer.bind(("", port))

print("UDP Server is listening on port " + str(port))

while True:
	try:
		data, addr = udpServer.recvfrom(2048)
		
	except WindowsError as e:
		print(e)
		time.sleep(1)
		continue
	if data == None:
			continue

	dumpFile.write(data + bytes("\r\n", 'ascii'))
	
	if len(data) != slave.end:
		print("incorrect data length {} != {}".format(len(data), slave.end))
		continue
	
	data_master = master.parse(data)
	data_slave  = slave.parse(data)
	
	data = data_master

	if not data_master:
		continue
		
	for prop in data_slave.keys():
		if data_slave:
			data["slave_" + prop] = data_slave[prop]
	
	data["datetime"] = str(datetime.datetime.now())
	data["count"] = count
	
	if cs == None:
		cs = csv.DictWriter(logFile, data.keys())
		cs.writeheader()
	
	cs.writerow(data)
	
	count+=1
	
	#pp.pprint(data)
	
	print("{:=14} Accel Z: Master: {:05} Slave: {:05}".format(count, data["accel_raw_z"], data["slave_accel_raw_z"]), end='\r', flush=True)
	