#!/usr/bin/env python

# Copyright (C) 2007
#  Petr Holub. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in the
#    documentation and/or other materials provided with the distribution.
# 3. All advertising materials mentioning features or use of this software
#    must display the following acknowledgement:
#
#  This product includes software developed by Petr Holub.
#
# 4. Neither the name of the author nor the names of its contributors
#    may be used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
# OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
# OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
# SUCH DAMAGE.

import socket
import sys, traceback
import appuifw

def to_hex(s):
	try:
		lst = []
		for ch in s:
				hv = hex(ord(ch)).replace('0x', '')
				if len(hv) == 1:
					hv = '0'+hv
				lst.append('\\x' + hv)
		return reduce(lambda x,y:x+y, lst)
	except:
		return "[unable to convert to hex]"

def to_bin(s):
	try:
		binstr = ''
		if s < 0: return '[negative number!]'
		if s == 0: return '0'
		while s > 0:
			binstr = str(s % 2) + binstr
			s = s >> 1
		return binstr
	except:
		return "[unable to convert to bin]"

def high_endian(value):
    h, l = divmod(value, 256)
    return chr(h) + chr(l)

def checksum(payload):
    csum = 0
    for c in payload:
        csum += ord(c)
    return high_endian(csum & 0x7FFF)

def NMEA_checksum(data):
	csum = 0
	for c in data:
		csum = csum ^ ord(c)
	hex_csum = "%02x" % csum
	return hex_csum.upper()

def sirf_message(payload):
    return ('\xA0\xA2' + high_endian(len(payload)) + payload +
            checksum(payload) + '\xB0\xB3')

def switch_to_sirf(baudrate):
	t = "$PSRF100,0," + str(baudrate) + ",8,1,0*"
	# append checksum and <CR><LF>
	return t + NMEA_checksum(t[1:-1]) + '\x0D\x0A'
    
def switch_to_nmea():
    mode = '\x02'
        # 0 = Enable NMEA debug messages
        # 1 = Disable NMEA debug messages
        # 2 = Do not change NMEA debug setting
    gga_rate = '\x01' # numer of GGA messages per second; 0 = off
    gga_checksum = '\x01' # 0 = off, 1 = on
    gll_rate = '\x01' # numer of GLL messages per second; 0 = off
    gll_checksum = '\x01' # 0 = off, 1 = on
    gsa_rate = '\x01' # numer of GSA messages per second; 0 = off
    gsa_checksum = '\x01' # 0 = off, 1 = on
    gsv_rate = '\x01' # numer of GSV messages per second; 0 = off
    gsv_checksum = '\x01' # 0 = off, 1 = on
    rmc_rate = '\x01' # numer of RMC messages per second; 0 = off
    rmc_checksum = '\x01' # 0 = off, 1 = on
    vtg_rate = '\x01' # numer of VTG messages per second; 0 = off
    vtg_checksum = '\x01' # 0 = off, 1 = on
    mss_rate = '\x01' # numer of MSS messages per second; 0 = off
    mss_checksum = '\x01' # 0 = off, 1 = on
    zda_rate = '\x01' # numer of ZDA messages per second; 0 = off
    zda_checksum = '\x01' # 0 = off, 1 = on
    baud_rate = high_endian(38400)
    return sirf_message('\x81' + mode + gga_rate + gga_checksum +
                        gll_rate + gll_checksum +
                        gsa_rate + gsa_checksum +
                        gsv_rate + gsv_checksum +
                        rmc_rate + rmc_checksum +
                        vtg_rate + vtg_checksum +
                        mss_rate + mss_checksum +
                        '\x00\x00' +
                        zda_rate + zda_checksum +
                        '\x00\x00' +
                        baud_rate)                     

def stubborn_readone(socket):
	s = ""
	while len(s) < 1:
		s = socket.recv(1)
	return s

