#! /usr/bin/python

'''
===============================================================================================
			H O V E R C R A F T   I N S T R U M E N T A T I O N

				GNSS / GPS DATA DISTRIBUTION OVER NETWORK

This program reads data from GNSS/GPS receiver that appears as a COM-port. It auto-detects
COM-port number and speed. GNSS/GPS receivers can transmit a various number of NMEA-telegrams
that belong to the same time instance (normally, each second); also the first and last telegram
in such a block can vary between different receivers  ---  so the program auto-detects last
telegram in such a block.  After finding last NMEA-telegram in block, it extracts information
from different telegrams and forwards everything as a single JSON-formatted package.
Data is tranmitted as UDP broadcast telegram.  There is also a TCP server that transmits
this JSON-package to clients (client can be a telnet session, like "telnet 127.0.0.01 20001").
An example client program is provided in project documentation at:
   http://www.polarhovercraft.no    section "GPS/GNSS data distribution"

Program includes GPS simullator, which is invoked by giving parameter SIM after program name.
Simulator runs in two modes (that user selects):

    A = Great circle starting at 81degN/12degE to 82.5degN/-4degW (303.4km),
        travelling approx 1m/sec.
    B = Rose pattern centered on crossing of Greenwich and Equator, testing
        all four quadrants.

PYTHON VERSION:
	Using ver 2.7.17 dated Oct 19,2019  (presumably last 2.7 version of Python)
	
EXTERNAL LIBRARIES REQUIRED:
	1)	Get "pySerial" from:  http://pypi.org/project/pyserial/2.7/
	2)	LatLongUTMconversion.py  --- a library found ca 2008, that later disappeared.
		Included here (embedded in this source code) to avoid keeping it in separate file.
		"pyproj" library might be an alternative.

----------------------------------------------------------------------------------
Ver		Date		By		Description
----------------------------------------------------------------------------------
2.0  25 Mar 2020	O.M.	Main new features:
								(a) Auto-detecting GNSS/GPS COM-port and speed.
								(b) Auto-detecting last NMEA telegram in block that is
									received at each time instance (normally every second).
									This is used when extracting data from various NMEA
									telegrams, to ensure all data belongs to the same time
									instance. (Last telegram in these blocks varies between
									different GNSS/GPS receivers).
								(c) TCP server features:
									- Accepts 5 simultaneous socket connections.
									- Transmits data as JSON-formatted string in PUSH mode.
									- Gracefully handles socket pull-down on client side.

0.1     ---- 2011	O.M.		Initial version
----------------------------------------------------------------------------------

                               Dept. of Earth Science
                                University of Bergen
                                    N O R W A Y
===============================================================================================
'''


#--------------------------------------------------------------------------
#   C O N S T A N T S
#--------------------------------------------------------------------------

VERSION = '''
=======================================================================
HOVERCRAFT  ---   GNSS/GPS DATA DISTRIBUTION OVER NETWORK
Ver. 2.0
Date: 25 March, 2020
By: O.M.
Department of Earth Science, University of Bergen, Norway

Terminate program by pressing <ENTER> key
Start GPS simulation mode by giving parameter "SIM" after program name
=======================================================================
'''

GPS_UDP_PORT		= 20001


#--------------------------------------------------------------------------
#       I M P O R T   S E C T I O N
#--------------------------------------------------------------------------

import socket
from string import split
import select
from time import time, gmtime, strftime, strptime, sleep
from calendar import timegm
import sys
import thread
import os
import datetime
import serial
import json
import Queue
import threading


#===================================================================
#========        U T I L I T I E S
#===================================================================

"""
Colors text in console mode application (win32).
Uses ctypes and Win32 methods SetConsoleTextAttribute and
GetConsoleScreenBufferInfo.

$Id: color_console.py 534 2009-05-10 04:00:59Z andre $
"""

from ctypes import windll, Structure, c_short, c_ushort, byref

SHORT = c_short
WORD = c_ushort

class COORD(Structure):
  """struct in wincon.h."""
  _fields_ = [
    ("X", SHORT),
    ("Y", SHORT)]

class SMALL_RECT(Structure):
  """struct in wincon.h."""
  _fields_ = [
    ("Left", SHORT),
    ("Top", SHORT),
    ("Right", SHORT),
    ("Bottom", SHORT)]

class CONSOLE_SCREEN_BUFFER_INFO(Structure):
  """struct in wincon.h."""
  _fields_ = [
    ("dwSize", COORD),
    ("dwCursorPosition", COORD),
    ("wAttributes", WORD),
    ("srWindow", SMALL_RECT),
    ("dwMaximumWindowSize", COORD)]

# winbase.h
STD_INPUT_HANDLE = -10
STD_OUTPUT_HANDLE = -11
STD_ERROR_HANDLE = -12

# wincon.h
FOREGROUND_BLACK     = 0x0000
FOREGROUND_BLUE      = 0x0001
FOREGROUND_GREEN     = 0x0002
FOREGROUND_CYAN      = 0x0003
FOREGROUND_RED       = 0x0004
FOREGROUND_MAGENTA   = 0x0005
FOREGROUND_YELLOW    = 0x0006
FOREGROUND_GREY      = 0x0007
FOREGROUND_INTENSITY = 0x0008 # foreground color is intensified.

BACKGROUND_BLACK     = 0x0000
BACKGROUND_BLUE      = 0x0010
BACKGROUND_GREEN     = 0x0020
BACKGROUND_CYAN      = 0x0030
BACKGROUND_RED       = 0x0040
BACKGROUND_MAGENTA   = 0x0050
BACKGROUND_YELLOW    = 0x0060
BACKGROUND_GREY      = 0x0070
BACKGROUND_INTENSITY = 0x0080 # background color is intensified.

stdout_handle = windll.kernel32.GetStdHandle(STD_OUTPUT_HANDLE)
SetConsoleTextAttribute = windll.kernel32.SetConsoleTextAttribute
GetConsoleScreenBufferInfo = windll.kernel32.GetConsoleScreenBufferInfo

def get_text_attr():
  """Returns the character attributes (colors) of the console screen
  buffer."""
  csbi = CONSOLE_SCREEN_BUFFER_INFO()
  GetConsoleScreenBufferInfo(stdout_handle, byref(csbi))
  return csbi.wAttributes

def set_text_attr(color):
  """Sets the character attributes (colors) of the console screen
  buffer. Color is a combination of foreground and background color,
  foreground and background intensity."""
  SetConsoleTextAttribute(stdout_handle, color)

default_colors = get_text_attr()
default_bg = default_colors & 0x0070


def test():
  """Simple test for color_console."""
  default_colors = get_text_attr()
  default_bg = default_colors & 0x0070
  set_text_attr(FOREGROUND_BLUE | default_bg |
                     FOREGROUND_INTENSITY)
  print '==========================================='
  set_text_attr(FOREGROUND_BLUE | BACKGROUND_GREY |
                     FOREGROUND_INTENSITY | BACKGROUND_INTENSITY)
  print 'And Now for Something',
  set_text_attr(FOREGROUND_RED | BACKGROUND_GREY |
                     FOREGROUND_INTENSITY | BACKGROUND_INTENSITY)
  print 'Completely Different!',
  set_text_attr(default_colors)
  print
  set_text_attr(FOREGROUND_RED | default_bg |
                     FOREGROUND_INTENSITY)
  print '==========================================='
  set_text_attr(default_colors)




