#!/usr/bin/python3

import pynmea2
import os
import threading
import socket
import time
import datetime
import sys
import copy

NMEAudpPort = None
# this is to get data from gps-share instead of the gnss directly
gsIP = '127.0.0.1'
#gsPort = 10110
gsPort = 0
VERSION = "0.4"
debug = False
UPDATE_DELAY = 1.0  # number of seconds between screen updates


class GtsData:

    def __init__(self):
        self.lat = 0.0
        self.lon = 0.0
        self.altitude = 0.0
        self.timestamp = None
        self.datestamp = None
        self.sats = {}
        self.sats['GP'] = {}
        self.sats['GL'] = {}
        self.gpSats = []
        self.glSats = []
        self.leap_seconds = 18
        self.accuracy = '99.0'
        self.pos_status = 'V'
        self.fix_type = '0'
        self.lock = threading.Lock()
        self.startTime = time.time()
        self.ttff = 0

    @property
    def solSats(self):
        with self.lock:
            sats = copy.deepcopy(self.gpSats) + copy.deepcopy(self.glSats)
        return sats

    @property
    def datetime(self):
        if self.datestamp and self.timestamp:
            return datetime.datetime.combine(self.datestamp, self.timestamp)

        return None

    @property
    def antenna(self):
        ret = 0
        if self.pos_status == 'A':
            ret = ret | 1
        if len(self.solSats) > 0:
            ret = ret | 2

        return ret

    @property
    def fixsuc(self):
        if self.pos_status == 'A':
            return 1

        return 0

    @property
    def fix(self):
        if self.fix_type == '2':
            return '2D'
        elif self.fix_type == '3':
            return '3D'
        else:
            return 'None'

        return 'NA'

    @property
    def fixtime(self):
        if self.datetime:
            return (self.datetime - datetime.datetime(1980, 1, 6)).total_seconds() + self.leap_seconds

        return 0

    @property
    def utctime(self):
        if self.datetime:
            return int(time.mktime(self.datetime.timetuple()))

        return 0

    @property
    def timems(self):
        if not self.timestamp:
            return 0

        # we need to spoof milliseconds because the module doesn't provide that
        timesec = time.time()
        return int((timesec - int(timesec)) * 1000)

    @property
    def time10ms(self):
        if not self.timestamp:
            return 0

        # we need to spoof milliseconds because the module doesn't provide that
        timesec = time.time()
        return int((timesec - int(timesec)) * 100)