class Msg:
	"Typed message class"

	NMEA_message = False
	SiRF_message = False
	msg = ""

	def __init__(self, type, message):
		if str(type) == 'NMEA':
			self.NMEA_message = True
		elif str(type) == 'SiRF':
			self.SiRF_message = True
		elif str(type) == 'unknown':
			pass
		else:
			print 'Msg type error: unknown type ' + str(type)
			raise
		self.msg = message

	def isNMEA(self):
		if self.NMEA_message == True:
			return True
		else:
			return False
	
	def isSiRF(self):
		if self.SiRF_message == True:
			return True
		else:
			return False
	
	def isUnknown(self):
		if self.SiRF_message == False and self.NMEA_message == False:
			return True
		else:
			return False
	
	def isEmpty(self):
		if len(self.msg) == 0:
			return True
		else:
			return False

	def getMsg(self):
		return self.msg

	def toString(self):
		if self.isSiRF():
			return to_hex(self.msg)
		else:
			return repr(self.msg)

	def toStringShort(self):
		maxlen = 16
		if len(self.msg) > maxlen:
			if self.isSiRF():
				return to_hex(self.msg[0:(maxlen-4)]) + ' ... ' + to_hex(self.msg[-4:])
			else:
				return repr(self.msg[0:(maxlen-4)]) + ' ... ' + repr(self.msg[-4:])
		else:
			return self.toString()

def get_msg(socket):
	NMEAmode = False
	SiRFmode = False
	s = ""
	# this is to read the fist byte and discard any new-lines from
	# the previous message
	while s == "" or s == '\r' or s == '\n':
		s = stubborn_readone(socket)
	# now we start parsing the actual message
	if s == '$':
		NMEAmode = True
		terminate = False
		while terminate == False:
			tmps = stubborn_readone(socket)
			# * marks end of the data and it is followed by 
			# a checksum
			if tmps == '*':
				terminate = True
			s = s + tmps
		i = 0
		# we read the two byte checksum
		while i < 2:
			s = s + stubborn_readone(socket)
			i = i + 1
		# and return the whole message
		return Msg('NMEA', s)

	elif s == '\xA0':
		tmps = stubborn_readone(socket)
		if tmps == '\xA2':
			# SiRF-mode binary message is recognized by
			# \xA0 and \xA2 at the beginning
			SiRFmode = True
		else:
			print 'Unknown message: ' + repr(s + tmps)
		s = s + tmps
		while s[-2:] != '\xB0\xB3':
			# this might be dangerous - payload length should
			# be parsed
			s = s + stubborn_readone(socket)
		return Msg('SiRF', s)
	else:
		print 'Unknown message: ' + repr(s)
		return Msg('unknown', s)


def send_msg(socket, data):
	#print '   ... sending:'
	#print '       ' + repr(data)
	socket.send(data)


def get_SiRF_reply(socket, msgID):
	print 'Waiting for GPS to reply'
	received = False
	i = 0
	max_iter = 200
	while not received and i < max_iter:
		try:
			s=get_msg(socket)
			if isinstance(s, Msg):
				if not s.isEmpty():
					if s.isSiRF():
						#print '   ... received:' + s.toStringShort()
						if s.getMsg()[4] == msgID:
							return s
						else:
							# print '   ... received msgID:' + to_hex(s.getMsg()[4])
							pass
					elif s.isNMEA():
						print '   ... received NMEA report :-('
					else:
						print '   ... received unknown message :-((('
				else:
					print '   ... received empty message!'
			else:
				print '   ... received wrong message type!'
		except:
			print '   ... failed to read, exception occurred!'
			#print '       ' + str(sys.exc_info()[:2])
		i = i + 1
	# we were not able to extract the requested message in max number
	# of iterations
	return None	