#---------------------------------------------------------------------
#  LatLongUTMconversion.py
#  Lat Long - UTM, UTM - Lat Long conversions
#  A library found on the internet in 2011, which disappeared later
#  Included to avoid the need for keeping it in an external file.
#----------------------------------------------------------------------

from math import pi, sin, cos, tan, sqrt

#LatLong- UTM conversion..h
#definitions for lat/long to UTM and UTM to lat/lng conversions
#include <string.h>

_deg2rad = pi / 180.0
_rad2deg = 180.0 / pi

_EquatorialRadius = 2
_eccentricitySquared = 3

_ellipsoid = [
#  id, Ellipsoid name, Equatorial Radius, square of eccentricity	
# first once is a placeholder only, To allow array indices to match id numbers
	[ -1, "Placeholder", 0, 0],
	[ 1, "Airy", 6377563, 0.00667054],
	[ 2, "Australian National", 6378160, 0.006694542],
	[ 3, "Bessel 1841", 6377397, 0.006674372],
	[ 4, "Bessel 1841 (Nambia] ", 6377484, 0.006674372],
	[ 5, "Clarke 1866", 6378206, 0.006768658],
	[ 6, "Clarke 1880", 6378249, 0.006803511],
	[ 7, "Everest", 6377276, 0.006637847],
	[ 8, "Fischer 1960 (Mercury] ", 6378166, 0.006693422],
	[ 9, "Fischer 1968", 6378150, 0.006693422],
	[ 10, "GRS 1967", 6378160, 0.006694605],
	[ 11, "GRS 1980", 6378137, 0.00669438],
	[ 12, "Helmert 1906", 6378200, 0.006693422],
	[ 13, "Hough", 6378270, 0.00672267],
	[ 14, "International", 6378388, 0.00672267],
	[ 15, "Krassovsky", 6378245, 0.006693422],
	[ 16, "Modified Airy", 6377340, 0.00667054],
	[ 17, "Modified Everest", 6377304, 0.006637847],
	[ 18, "Modified Fischer 1960", 6378155, 0.006693422],
	[ 19, "South American 1969", 6378160, 0.006694542],
	[ 20, "WGS 60", 6378165, 0.006693422],
	[ 21, "WGS 66", 6378145, 0.006694542],
	[ 22, "WGS-72", 6378135, 0.006694318],
	[ 23, "WGS-84", 6378137, 0.00669438]
]

#Reference ellipsoids derived from Peter H. Dana's website- 
#http://www.utexas.edu/depts/grg/gcraft/notes/datum/elist.html
#Department of Geography, University of Texas at Austin
#Internet: pdana@mail.utexas.edu
#3/22/95

#Source
#Defense Mapping Agency. 1987b. DMA Technical Report: Supplement to Department of Defense World Geodetic System
#1984 Technical Report. Part I and II. Washington, DC: Defense Mapping Agency

#def LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long, 
#			 double &UTMNorthing, double &UTMEasting, char* UTMZone)

def LLtoUTM(ReferenceEllipsoid, Lat, Long):
#converts lat/long to UTM coords.  Equations from USGS Bulletin 1532 
#East Longitudes are positive, West longitudes are negative. 
#North latitudes are positive, South latitudes are negative
#Lat and Long are in decimal degrees
#Written by Chuck Gantz- chuck.gantz@globalstar.com

    a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
    eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
    k0 = 0.9996

#Make sure the longitude is between -180.00 .. 179.9
    LongTemp = (Long+180)-int((Long+180)/360)*360-180 # -180.00 .. 179.9

    LatRad = Lat*_deg2rad
    LongRad = LongTemp*_deg2rad

    ZoneNumber = int((LongTemp + 180)/6) + 1
  
    if Lat >= 56.0 and Lat < 64.0 and LongTemp >= 3.0 and LongTemp < 12.0:
        ZoneNumber = 32

    # Special zones for Svalbard
    if Lat >= 72.0 and Lat < 84.0:
        if  LongTemp >= 0.0  and LongTemp <  9.0:ZoneNumber = 31
        elif LongTemp >= 9.0  and LongTemp < 21.0: ZoneNumber = 33
        elif LongTemp >= 21.0 and LongTemp < 33.0: ZoneNumber = 35
        elif LongTemp >= 33.0 and LongTemp < 42.0: ZoneNumber = 37

    LongOrigin = (ZoneNumber - 1)*6 - 180 + 3 #+3 puts origin in middle of zone
    LongOriginRad = LongOrigin * _deg2rad

    #compute the UTM Zone from the latitude and longitude
    UTMZone = "%d%c" % (ZoneNumber, _UTMLetterDesignator(Lat))

    eccPrimeSquared = (eccSquared)/(1-eccSquared)
    N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad))
    T = tan(LatRad)*tan(LatRad)
    C = eccPrimeSquared*cos(LatRad)*cos(LatRad)
    A = cos(LatRad)*(LongRad-LongOriginRad)

    M = a*((1
            - eccSquared/4
            - 3*eccSquared*eccSquared/64
            - 5*eccSquared*eccSquared*eccSquared/256)*LatRad 
           - (3*eccSquared/8
              + 3*eccSquared*eccSquared/32
              + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
           + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) 
           - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad))
    
    UTMEasting = (k0*N*(A+(1-T+C)*A*A*A/6
                        + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
                  + 500000.0)

    UTMNorthing = (k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
                                        + (61
                                           -58*T
                                           +T*T
                                           +600*C
                                           -330*eccPrimeSquared)*A*A*A*A*A*A/720)))

    if Lat < 0:
        UTMNorthing = UTMNorthing + 10000000.0; #10000000 meter offset for southern hemisphere
    return (UTMZone, UTMEasting, UTMNorthing)


def _UTMLetterDesignator(Lat):
#This routine determines the correct UTM letter designator for the given latitude
#returns 'Z' if latitude is outside the UTM limits of 84N to 80S
#Written by Chuck Gantz- chuck.gantz@globalstar.com

    if 84 >= Lat >= 72: return 'X'
    elif 72 > Lat >= 64: return 'W'
    elif 64 > Lat >= 56: return 'V'
    elif 56 > Lat >= 48: return 'U'
    elif 48 > Lat >= 40: return 'T'
    elif 40 > Lat >= 32: return 'S'
    elif 32 > Lat >= 24: return 'R'
    elif 24 > Lat >= 16: return 'Q'
    elif 16 > Lat >= 8: return 'P'
    elif  8 > Lat >= 0: return 'N'
    elif  0 > Lat >= -8: return 'M'
    elif -8> Lat >= -16: return 'L'
    elif -16 > Lat >= -24: return 'K'
    elif -24 > Lat >= -32: return 'J'
    elif -32 > Lat >= -40: return 'H'
    elif -40 > Lat >= -48: return 'G'
    elif -48 > Lat >= -56: return 'F'
    elif -56 > Lat >= -64: return 'E'
    elif -64 > Lat >= -72: return 'D'
    elif -72 > Lat >= -80: return 'C'
    else: return 'Z'	# if the Latitude is outside the UTM limits

