#!/usr/bin/env python
# Parse a file with NMEA GGA & ZDA to get the latency at the 
# IO layer of the receiver. The time stamp in GGA is the 
# time of validity of the position, the ZDA is created by the receiver
# after the GGA and is the current time. Difference the time tags,
# and you get an approximation of the latency.
#
# Copyright Trimble Inc 2022

import datetime
import leapseconds
import re
import sys

def checksumtest(s):
  '''Returns True if NMEA input 's' passes its checksum'''
  tokens = s.split(b'*')
  if len(tokens) >= 2:
    cs1 = int(tokens[1][:2].decode(),16)
    cs2 = 0
    for val in tokens[0][1:]:
      cs2 ^= val
  else:
    return False
  return (cs1 == cs2)

if __name__ == "__main__":
 
  if(len(sys.argv) != 2):
    print("Usage: parseNMEA.py logged_gga_zda_file.txt")
    sys.exit(-1)

  filename = sys.argv[1]
  
  # Reference GPS Week
  gps_epoch = datetime.datetime(1980,1,6)

  gotGGA = False
  GotLeapSeconds = False
  with open(filename,'r') as fid:
    for line in fid:
      received = bytes(line,'ascii')
      s = received.rstrip()
      tokens = s.split(b',')
      if len(tokens) < 5:
        continue
      if len(tokens[2]) == 0:
        continue

      if(GotLeapSeconds == False):
        if( (re.match(b'\$G.ZDA.*',s) or re.match(b'\$I.ZDA.*',s)) and checksumtest(s)):
          time_str = bytearray(b',').join(tokens[1:5]).decode('ascii')
          utc_date = datetime.datetime.strptime(time_str,'%H%M%S.%f,%d,%m,%Y')
          # Leap second call is expensive, so only do it once (at least on a PC under WSL)
          gps_time = leapseconds.utc_to_gps(utc_date) - gps_epoch
          # Get the leap seconds
          LS = (gps_time - (utc_date - gps_epoch )).seconds
          GotLeapSeconds = True
        else:
          continue

      if( (re.match(b'\$G.GGA.*',s) or re.match(b'\$I.GGA.*',s)) and checksumtest(s)):
        # Save the GGA
        GGAData = s  
        gotGGA = True
      elif((gotGGA == True) and checksumtest(s) and (re.match(b'\$G.ZDA.*',s) or re.match(b'\$I.ZDA.*',s))):
        time_str = bytearray(b',').join(tokens[1:5]).decode('ascii')
        utc_date = datetime.datetime.strptime(time_str,'%H%M%S.%f,%d,%m,%Y')
        secs = (utc_date - gps_epoch).total_seconds() + LS
        WN = int(secs/(7*86400))
        secs = secs - WN*86400*7
        #print('%.3f %.3f' % (secs, float(tokens[1]) - float(GGAData.split(b',')[1])))
        print('%.3f %.6f' % (secs, float(tokens[1]) - float(GGAData.split(b',')[1])))
        gotGGA = False