def perform_switch_to_SiRF(socket):
	baudrates = [ 115200, 57600, 38400, 19200, 9600, 4800]
	switched = False
	i = 0
	while switched == False and i < len(baudrates):
		msg = switch_to_sirf(baudrates[i])
		print '   ... trying at ' + str(baudrates[i]) + 'bps'
		send_msg(socket, msg)

		# we will see if we made it to SiRF mode based on reply from the GPS
		#print '   ... waiting for reply'
		try:
			s=get_msg(socket)
			if not s.isEmpty():
				#print '   ... recived:'
				if s.isSiRF():
					#print '       ' + s.toString()
					print '   ... switched to SiRF mode'
					switched = True
				else:
					#print '       ' + s.toStringShort()
					pass
			else:
				print '   ... received empty message!'
		except:
			print '   ... failed to read, exception occurred!'
			print '       ' + sys.exc_info()[0]

		# let's try lower speed
		i = i+1
	return switched


############################################################################
#  Here we go
############################################################################

appuifw.note(u"Static Navigation Tweaker 1.1\nBSD licenced with all implications!\n(c)2007 Petr Holub", 'info')

index = appuifw.selection_list([u"Disable SN", u"Enable SN", u"Enable DGPS", u"Disable DGPS", u"Get status"])
if index == 0:
	mode = 'disable-sn'
elif index == 1:
	mode = 'enable-sn'
elif index == 2:
	mode = 'enable-dgps'
elif index == 3:
	mode = 'disable-dgps'
else:
	mode = 'poll'


gps_addr,services=socket.bt_discover()
target=(gps_addr,services.values()[0])

sock=socket.socket(socket.AF_BT, socket.SOCK_STREAM)
sock.connect(target)

print 'Connected to the GPS [' + str(gps_addr) + ']'

# first, we assume we're in the NMEA mode
print 'Switching to SiRF'
switched = perform_switch_to_SiRF(sock)

if not switched:
	print '   ... failed to switch to SiRF mode,'
	print '       maybe already in deaf SiRF mode'
	# maybe we're already in pseudo-SiRF mode (hearing NMEA messages
	# from GPS but not receiving NMEA input)
	print 'Switching to NMEA first'
	msg = switch_to_nmea()
	send_msg(sock, msg)
	print 'Switching to SiRF again'
	switched = perform_switch_to_SiRF(sock)

if not switched:
	print '   ... failed to switch to SiRF mode again, bailing out'