#void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
#			  double& Lat,  double& Long )

def UTMtoLL(ReferenceEllipsoid, northing, easting, zone):

#converts UTM coords to lat/long.  Equations from USGS Bulletin 1532 
#East Longitudes are positive, West longitudes are negative. 
#North latitudes are positive, South latitudes are negative
#Lat and Long are in decimal degrees. 
#Written by Chuck Gantz- chuck.gantz@globalstar.com
#Converted to Python by Russ Nelson <nelson@crynwr.com>

    k0 = 0.9996
    a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
    eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
    e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared))
    #NorthernHemisphere; //1 for northern hemispher, 0 for southern

    x = easting - 500000.0 #remove 500,000 meter offset for longitude
    y = northing

    ZoneLetter = zone[-1]
    ZoneNumber = int(zone[:-1])
    if ZoneLetter >= 'N':
        NorthernHemisphere = 1  # point is in northern hemisphere
    else:
        NorthernHemisphere = 0  # point is in southern hemisphere
        y -= 10000000.0         # remove 10,000,000 meter offset used for southern hemisphere

    LongOrigin = (ZoneNumber - 1)*6 - 180 + 3  # +3 puts origin in middle of zone

    eccPrimeSquared = (eccSquared)/(1-eccSquared)

    M = y / k0
    mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256))

    phi1Rad = (mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu) 
               + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
               +(151*e1*e1*e1/96)*sin(6*mu))
    phi1 = phi1Rad*_rad2deg;

    N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad))
    T1 = tan(phi1Rad)*tan(phi1Rad)
    C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad)
    R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5)
    D = x/(N1*k0)

    Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
                                          +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720)
    Lat = Lat * _rad2deg

    Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
            *D*D*D*D*D/120)/cos(phi1Rad)
    Long = LongOrigin + Long * _rad2deg
    return (Lat, Long)

###if __name__ == '__main__':
###    (z, e, n) = LLtoUTM(23, 45.00, -75.00)
###    print z, e, n
###    (lat, lon) = UTMtoLL(23, n, e, z)
###    print lat, lon







#------------------------------------------------------
#  AUTO-DETECT SERIAL PORT WITH GNSS/GPS DATA STREAM
#------------------------------------------------------
  
def check_serial_ports():
	ports = ['COM%s' % (i + 1) for i in range (256)]
	result = []
	for port in ports:
		try:
			s = serial.Serial(port)
			s.close()
			result.append(port)
		except (OSError, serial.SerialException):
			pass

	if result:
		print "Searching for COM port with GNSS/GPS NMEA telegrams, also determining correct baudrate ..."
	else:
		set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY)
		print
		print "ERROR: No COM ports found on this computer"
		print
		set_text_attr(default_colors)
		answer = raw_input('Press <ENTER> to exit program')
		sys.exit()

	found = ''
	baudrates = [9600, 4800, 19200]
	for COM_port in result:
		data = ''
		print COM_port + ': ',		
		for baudrate in baudrates:
			print '%d bits/s  ' % (baudrate),
			s = serial.Serial(COM_port, baudrate, timeout=1.3)
			data = s.read(100)
			s.close()
			if (('$GP' in data) or ('$GN' in data)):
				set_text_attr(FOREGROUND_GREEN | default_bg | FOREGROUND_INTENSITY)
				print 'Found NMEA telegrams on %s, at %d bits/s:' % (COM_port, baudrate)
				set_text_attr(FOREGROUND_YELLOW | default_bg | FOREGROUND_INTENSITY)
				found = (COM_port, baudrate)
				print data
				set_text_attr(default_colors)
				break
		if not data:
			print
		if found:		
			break

	if not found:
		set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY)
		print
		print "ERROR: No COM port with NMEA telegrams found"
		print
		set_text_attr(default_colors)
		answer = raw_input('Press <ENTER> to exit program')
		sys.exit()

	(COM_port, baudrate) = found
	sleep(3)
	print
	print "Now analyzing timing of NMEA telegrams, to determine closing telegram in each data block."
	print "This lasts for 10 seconds, or 100 telegrams - whichever comes first."
	print
	sleep(3)
	
	s = serial.Serial(COM_port, baudrate, timeout=1)
	start_time = time()
	delta_time = 0.0
	previous_time = 0.0
	previous_telegram = ''
	scores = {}
	telegram = ''
	
	for k in range(100):		
		data = s.readline()
		timestamp = time() - start_time
		delta_time = timestamp - previous_time
		if data.startswith('$'):
			print "%1.3f s -- delta=%1.3f s:  " % (timestamp, delta_time),
			set_text_attr(FOREGROUND_YELLOW | default_bg | FOREGROUND_INTENSITY)
			print "%s" % (data),
			set_text_attr(default_colors)
			t = data.split(',')
			telegram = t[0]
			
			if previous_telegram in scores:
				scores[previous_telegram] += delta_time
			else:
				scores[previous_telegram] = delta_time
		previous_time = timestamp
		previous_telegram = telegram
		if (timestamp >= 10.0):
			break
	s.close()

	print
	print "Accumulated interval of silence after telegram transmitted ---"
	print "the one with highest score represents end-of-block telegram."
	max_value = 0.0
	block_tail = ''
	for telegram in scores:
		if telegram:
			print "   %s: %1.3f s"  % (telegram, scores[telegram])
			if scores[telegram] > max_value:
				max_value = scores[telegram]
				block_tail = telegram
	
	
	set_text_attr(FOREGROUND_GREEN | default_bg | FOREGROUND_INTENSITY)
	print "Last telegram in block:", block_tail
	set_text_attr(default_colors)
	sleep(1)
	return (COM_port, baudrate, block_tail)
	





# -------------------------------------------------
# Thread to catch keyboard input
# Win can't have select on stdin, only on sockets
#--------------------------------------------------

def keyboard_watch():
	global Running
	command = raw_input('\nPress <ENTER> to stop program\n\n')		# Will just wait for <ENTER> key press here.
	print "Shutting down / closing:"
	#sleep(0.3)
	Running = False





#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++        G P S   C L A S S
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

