#!/usr/bin/python3

import argparse
import serial
import sys
import time

# UBX commands

# UBX-CFG_TP5, 0x06 0x31
tp5 = [
0x00, 0x01, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 
0x40, 0x42, 0x0f, 0x00, 0x40, 0x42, 0x0f, 0x00, 
0xA0, 0x86, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0xf3, 0x00, 0x00, 0x00]

# UBX-CFG-CFG, 0x06, 0x09
# store MSG, NMEA and TP5 config
cfg = [
0x00, 0x00, 0x00, 0x00,
0x1a, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x17
]

# UBX-CFG-CFG, 0x06, 0x09
# reset all values to factory default
cfg_clr = [
0xff, 0xff, 0xff, 0xff,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x17
]

# UBX-CFG-NMEA, 0x06 0x17 (v1)
# * set NMEA version to 2.3 (same as Neo6)
# * set main Talker ID to 1 (GPS), otherwise GSA/GSV/etc will not work correctly
nmea_cfg = [
0x00, 0x23, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00
]

# UBX-CFG_MSG, 0x06 0x01
# set rates for NMEA msgs to 0 to stop them from being sent, they are on by default 
gga_off = [
0xf0, 0x00, 0x00, 0x00, 0x00, 0x00
]
gll_off = [
0xf0, 0x01, 0x00, 0x00, 0x00, 0x00
]
rmc_off = [
0xf0, 0x04, 0x00, 0x00, 0x00, 0x00
]
gsa_off = [
0xf0, 0x02, 0x06, 0x00, 0x00, 0x00
]


def checksum(data):
	'''Calculate UBX checksum'''
	cka = ckb = 0

	for d in data:
		cka = (cka + d) & 0xff
		ckb = (ckb + cka) & 0xff

	#print("{:02x}/{:02x}".format(cka, ckb))
	return cka, ckb


def ubx_cmd(uclass, uid, data):
	''' Create UBX command based on class and ID.
	    Length is determined by data array'''

	len1 = len(data) & 0xff
	len2 = (len(data) & 0xff00) >> 8
	head = [0xb5, 0x62, uclass, uid, len1, len2]
	packet = head + data

	cka, ckb = checksum(packet[2:])
	print('UBX<  class: {:02x}, id: {:02x}, len: {}, cka: {:02x}, ckb: {:02x} '.format(
			uclass, uid, len(data), cka, ckb))
	return bytes(packet + [cka, ckb])


def print_data(data):
	''' Pretty-print hex data '''
	s = ""
	for d in data:
		s += "0x{:02x}, ".format(d)
	print(s)

def print_nmea(nmea):
	''' Pretty-print NMEA data '''
	for n in nmea.decode(errors='replace').split('\n'):
		print('NMEA> {}'.format(n))


def decode_ubx(data):
	''' Decode binary data.
	    UBX data will get decoded, everything else is assumed
	    to be NMEA strings and will just get printed. '''

	nmea = bytearray()
	while True:
		#print_data(data)
		if len(data) < 8:
			if nmea:
				print_nmea(nmea)
			else:
				print("garbage:")
				print_data(data)
			return

		if data[0] != 0xb5 and data[1] != 0x62:
			#print("no valid start seq, skipping 1")
			#print_data(data)
			nmea.append(data[0])
			data = data[1:]
			continue
		if nmea:
			print_nmea(nmea)
		nmea = bytearray()
		# start seq found
		uclass = data[2]
		uid = data[3]
		ulen = data[5] << 8 | data[4]
		if len(data) < ulen + 8:
			print("packet too small:")
			print_data(data)
			return

		ucka = data[6+ulen] 
		uckb = data[6+ulen + 1]
		print("UBX>  class: {:02x}, id: {:02x}, len: {}, cka: {:02x}, ckb: {:02x} ".format(
			uclass, uid, ulen, ucka, uckb))

		if uclass == 0x01 and uid == 0x26:
			s = ""
			for d in data[6:6+ulen]:
				s += "0x{:02x}, ".format(d)
			print(s)
			sys.exit(0)

		data = data[8 + ulen:]
		if len(data) == 0:
			return

def disable_nmea():
	p = ubx_cmd(0x06, 0x01, gga_off)
	port.write(p)
	p = ubx_cmd(0x06, 0x01, gll_off)
	port.write(p)
	p = ubx_cmd(0x06, 0x01, rmc_off)
	port.write(p)
	p = ubx_cmd(0x06, 0x01, gsa_off)
	port.write(p)

def enable_timepulse():
	p = ubx_cmd(0x06, 0x31, tp5)
	port.write(p)

def set_nmea_config():
	p = ubx_cmd(0x06, 0x17, nmea_cfg)
	port.write(p)

def save_cfg():
	p = ubx_cmd(0x06, 0x09, cfg)
	port.write(p)

def clear_cfg():
	p = ubx_cmd(0x06, 0x09, cfg_clr)
	port.write(p)

def test():
	p = ubx_cmd(0x01, 0x26, [])
	port.write(p)



if __name__ == "__main__":

	parser = argparse.ArgumentParser(description='Monitor or reprogram U-blox M8N module on NAL9602LP.')
	parser.add_argument('device', type=str,
		help='serial device to communicate')
	parser.add_argument('-f', '--fix', action="store_true",
		help='send commands to fix cfg')
	parser.add_argument('-r', '--reset', action="store_true",
		help='reset cfg to factoy default')


	args = parser.parse_args()

	try:
		port = serial.Serial(args.device, 9600, timeout=0.1) 
	except Exception as e:
		print(str(e))
		sys.exit(1)

	#test()

	if args.reset:
		clear_cfg()

	if args.fix:
		# send UBX command to change config
		disable_nmea()
		enable_timepulse()
		set_nmea_config()
		# store CFG in flash
		save_cfg()

	# Receive loop
	chunk = 64
	data = bytes()
	start = None
	loops = 0
	sent = False
	sent2 = False
	while True:
		rdata = port.read(chunk)
		if len(rdata) == 0:
			# ublox sends continuous data in 1 sec intervals
			# if we didn't receive anything for a while, assume
			# we got all the data for the last second and start decoding
			if start is not None and time.time() - start > 0.3:
				start = None
				print('\n' + '*'*10 + time.ctime(time.time()) + '*' *10)
				decode_ubx(data)
				data = bytes()
			time.sleep(0.1)
			continue

		start = time.time()
		data += rdata
		#print("read " + str(len(rdata)))
		#print_data(rdata)