class GnssThread(threading.Thread):

    def __init__(self, ip=None, port=None):
        threading.Thread.__init__(self)
        self.gts = GtsData()
        self.NMEAdata = []
        if port != 0:
            self.gnss = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            if ip is not None and port is not None:
                self.gnss.connect((ip, port))
        else:
            self.gnss = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
            self.gnss.connect('/var/run/gnss-share.sock')
        self.almanac = False
        self.running = True

    def write_cmd(self, cmd, expect=None):
        print("cmd: %s" % str(cmd).encode('ascii'))
        self.gnss.send(str(cmd).encode("ascii"))
        self.gnss.send(b'\r\n')
        if expect:
            for i in range(20):
                line = self.gnss.recv(1024)
                if expect.encode("ascii") in line:
                    sys.stderr.write('found: %s:%s\n' % (expect, line))
                    return True, line

            sys.stderr.write('not found: %s:%s\n' % (expect, line))
            return False, line

        # wait for cmd completion
        for i in range(20):
            line = self.gnss.recv(1024)
            if str(cmd).encode("ascii") in line:
                return True, line

        return False, line

    def set_constellation(self, cons_mask):
        '''
        bit 0: GPS constellation enabling/disabling
        bit 1: GLONASS constellation enabling/disabling
        bit 2: QZSS constellation enabling/disabling
        bit 3: GALILEO constellation enabling/disabling
        bit 7: BEIDOU constellation enabling/disabling
        '''

        msg = pynmea2.GGA('P', 'STMSETCONSTMASK', (str(cons_mask,)))
        self.write_cmd(msg)

    def wait_for_start(self):
        started = False
        while not started:
            data = self.gnss.recv(32768)
            lines = data.decode('ascii', errors='replace').split("\n")
            lines = filter(None, lines)
            data = b''
            for line in lines:
                if not line.startswith('$'):
                    continue
                try:
                    pynmea2.parse(line)
                    print("Found valid NMEA: %s\n" % line)
                    started = True
                except:
                    continue

    def run(self):
        self.wait_for_start()
        self.set_constellation(143)
        data = b''
        while self.running:
            data += self.gnss.recv(32768)
            if not data.decode('ascii', errors='replace').endswith("\n"):
                # sys.stderr.write('skipping: %s\n' % data)
                continue
            if debug:
                sys.stderr.write('read: %s\n' % data)

            lines = data.decode('ascii', errors='replace').split("\n")
            lines = filter(None, lines)
            data = b''
            for line in lines:
                if not line.startswith('$'):
                    continue
                msg = None
                if NMEAudpPort:
                    self.NMEAdata.append(line)
                if True:
                    try:
                        msg = pynmea2.parse(line)
                    except:
                        continue
                    if debug:
                        print(repr(msg))
                    if isinstance(msg, pynmea2.types.talker.GSV):
                        with self.gts.lock:
                            if msg.msg_num == '1':
                                self.gts.sats[msg.talker] = {}
                                if debug:
                                    print("clearing in view satelites:", msg.talker)
                            if len(msg.sv_prn_num_1):
                                if msg.snr_1:
                                    self.gts.sats[msg.talker][msg.sv_prn_num_1] = (msg.elevation_deg_1, msg.azimuth_1, msg.snr_1)
                            if len(msg.sv_prn_num_2):
                                if msg.snr_2:
                                    self.gts.sats[msg.talker][msg.sv_prn_num_2] = (msg.elevation_deg_2, msg.azimuth_2, msg.snr_2)
                            if len(msg.sv_prn_num_3):
                                if msg.snr_3:
                                    self.gts.sats[msg.talker][msg.sv_prn_num_3] = (msg.elevation_deg_3, msg.azimuth_3, msg.snr_3)
                            if len(msg.sv_prn_num_4):
                                if msg.snr_4:
                                    self.gts.sats[msg.talker][msg.sv_prn_num_4] = (msg.elevation_deg_4, msg.azimuth_4, msg.snr_4)
                            # print( "sats:" , self.gts.sats)
                    elif isinstance(msg, pynmea2.types.talker.GGA):
                        # print(repr(msg))
                        self.gts.timestamp = msg.timestamp
                        self.gts.lon = msg.longitude
                        self.gts.lat = msg.latitude
                        self.gts.altitude = msg.altitude
                        self.gts.accuracy = msg.horizontal_dil
                    elif isinstance(msg, pynmea2.types.talker.RMC):
                        # print(repr(msg))
                        self.gts.datestamp = msg.datestamp
                        self.gts.timestamp = msg.timestamp
                        self.gts.pos_status = msg.status
                    elif isinstance(msg, pynmea2.types.talker.GSA):
                        with self.gts.lock:
                            solSats = []
                            self.gts.fix_type = msg.mode_fix_type
                            if len(msg.sv_id01):
                                solSats.append(msg.sv_id01)
                            if len(msg.sv_id02):
                                solSats.append(msg.sv_id02)
                            if len(msg.sv_id03):
                                solSats.append(msg.sv_id03)
                            if len(msg.sv_id04):
                                solSats.append(msg.sv_id04)
                            if len(msg.sv_id05):
                                solSats.append(msg.sv_id05)
                            if len(msg.sv_id06):
                                solSats.append(msg.sv_id06)
                            if len(msg.sv_id07):
                                solSats.append(msg.sv_id07)
                            if len(msg.sv_id08):
                                solSats.append(msg.sv_id08)
                            if len(msg.sv_id09):
                                solSats.append(msg.sv_id09)
                            if len(msg.sv_id10):
                                solSats.append(msg.sv_id10)
                            if len(msg.sv_id11):
                                solSats.append(msg.sv_id11)
                            if len(msg.sv_id12):
                                solSats.append(msg.sv_id12)
                            if len(solSats):
                                if int(solSats[0]) > 64:
                                    # GLONASS sats
                                    self.gts.glSats = solSats
                                else:
                                    self.gts.gpSats = solSats

            if self.gts.fix not in ['2D', '3D']:
                self.gts.ttff = 0

            if self.gts.fix == '3D':
                if self.gts.ttff == 0:
                    self.gts.ttff = time.time() - self.gts.startTime


if __name__ == '__main__':
    if gsIP is not None:
        gnsst = GnssThread(gsIP, gsPort)
    while not gnsst.running:
        time.sleep(0.5)

    gnsst.start()
    print('Fix time      ', gnsst.gts.fixtime)
    if True:
        while True:
            if not debug:
                os.system('clear')

            sats = ''
            viewSats = ''
            solSats = ''
            with gnsst.gts.lock:
                localSats = copy.deepcopy(gnsst.gts.sats)
            localSolSats = gnsst.gts.solSats
            # print( "sats: ", localSats)
            for key in localSats:
                # print( "Checking constelation %s"% key)
                for sat in localSats[key]:
                    # print( "found sat: ", sat)
                    if len(sats) > 0:
                        sats = sats + '&'
                        viewSats = viewSats + ' '
                    sats = sats + 'S%s:%.1f' % (sat, float(localSats[key][sat][2]))
                    viewSats = viewSats + 'S%s:%.1f' % (sat, float(localSats[key][sat][2]))
                    if sat in localSolSats:
                        solSats = solSats + 'S%s:%.1f ' % (sat, float(localSats[key][sat][2]))

            print
            print('GPS %s' % (VERSION))
            print('--------------------------------------')
            print('longitude      %3.8f' % gnsst.gts.lon)
            print('latitude       %2.8f' % gnsst.gts.lat)
            print('altitude (m)   %.2f' % gnsst.gts.altitude)
            print('accuracy       %s' % gnsst.gts.accuracy)
            print('fix            %s' % gnsst.gts.fix)
            print('View sats             ')
            print(viewSats)
            print('Solution sats             ')
            print(solSats)
            print('timestamp      %s.%02d' % (gnsst.gts.datetime, gnsst.gts.time10ms))
            print('Fix time       %.3f' % gnsst.gts.utctime)
            print('TTFF           %.1f' % gnsst.gts.ttff)
            print

            while (len(gnsst.NMEAdata)):
                data = gnsst.NMEAdata.pop()
                if debug:
                    print("nmea send: ", data)

            time.sleep(UPDATE_DELAY)

    print("Done.\nExiting.")