else:
	if mode != 'poll':
		if mode == 'enable-sn':
			print 'Enabling static navigation'
			send_msg(sock, sirf_message('\x8F' + '\x01'))
		elif mode == 'disable-sn':
			print 'Disabling static navigation'
			send_msg(sock, sirf_message('\x8F' + '\x00'))
		elif mode == 'enable-dgps':
			print 'Enabling DGPS navigation'
			send_msg(sock, sirf_message(
					# msgID
					'\x85' + 
					# SBAS: WAAS/EDGAS
					'\x01' + 
					# internal beacon frequency - not used in SBAS
					'\x00\x00\x00\x00' + 
					# internal beacon bitrate - not used in SBAS
					'\x00'
					))
			# SBAS to auto/integrity mode
			# http://www.koders.com/c/fid972B4BBA85F8344813FA40A4995403418F35914D.aspx
			send_msg(sock, sirf_message(
					# msgID
					'\xAA\x00\x01\x00\x00\x00'
					))
		elif mode == 'disable-dgps':
			print 'Disabling DGPS navigation'
			send_msg(sock, sirf_message(
					# msgID
					'\x85' + 
					# WAAS/EDGAS
					'\x00' + 
					# internal beacon frequency
					'\x00\x00\x00\x00' + 
					# internal beacon bitrate
					'\x00'
					))
	
	try:
		if mode == 'poll':
			print 'Getting firmware version:'
			send_msg(sock, sirf_message('\x84\x00'))
			msg = get_SiRF_reply(sock, '\x06')
			if msg == None:
				print '   ... failed to get requested message'
			else:
					msg_data = msg.getMsg()
					if len(msg_data) < 6:
						print '   ... unsupported version string: ' + to_hex(msg_data)
					else:
						print '   ... version ' + str(msg_data[5:(len(msg_data)-5)])
	except:
		print '   ... exception occurred'
		print '       '.join(traceback.format_exception(
			*sys.exc_info())[-2:]).strip().replace('\n',': ')
	
	try:
		print 'Polling static navigation status'
		send_msg(sock, sirf_message('\xA8' + '\x8F'))
		print 'Polling navigation parameters'
		send_msg(sock, sirf_message('\x98' + '\x00'))
		# Get the reply at some point
		msg = get_SiRF_reply(sock, '\x13')
		if msg == None:
			print '   ... failed to get requested message'
		else:
			msg_data = msg.getMsg()
			print '   ... static navigation is ' + hex(ord(msg_data[17]))
			# print '   ... got ' + to_hex(msg_data)
			if ord(msg_data[17]) == 0:
				print "Static navigation is disabled."
			elif ord(msg_data[17]) == 1:
				print "Static navigation is enabled."
			else:
				print "Unknown static navigation status."
			print 'Extracted DGPS information'
			print '   ... DGPS source is ' + hex(ord(msg_data[31]))
			print '   ... DGPS mode is ' + hex(ord(msg_data[32]))
			print '   ... DGPS timeout is ' + hex(ord(msg_data[33]))
	except:
		print '   ... exception occurred'
		print '       '.join(traceback.format_exception(
			*sys.exc_info())[-2:]).strip().replace('\n',': ')

	
	# Match precision w/ and w/o DGPS
	try:
		if mode == 'enable-dgps' or mode == 'disable-dgps' or mode == 'poll':
			print 'Testing (D)GPS data precision:'
			msg = get_SiRF_reply(sock, '\x02')
			if msg == None:
				print '   ... failed to get requested message'
			else:
				msg_data = msg.getMsg()
				print '   ... data length ' + str(len(msg_data))
				print '   ... got mode ' + to_bin(ord(msg_data[23]))
				print '   ... got DOP ' + str((ord(msg_data[24])/5.0))
	except:
		print '   ... exception occurred'
		print '       '.join(traceback.format_exception(
			*sys.exc_info())[-2:]).strip().replace('\n',': ')

	
	# Get SBAS data
	try:
		if mode == 'enable-dgps' or mode == 'poll':
			print 'Getting SBAS information:'
			send_msg(sock, sirf_message('\xA8' + '\x8A'))
			send_msg(sock, sirf_message('\x98' + '\x00'))
			msg = get_SiRF_reply(sock, '\x32')
			if msg == None:
				print '   ... failed to get requested message'
			else:
				msg_data = msg.getMsg()
				if ord(msg_data[5]) == 0:
					print '   ... SBAS PRN is auto'
				elif ord(msg_data[5]) >= 120 and ord(msg_data[5]) <= 138:
					print '   ... SBAS PRN exclusive set by user: ' + str(ord(msg_data[5]))
				else:
					print '   ... SBAS PRN unknown: ' + ord(msg_data[5])
				if ord(msg_data[6]) == 0:
					print '   ... SBAS mode: testing'
				elif ord(msg_data[6]) == 0:
					print '   ... SBAS mode: integrity'
				else:
					print '   ... SBAS mode: unknown'
				print '   ... SBAS timeout ' + str(ord(msg_data[7])) + 's'
				print '   ... SBAS flags ' + to_bin(ord(msg_data[8]))
	except:
		print '   ... exception occurred'
		print '       '.join(traceback.format_exception(
			*sys.exc_info())[-2:]).strip().replace('\n',': ')
		


	print 'Switching back to NMEA'
	msg=switch_to_nmea()
	send_msg(sock, msg)

print 'Closing connection to [' + str(gps_addr) + ']'
sock.close()