class c_GPS:

	def __init__(self, serial_port, serial_speed, UDP_port, mode, last_in_block, simulation_mode=''):
		self.type				= 'GNSS receiver'
		try:
			self.s_UDP	 			= socket.socket( socket.AF_INET, socket.SOCK_DGRAM )     # Get UDP socket
		except socket.error, msg:
			set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY)
			print "ERROR: GNSS object: Failed to create UDP socket. Error code: " + str(msg[0]) + " Message: " + msg[1]
			set_text_attr(default_colors)
			self.running = False
			answer = raw_input('Press <ENTER> to exit program')
			sys.exit()
			
		self.s_UDP.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
		self.s_UDP.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
		self.mode				= mode
		self.simulation_mode	= simulation_mode
		
		self.host 				= ''  			 #'<broadcast>'		#Bind to all ##"127.0.0.1"
		self.UDP_port 			= UDP_port
		self.TCP_port			= UDP_port
		self.UDP_Telegram		= ''
		self.TCP_Telegram		= ''
		self.IP_address     	= ''
		self.host_name      	= ''
		self.serial_port 		= serial_port
		self.serial_speed		= serial_speed
		self.last_in_block		= last_in_block
		self.id					= 'SN:1'
		self.data				= ''
		self.data_last_24_hours	= []
		self.processing_queue 	= Queue.LifoQueue()	 	# Construct Last-In-First-Out queue that will act as pipe between data collecting thread and data processing thread
		self.TCP_queue 			= Queue.LifoQueue()	 	# Construct Last-In-First-Out queue that will act as pipe between data processing thread and TCP/IP server thread
		self.distribution_queue = Queue.LifoQueue()
		self.TCP_lock			= threading.Lock()
		self.TCP_data			= json.dumps({}) 
		##self.plot_counter		= 0
		##self.plot_time			= -1
		##self.plot_interval		= 20		# In seconds
		##self.plot_filename		= 'gps_plot.png'
									
		self.busy				= False
		self.running			= False
		self.ok					= True
		self.starttime			= ''
		self.timestamp			= 0.0
		self.XML				=''
		self.verbose			= True
		self.msg_counter		= 0
		
		self.json_txt			= ''
		self.terminal_courtesy_message_interval = 15		# In seconds
		self.terminal_line_counter	= self.terminal_courtesy_message_interval - 4
		self.date				= ''
		self.time				= ''
		self.message_prefix		= ''
		self.Latitude_decimal   = ''
		self.Longitude_decimal  =  ''
		if self.verbose:
			print "Created GNSS object"
			print "UDP network broadcast at port=%d" 	% self.UDP_port
		
		self.start()


		
	def start(self):
		self.running = True
		if self.mode == 'GPS':
			try:
				self.GPS_port = serial.Serial(port=self.serial_port, baudrate=self.serial_speed, timeout=1)
			except:
				set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY)
				print "Error: Could not open GNSS serial port:", self.serial_port
				print "Check: Control Panel > System > Hardware > Device Manager > Ports (COM & LPT)"
				print "Exit program"
				set_text_attr(default_colors)
				self.running = False
				answer = raw_input('Press <ENTER> to exit program')
				sys.exit()			
			self.GPS_port.close()
		
		print "Starting threads:"		
		thread.start_new(self.supervisor_thread,())
		sleep(0.1)	
		thread.start_new(self.data_distribution_thread,())
		sleep(0.1)	
		thread.start_new(self.process_data_thread,())
		sleep(0.1)	
		thread.start_new(self.tcp_server_thread,())
		sleep(0.1)
		
		if self.mode == 'GPS':
			thread.start_new(self.serial_port_thread,())
		elif self.mode == 'SIM':
			thread.start_new(self.GPS_simulator_thread,())
		else:
			print "ERROR: Invalid system mode: ", self.mode
			answer = raw_input('Press <ENTER> to exit program')
			sys.exit()
		


	def stop(self):
		self.running = False
		##print self.message_prefix, "says:"
		print "   - %s object" % self.type
		self.s_UDP.shutdown(1)
		self.s_UDP.close()
		print "   - UDP socket"
		



	def serial_port_thread(self):			# -- THREAD: Get data from GPS on serial port
		data_packet = []

		if self.verbose:
			print "   - Started GNSS/GPS serial port data collection on %s" % self.serial_port

		try:
			self.GPS_port = serial.Serial(port=self.serial_port, baudrate=self.serial_speed, timeout=1)
		except:
			set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY)
			print "Error: Could not open GNSS serial port:", self.serial_port
			print "Check: Control Panel > System > Hardware > Device Manager > Ports (COM & LPT)"
			print "Exit program"
			set_text_attr(default_colors)
			sleep(0.1)
			self.running = False
		
		while self.running:
			self.data = self.GPS_port.readline()
			self.data = self.data.strip()
			#print self.data	

			#------ Sanitize date before sending them to procssing
			if ((len(self.data) > 0) and (self.data[0] == "$") and ('*' in self.data)):
			
				#---- $GPGGA or $GNGGA ---------
				if (("$GPGGA" in self.data) or ("$GNGGA" in self.data)):
					data_packet.append(self.data)
					
				#---- $GPRMC or $GNRMC ---------
				if (("$GPRMC" in self.data) or ("$GNRMC" in self.data)):
					data_packet.append(self.data)
					
				#---- $GPGSA or $GNGSA :  GPS/GNSS DOP and Active Satellites ---------						
				if (("$GPGSA" in self.data) or ("$GNGSA" in self.data)):
					data_packet.append(self.data)

				#---- $GPGSV ---------						
				if "$GPGSV" in self.data:
					data_packet.append(self.data)
					
				#---- $GNGNS : GNSS fix data---------						
				if "$GNGNS" in self.data:
					data_packet.append(self.data)

				#---- $GNGST : GNSS Pseudo Range Error Statistics ---------						
				if "$GNGST" in self.data:
					data_packet.append(self.data)
	
				#---- Compare to last NMEA telegram in block  ---------						
				if self.last_in_block in self.data:
					self.processing_queue.put(data_packet)
					data_packet = []	
				
				
		self.GPS_port.close()
		#--  Shutting down
		print "   - GNSS/GPS serial port on %s" % self.serial_port
		print "   - GNSS/GPS data collection thread"
		



	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	#  THREAD: GPS simulation
	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

	def GPS_simulator_thread(self):

		from math import cos, sin
		
		data_packet = []

		RADIUS = 10000
		#PERIOD = 20*1.666662769e-5		# 50 meter in 5 minutes; total 62832m circle length (r=10000m)
		PERIOD = 30*1.666662769e-5		# 50 meter in ca 1 minute; total 62832m circle length (r=10000m)
		SLEEP  = 1.0
		FALSE_EASTING  		= 500000		# UTM
		FALSE_NORTHING 		= 10000000	#
		GREENWICH_MERIDIAN 	= 166021.443179

		print "GPS - simulation: Thread started."
		print "   Simulation mode =", self.simulation_mode
		
		
		###################################################
		#########    SIM MODE A     #######################
		###################################################
		
		if self.simulation_mode == 'A':
			
			# Sources:
			#   http://gis.stackexchange.com/questions/47/what-tools-in-python-are-available-for-doing-great-circle-distance-line-creatio
			#   http://www.movable-type.co.uk/scripts/latlong.html
			import math
			ptlon1 = 12.0
			ptlat1 = 81.0
			ptlon2 = -4.0
			ptlat2 = 82.5
			numberofsegments = 303400
			onelessthansegments = numberofsegments - 1
			fractionalincrement = (1.0/onelessthansegments)
			ptlon1_radians = math.radians(ptlon1)
			ptlat1_radians = math.radians(ptlat1)
			ptlon2_radians = math.radians(ptlon2)
			ptlat2_radians = math.radians(ptlat2)
			distance_radians=2*math.asin(math.sqrt(math.pow((math.sin((ptlat1_radians-ptlat2_radians)/2)),2) + math.cos(ptlat1_radians)*math.cos(ptlat2_radians)*math.pow((math.sin((ptlon1_radians-ptlon2_radians)/2)),2)))
			# 6371.009 represents the mean radius of the earth
			# shortest path distance
			distance_km = 6371.009 * distance_radians

			mylats = []
			mylons = []
			# write the starting coordinates
			mylats.append([])
			mylons.append([])
			mylats[0] = ptlat1
			mylons[0] = ptlon1
			f = fractionalincrement
			icounter = 1
			while (icounter <  onelessthansegments):
				icountmin1 = icounter - 1
				mylats.append([])
				mylons.append([])
				# f is expressed as a fraction along the route from point 1 to point 2
				A=math.sin((1-f)*distance_radians)/math.sin(distance_radians)
				B=math.sin(f*distance_radians)/math.sin(distance_radians)
				x = A*math.cos(ptlat1_radians)*math.cos(ptlon1_radians) + B*math.cos(ptlat2_radians)*math.cos(ptlon2_radians)
				y = A*math.cos(ptlat1_radians)*math.sin(ptlon1_radians) +  B*math.cos(ptlat2_radians)*math.sin(ptlon2_radians)
				z = A*math.sin(ptlat1_radians) + B*math.sin(ptlat2_radians)
				newlat=math.atan2(z,math.sqrt(math.pow(x,2)+math.pow(y,2)))
				newlon=math.atan2(y,x)
				newlat_degrees = math.degrees(newlat)
				newlon_degrees = math.degrees(newlon)
				mylats[icounter] = newlat_degrees
				mylons[icounter] = newlon_degrees
				icounter += 1 
				f = f + fractionalincrement
			# write the ending coordinates
			mylats.append([])
			mylons.append([])
			mylats[onelessthansegments] = ptlat2
			mylons[onelessthansegments] = ptlon2
			# Now, the array mylats[] and mylons[] have the coordinate pairs for intermediate points along the geodesic
			# My mylat[0],mylat[0] and mylat[num_of_segments-1],mylat[num_of_segments-1] are the geodesic end points
			
			k = 0
			
			last = len(mylats)-1
			print "   Start coordinates:",  mylats[0], mylons[0]
			print "   End coordinates:",    mylats[last], mylons[last]
			print
			
			while self.running:
				if k == len(mylats):
					k = 0
				lat, lon = mylats[k], mylons[k]
				##print lat, lon
				k += 1
				
				###########################    DUPLICATE CODE  -   CLEAN UP LATER    ###########
				
				#--------- Check: South of Equator?
				if lat < 0.0:
					hemisphere_NS = 'S'
					lat = -lat
				else:
					hemisphere_NS = 'N'
	
				#-------- West of Greenwich meridian?
				if lon < 0.0:
					hemisphere_EW = 'W'
					lon = -lon
				else:
					hemisphere_EW = 'E'
		
				#-------- Prepare GPS lat/lon format (like 0530.0000,N for 5.5 deg N)
				lat_fraction = '0.' + ("%1.8f" % lat).split('.')[1]
				lon_fraction = '0.' + ("%1.8f" % lon).split('.')[1]

				lat_min = "%07.4f" % (float(lat_fraction) * 60)
				lon_min = "%07.4f" % (float(lon_fraction) * 60)

				lat_deg = "%02d" % (int(("%1.8f" % lat).split('.')[0]))
				lon_deg = "%03d" % (int(("%1.8f" % lon).split('.')[0]))
	
				gps_lat = lat_deg + lat_min
				gps_lon = lon_deg + lon_min
	
				time_hhmmss = strftime("%H%M%S", gmtime())
				date_ddmmyy = strftime("%d%m%y", gmtime())
				GPGGA = "$GPGGA,%s,%s,%s,%s,%s,1,04,2.9,43.8,M,43.9,M,,*78" % (
																	time_hhmmss,
																	gps_lat, hemisphere_NS,
																	gps_lon, hemisphere_EW)

				GPRMC = "$GPRMC,%s,A,%s,%s,%s,%s,0.1,180.0,%s,5.0,E,A*78"  % ( 
																	time_hhmmss,
																	gps_lat, hemisphere_NS,
																	gps_lon, hemisphere_EW,
																	date_ddmmyy)

				data_packet.append(GPGGA)
				data_packet.append(GPRMC)
				self.processing_queue.put(data_packet)
				data_packet = []
				
				
				
				
				
				sleep(1.0)
		

		
		###################################################
		#########    SIM MODE B     #######################
		###################################################
		
		elif self.simulation_mode == 'B':	
		
			GPGGA = ''
			GPRMC = ''
			Time, Day, Month, Year   = '','','',''
	
			t = 0

			while self.running:
		
				r = RADIUS * sin(t*PERIOD/5)	# Polar curve: A nice rose!
				x = r * cos(t*PERIOD)
				y = r * sin(t*PERIOD)
				#x = RADIUS * cos(PERIOD * t)	# These two for a circle
				#y = RADIUS * sin(PERIOD * t)
				t += 1
				easting = GREENWICH_MERIDIAN + x
	
				if y < 0.0:		# South of Equator?
					northing = FALSE_NORTHING + y
					zone = '31M'
				else:
					northing = y
					zone = '31N'
	
				#--------------------------------------------------
				# Convert Easting, Northing to Lat/Long
				(lat, lon)  =  UTMtoLL(23, northing, easting, zone) # Beware: Small numbers given in scientific format, e.g. 1.2345e-05
	
				#--------- Check: South of Equator?
				if lat < 0.0:
					hemisphere_NS = 'S'
					lat = -lat
				else:
					hemisphere_NS = 'N'
	
				#-------- West of Greenwich meridian?
				if lon < 0.0:
					hemisphere_EW = 'W'
					lon = -lon
				else:
					hemisphere_EW = 'E'
		
				#-------- Prepare GPS lat/lon format (like 0530.0000,N for 5.5 deg N)
				lat_fraction = '0.' + ("%1.8f" % lat).split('.')[1]
				lon_fraction = '0.' + ("%1.8f" % lon).split('.')[1]

				lat_min = "%07.4f" % (float(lat_fraction) * 60)
				lon_min = "%07.4f" % (float(lon_fraction) * 60)

				lat_deg = "%02d" % (int(("%1.8f" % lat).split('.')[0]))
				lon_deg = "%03d" % (int(("%1.8f" % lon).split('.')[0]))
	
				gps_lat = lat_deg + lat_min
				gps_lon = lon_deg + lon_min
	
				time_hhmmss = strftime("%H%M%S", gmtime())
				date_ddmmyy = strftime("%d%m%y", gmtime())
				GPGGA = "$GPGGA,%s,%s,%s,%s,%s,1,04,2.9,43.8,M,43.9,M,,*78" % (
																	time_hhmmss,
																	gps_lat, hemisphere_NS,
																	gps_lon, hemisphere_EW)

				GPRMC = "$GPRMC,%s,A,%s,%s,%s,%s,0.1,180.0,%s,5.0,E,A*78"  % ( 
																	time_hhmmss,
																	gps_lat, hemisphere_NS,
																	gps_lon, hemisphere_EW,
																	date_ddmmyy)

				data_packet.append(GPGGA)
				data_packet.append(GPRMC)
				self.processing_queue.put(data_packet)
				data_packet = []
			
				sleep(1.0)
		
		else:
			print "Invalid choice, exit"
			self.running = False

		print "   - GPS simulation thread"





	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	#  THREAD: Process data
	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

	def process_data_thread(self):
		print "   - Data processing"
		no_of_GPS		= (0,0)
		no_of_GLONASS	= (0,0)
		
		while self.running:	
			if not self.processing_queue.empty():
				input = self.processing_queue.get()
				self.processing_queue.task_done()
				
				#--- Reset all fields
				try:
					data.clear()
				except:
					data = {}
				data['Time']					= ''
				data['Date'] 					= ''
				data['Latitude'] 				= ''
				data['Longitude'] 				= ''
				data['NS_Hemisphere'] 			= ''
				data['EW_Greenwich'] 			= ''
				data['Speed_over_ground'] 		= ''
				data['Course_made_good'] 		= ''
				data['Fix_Quality'] 			= ''
				data['Number_of_Satellites']	= ''
				data['Hours']					= ''
				data['Minutes']					= ''
				data['Seconds']					= ''
				data['Day']						= ''
				data['Month']					= ''
				data['Year']					= ''
				data['IP_address']     			= socket.gethostbyname(socket.gethostname())
				data['host_name']      			= socket.gethostname()
				data['Latitude_decimal']		= 0.0
				data['Longitude_decimal']		= 0.0
				data['Latitude_readable']		= ''
				data['Longitude_readable']		= ''
				data['UTM_zone']				= ''
				data['Easting']					= ''
				data['Northing']				= '' 
				#--data['GPGSV_data']				= {}
				#--data['sats_in_use']				= []
				
				data['RMC_status']  			= 'Invalid'
				
				data['GSA_opMode']				= ''
				data['GSA_navMode']				= ''
				data['GSA_PDOP']				= ''
				data['GSA_HDOP']				= ''
				
				data['GST_stdLat']				= '_'
				data['GST_stdLong']				= '_'
				
				data['GGA_Fix_Quality']			= ''
				data['GGA_nsats']				= ''
				data['GGA_HDOP']				= ''
				
				data['GNS_posMode']				= ''
				data['GNS_nsats']				= ''
				data['GNS_HDOP']				= ''
				
				GNSS							= {}
				GNSS['GPS'] 	= 0
				GNSS['GLONASS'] = 0
				GNSS['Galileo'] = 0
				GNSS['BeiDou']	= 0
				GNSS['SBAS']	= 0
				
				
				for item in input:					
					L = item.split(',')					
					
					#----  GPGGA or GNGGA   ----					
					if (("$GPGGA" in item) or ("$GNGGA" in item)):
						data['GGA_Fix_Quality']			= L[6]
						data['GGA_nsats']				= L[7]
						data['GGA_HDOP']				= L[8]
					
					
					#----  GPRMC or GNRMC  ----					
					if (("$GPRMC" in item) or ("$GNRMC" in item)):
						if L[2] == 'V':
							data['RMC_status']  = 'invalid'
						else:
							data['RMC_status'] 			= 'ok'
							data['Latitude']   			= L[3]
							data['Longitude']  			= L[5]
							data['NS_Hemisphere']		= L[4]
							data['EW_Greenwich']		= L[6]
							data['Time']       			= L[1]
							data['Date']       			= L[9]
							data['Speed_over_ground']	= L[7]		# In knots
							data['Course_made_good'] 	= L[8]
							
							if '.' in data['Time']:
								data['Time'] = data['Time'].split('.')[0]   # Get integer part of time (some GPS provide fraction of seconds)
							
							data['Hours']				= data['Time'][:2]
							data['Minutes']				= data['Time'][2:4]
							data['Seconds']				= data['Time'][4:6]
							data['Day']					= data['Date'][:2]
							data['Month']				= data['Date'][2:4]
							data['Year']				= "20" + data['Date'][4:6]		# A year 2100 problem is ticking here ...
							
							if (data['Latitude'] and data['Longitude'] and data['NS_Hemisphere'] and data['EW_Greenwich']):
									data['Latitude_decimal'] 	= float(data['Latitude'][:2])  + (float(data['Latitude'][2:])/60.0)
									data['Longitude_decimal']	= float(data['Longitude'][:3]) + (float(data['Longitude'][3:])/60.0)
									
									la 	= "%1.5f" % data['Latitude_decimal']
									lo	= "%1.5f" % data['Longitude_decimal']
									la = (float(la) - int(data['Latitude'][:2])) * 60.0
									lo = (float(lo) - int(data['Longitude'][:3])) * 60.0
									
									
									##data['Latitude_readable']	= "%s&#176; %05.3f&#39; %s" % (data['Latitude'][:2].lstrip('0'),  la, data['NS_Hemisphere'])
									##data['Longitude_readable']	= "%s&#176; %05.3f&#39; %s" % (data['Longitude'][:3].lstrip('0'), lo, data['EW_Greenwich'])
									data['Latitude_readable']	= "%s&#176; %05.3f&#39; %s" % (data['Latitude'][:2],  la, data['NS_Hemisphere'])
									data['Longitude_readable']	= "%s&#176; %05.3f&#39; %s" % (data['Longitude'][:3], lo, data['EW_Greenwich'])
									
									#East Longitudes are positive, West longitudes are negative. 
									#North latitudes are positive, South latitudes are negative
									
									if data['NS_Hemisphere'] == 'S':
										data['Latitude_decimal']  *= -1.0
									if data['EW_Greenwich']  == 'W':	
										data['Longitude_decimal'] *= -1.0
										
									# '23' = WGS 84
									( data['UTM_zone'], data['Easting'], data['Northing'] ) = LLtoUTM( 23, data['Latitude_decimal'], data['Longitude_decimal'] ) 
									
									#e = "%1.1f" % e
									#n = "%1.1f" % n
									#print
									#print z, e, n, self.Number_of_Satellites, self.HDOP
							
					
					
					#----  GPGSV  =  ----					
					'''
					if "$GPGSV" in item:			# $GPGSV,3,1,11,  25,70,246,44,  12,64,098,34,  14,36,243,44,  29,30,197,41*7B
													#                  4              8             12             16
						try:					
							for k in range(4):
								m = 4*(k+1)
								if '*' in L[m+3]:
									L[m+3] = L[m+3].split('*')[0]
								data['GPGSV_data'][L[m]] = ( (L[m+1], L[m+2], L[m+3]) )
						except:
							pass
					'''
					
					#----  GNGNS  =  GNSS fix data					
					if "$GNGNS" in item:
						data['GNS_posMode']		= L[6]
						data['GNS_nsats']		= L[7]
						data['GNS_HDOP']		= L[8]
					
		
					#----  GPGSA  or GNGSA =  DOP and active satellites  ----					
					if (("$GPGSA" in item) or ("$GNGSA" in item)):					
						data['GSA_opMode']		= L[1]
						data['GSA_navMode']		= L[2]
						data['GSA_PDOP']		= L[15]
						data['GSA_HDOP']		= L[16]
						for k in range(12):
							if not L[k+3]: break
							if 0  < int(L[k+3]) < 33:  GNSS['GPS'] 		+= 1
							if 64 < int(L[k+3]) < 97:  GNSS['GLONASS'] 	+= 1
						# Some GNSS receivers transmits GSA telegram each 5th second, so keep values for a while
						no_of_GPS		= (GNSS['GPS'], 	int(time()))
						no_of_GLONASS	= (GNSS['GLONASS'], int(time()))

						
						
					#----  GNGST  = GNSS Pseudo Range Error Statistics  ----				
					if "$GNGST" in item:
						data['GST_stdLat']		= L[6]
						data['GST_stdLong']		= L[7]
						

				#----   End of processing, now forward package to data distribution thread				
				self.distribution_queue.put(data)	
				
				#----	Print info to screen
				if ( (GNSS['GPS'] == 0) and (GNSS['GLONASS'] == 0) ):	# We have a GNSS receicer that does not emit GSA regularly; use old data in that case
					(GNSS['GPS'], t)		= no_of_GPS
					(GNSS['GLONASS'], t)	= no_of_GLONASS
					data['GST_stdLat']	= ' '	# In this case, these parameters will not be available, but
					data['GST_stdLong']	= ' '

					
				if (data['Hours'] and data['Minutes'] and data['Seconds']):
					print "%s:%s:%s  %s  %09.6f%s  %010.6f%s  %02d|%02d  %s  %sm / %sm" 	% ( data['Hours'], data['Minutes'], data['Seconds'],
																				data['RMC_status'],
																				data['Latitude_decimal'], data['NS_Hemisphere'],
																				data['Longitude_decimal'], data['EW_Greenwich'],
																				GNSS['GPS'], GNSS['GLONASS'],
																				data['GGA_HDOP'],
																				data['GST_stdLat'], data['GST_stdLong'] ),
				else:
					print "No fix - %s" % data['RMC_status'],
	
				if self.mode == 'GPS':
					print
				else:
					print " [mode = %s]" % self.mode
					
				if (data['Seconds'] == '00'):
					set_text_attr(FOREGROUND_YELLOW | default_bg | FOREGROUND_INTENSITY)
					print "------------------------------------------------------------------"
					print "  UTC    Fix  Lat         Long         US|RU  HDOP  StdLat/StdLong"
					print "------------------------------------------------------------------"
					set_text_attr(default_colors)
					
				if (data['Seconds'] == '30'):
					set_text_attr(FOREGROUND_YELLOW | default_bg | FOREGROUND_INTENSITY)
					print "Terminate program by pressing <ENTER> key"
					set_text_attr(default_colors)

			sleep(0.1)
		sleep(0.2)
		print "   - Data processing thread"		# Exit program
		
		
		
		
		
	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	#  THREAD: Data distribution
	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

	def data_distribution_thread(self):
		print "   - Data distribution"
		self.UDP_external_network_blocked = False
		while self.running:	
			if not self.distribution_queue.empty():
				input = self.distribution_queue.get()
				self.distribution_queue.task_done()
				
				##Time_Unix_epoch  =  int(timegm(strptime("%s %s %s %s%s%s " % (input['Day'], input['Month'], input['Year'], input['Hours'], input['Minutes'], input['Seconds']) + "UTC", "%d %m %Y %H%M%S %Z")))
				##print Time_Unix_epoch
				##print strftime("%d-%b-%Y, %H:%M:%S [UTC]", gmtime(Time_Unix_epoch))
				self.msg_counter 		+= 1
				try:
					TCP.clear()
				except:
					TCP = {}
				TCP['type'] 			= 'GPS'
				TCP['id']				= self.id
				TCP['mode']				= self.mode
				TCP['counter']			= self.msg_counter
				TCP['ip']				= input['IP_address']
				TCP['host']				= input['host_name']
				##print input
				##exit()
				TCP['RMC_status']		= input['RMC_status'] 	# 'ok' or 'invalid'
				TCP['time']				= "%s:%s:%s" % (input['Hours'], input['Minutes'], input['Seconds']) 
				TCP['date']				= "%s.%s.%s" % (input['Day'],   input['Month'],   input['Year'] )
				TCP['lat']				= input['Latitude_readable']   ##input['Latitude']
				TCP['lon']				= input['Longitude_readable']
				
				try:
					Time_Unix_epoch  		=  int(timegm(strptime("%s %s %s %s%s%s " % (input['Day'], input['Month'], input['Year'], input['Hours'], input['Minutes'], input['Seconds']) + "UTC", "%d %m %Y %H%M%S %Z")))
					TCP['unix_time']		= "%d" % Time_Unix_epoch
					TCP['lat_decimal']		= "%1.6f" % input['Latitude_decimal']
					TCP['lon_decimal']		= "%1.6f" % input['Longitude_decimal']
					TCP['utm_zone']			= input['UTM_zone']
					TCP['easting']			= "%1.1f" % input['Easting']
					TCP['northing']			= "%1.1f" % input['Northing']
					TCP['hemisphere_NS']	= input['NS_Hemisphere']
					TCP['hemisphere_EW']    = input['EW_Greenwich']
				except:
					TCP['unix_time']		= ''
					TCP['lat_decimal']		= ''
					TCP['lon_decimal']		= ''
					TCP['utm_zone']			= ''
					TCP['easting']			= ''
					TCP['northing']			= '' 
					TCP['hemisphere_NS']	= ''
					TCP['hemisphere_EW']    = ''
				
				TCP['GGA_nsats']			= input['GGA_nsats']
				TCP['GGA_hdop']				= input['GGA_HDOP']
				
				TCP['GNS_nsats']			= input['GNS_nsats']
				TCP['GNS_hdop']				= input['GNS_HDOP']
				
				##TCP['plot_counter'] 	= '0'
				##TCP['plot_filename'] 	= ''
				##TCP['status'] 			= 'ok'
				##TCP['filename']			= ''
				##if ( input['Year'] and input['Month'] and input['Day'] and input['Hours'] and input['Minutes'] and input['Seconds'] ):
				##	TCP['filename'] 		= '%s_%s_%s_%s%s%s' % (input['Year'], input['Month'], input['Day'], input['Hours'], input['Minutes'], input['Seconds'] )
				##TCP['delta']            = ''
				
				try:
					UDP.clear()
				except:
					UDP = {}
				UDP['type'] 			= 'GPS'
				UDP['id'] 				= self.id
				UDP['mode']				= self.mode
				UDP['counter']			= self.msg_counter
				UDP['status'] 			= 'ok'
				UDP['ip']				= input['IP_address']
				UDP['host']				= input['host_name']
				UDP['RMC_status']		= input['RMC_status'] # 'ok' or 'invalid'
				UDP['time']				= "%s:%s:%s" % (input['Hours'], input['Minutes'], input['Seconds']) 
				UDP['date']				= "%s.%s.%s" % (input['Day'],   input['Month'],   input['Year'] )
				UDP['lat']				= input['Latitude_readable']   ##input['Latitude']
				UDP['lon']				= input['Longitude_readable']
				UDP['latitude']         = input['Latitude']
				UDP['longitude']        = input['Longitude']
				UDP['hemisphere_NS']	= input['NS_Hemisphere']
				UDP['hemisphere_EW']    = input['EW_Greenwich']
				UDP['time_raw']         = input['Time']
				UDP['date_raw']         = input['Date']
				UDP['unix_time']		= TCP['unix_time']
				try:
					UDP['lat_decimal']		= "%1.6f" % input['Latitude_decimal']
					UDP['lon_decimal']		= "%1.6f" % input['Longitude_decimal']
					UDP['utm_zone']			= input['UTM_zone']
					UDP['easting']			= "%1.1f" % input['Easting']
					UDP['northing']			= "%1.1f" % input['Northing'] 
				except:
					UDP['lat_decimal']		= ''
					UDP['lon_decimal']		= ''
					UDP['utm_zone']			= ''
					UDP['easting']			= ''
					UDP['northing']			= ''
					
				UDP['GGA_nsats']		= input['GGA_nsats']
				UDP['GGA_HDOP']			= input['GGA_HDOP']
				
				UDP['GNS_nsats']		= input['GNS_nsats']
				UDP['GNS_HDOP']			= input['GNS_HDOP']
				UDP['GNS_posMode']		= input['GNS_posMode']				
				
				UDP['GSA_opMode']		= input['GSA_opMode']
				UDP['GSA_navMode']		= input['GSA_navMode']
				UDP['GSA_PDOP']			= input['GSA_PDOP']
				UDP['GSA_HDOP']			= input['GSA_HDOP']
				
				UDP['GST_stdLat']		= input['GST_stdLat']
				UDP['GST_stdLong']		= input['GST_stdLong']				
				
				self.TCP_lock.acquire() 
				self.TCP_data = json.dumps(TCP)
				self.TCP_lock.release()
				##print "   Distribution thread: Data available for TCP/IP server"
				
				self.UDP_Telegram = json.dumps(UDP)
				try:		# UDP broadcast to own (internal) network interface
					self.s_UDP.sendto(self.UDP_Telegram, ('127.0.0.1', self.UDP_port))	# Send UDP telegram
					#self.s_UDP.sendto(self.UDP_Telegram, (self.host, self.UDP_port))	# Send UDP telegram
					##print TCP['time'], "UTC: UDP telegram -> "
				except socket.error, msg:
					set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY)
					print "ERROR: Unable to send internal UDP telegram. Code=" + str(msg[0]) + ": " + msg[1]
					set_text_attr(default_colors)
				
				
				try:	# UDP broadcast to external network interface
					self.s_UDP.sendto(self.UDP_Telegram, ('<broadcast>', self.UDP_port))	# Send UDP telegram
					#self.s_UDP.sendto(self.UDP_Telegram, (self.host, self.UDP_port))	# Send UDP telegram
					##print TCP['time'], "UTC: UDP telegram -> "
					if self.UDP_external_network_blocked:		# If we got so far, sending was a success.
						set_text_attr(FOREGROUND_GREEN | default_bg | FOREGROUND_INTENSITY)
						print "Now sending external UDP telegram"
						set_text_attr(default_colors)
						self.UDP_external_network_blocked = False
				except socket.error, msg:
					if not self.UDP_external_network_blocked:
						set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY)
						print "WARNING: Unable to send UDP telegram on external network interface"
						print "         Code=" + str(msg[0]) + ": " + msg[1]
						print "         UDP telegram now only sent on internal network interface"
						print "         Automatically reverting to external if it becomes available"
						set_text_attr(default_colors)
					self.UDP_external_network_blocked = True
					
				
			sleep(0.1)
		
		sleep(0.1)
		print "   - Data distribution thread"


		
		
	

	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	#  THREAD: TCP server, with handler that is called for each new clent
	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

	#---   Routine that handles each new TCP socket client.  Data is continously sent to client,
	#---   as JSON-formatted telegram. 
	#---   Try this command in terminal window: "telnet 127.0.0.1 20001", to see raw data.
	#---   Polling is used (at 3 Hz) to determine arrival of new data; this means there
	#---   can be a delay of 0.33 seconds until client gets new data (which is acceptable
	#---   for this application).
	
	def handle_client(self, client_socket, address, client_number):
		set_text_attr(FOREGROUND_BLUE | default_bg | FOREGROUND_INTENSITY)
		print "TCP client #%d connected" % client_number
		set_text_attr(default_colors)
		last_timestamp = ''
		
		while self.running:
			self.TCP_lock.acquire()
			data = json.loads(self.TCP_data)
			self.TCP_lock.release()
			
			if data['unix_time'] != last_timestamp:
				try:
					sent = client_socket.send(json.dumps(data))
					last_timestamp = data['unix_time']
					if sent == 0:
						set_text_attr(FOREGROUND_BLUE | default_bg | FOREGROUND_INTENSITY)
						print "TCP client #%d exits"  %  client_number
						set_text_attr(default_colors)
						break
				except:
					set_text_attr(FOREGROUND_BLUE | default_bg | FOREGROUND_INTENSITY)
					print "TCP client #%d abruptly terminated TCP socket connection" % client_number
					set_text_attr(default_colors)
					break
			sleep(0.33)
		client_socket.close()


#--- This is the container TCP server. It spawns a new handler for each client (max 5 simultaneously)

	def tcp_server_thread(self):
		s2 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		s2.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR,1)
		s2.bind(('', self.TCP_port))
		s2.listen(5)
		print "   - TCP server running on port:", self.TCP_port
		self.TCP_client_number = 1
		while self.running:
			(client_socket, address) = s2.accept()        # Blocking wait ...
			t = threading.Thread(target=self.handle_client, args=(client_socket, address, self.TCP_client_number))
			self.TCP_client_number += 1
			t.daemon = True
			t.start()

		print "   - TCP server thread"	# NOTE: Since accept() is a blocking wait, this thread will never exit





	
	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	#  THREAD: Superviser
	#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

	def supervisor_thread(self):
		print "   - Supervisor"	
		while self.running:
			sleep(0.1)
			
		sleep(0.3)
		print "   - Supervisor thread"




#===================================================================
#========        S T A R T   O F   P R O G R A M
#===================================================================

print VERSION
sleep(3)

Running 			= True
Mode				= 'GPS'

Simulation_mode = ''
if len(sys.argv) > 1:					#-----  Should program run in simulator mode?
	if (sys.argv[1].upper() == 'SIM'):
		print
		print "SYSTEM RUNNING IN SIMULATION MODE"
		Mode = sys.argv[1].upper()
		print '''

    A = Great circle starting at 81degN/12degE to 82.5degN/-4degW (303.4km),
        travelling approx 1m/sec
    B = Rose pattern centered on crossing of Greenwich and Equator, testing
        all four quadrants

    Your choice:
'''
	Simulation_mode = raw_input().upper()	# No input check - this feature only used for testing Mini-GUNCO program
	GPS_SERIAL_PORT, GPS_SERIAL_SPEED, block_tail = 'COM1', 9600, ''	# Need dummy values for simulator mode
	sleep(2)
else:
	(GPS_SERIAL_PORT, GPS_SERIAL_SPEED, block_tail)  = check_serial_ports()


	
thread.start_new(keyboard_watch,())			# Start thread listening on keyboard ...

#--- Create GNSS / GPS / SIMULATOR object ...
GPS = c_GPS(serial_port=GPS_SERIAL_PORT, serial_speed=GPS_SERIAL_SPEED, UDP_port=GPS_UDP_PORT, mode=Mode, simulation_mode=Simulation_mode, last_in_block=block_tail)

while Running:
	sleep(1.0)

GPS.stop()

sleep(1)
set_text_attr(FOREGROUND_GREY | default_bg)
sys.exit(0)