#!/usr/bin/env python
usage="""
First turn on GGA+ZDA and enable DEBUG_TIMESTAMP_TCP_PACKET in the code.
Log some the data, e.g.
  nc -v 10.1.x.y 28001 > diag.txt
Plot with:
  ./plot_nmea_eth_latency.py diag.txt
"""

from pylab import *
import sys
import datetime
import leapseconds
import argparse

class NMEAToGPSTime:
    def __init__(self):
        self.gps_epoch = datetime.datetime(1980,1,6)
        self.have_leap_sec = False
        self.leap_sec = 0

    def convert(self,tod,days,months,years):
        time_str = tod + ',' + days + ',' + months + ',' + years
        utc_date = datetime.datetime.strptime(time_str,'%H%M%S.%f,%d,%m,%Y')
        if not self.have_leap_sec:
            gps_time = leapseconds.utc_to_gps(utc_date) - self.gps_epoch
            self.leap_sec = (gps_time - (utc_date - self.gps_epoch )).seconds
            print("Using leap_sec = %d"%self.leap_sec)
            self.have_leap_sec = True
        gps_time = (utc_date - self.gps_epoch).total_seconds() + self.leap_sec
        return gps_time % (86400*7.0)


def parse_data(filename):
    print("Parsing data...")
    nmea_to_gps = NMEAToGPSTime()
    gps_epoch = datetime.datetime(1980,1,6)
    s_in = open(filename,'r')
    d = []
    last_l2 = []
    last_l3 = []
    last_dt = -1
    while True:
        l1=s_in.readline()
        if l1.find('ZDA') > 0:
            l1=s_in.readline()
            print("shift...")
        l1=l1.split(',')
        l2=s_in.readline()
        l2=l2.split(',')
        l3=s_in.readline()
        l3=re.split(r',|\*',l3)
        if len(l1)<2 or len(l2)<5 or len(l3)<3:
            break
        t1 = nmea_to_gps.convert(l1[1],l2[2],l2[3],l2[4])
        t2 = nmea_to_gps.convert(l2[1],l2[2],l2[3],l2[4])
        dt = t2-t1
        dt_eth = nan

        if len(last_l2) > 0:
            last_zda_frac = last_l2[1].split('.')[1]
            if last_zda_frac == l3[1]:
                t_u32 = np.array([int(last_l3[2]),int(l3[3])],dtype=np.uint32)
                dt_eth = last_dt + np.diff(t_u32)[0]*1e-6
            else:
                print('mismatch ',last_zda_frac,l3[1],l1[1])
        d.append( [t1,dt,dt_eth] )

        last_l2 = l2
        last_l3 = l3
        last_dt = dt
    print(" done!")

    d = array(d)

    t_zda = d[1:,0]
    t_eth = d[:-1,0]
    d = d[1:,:]
    return t_zda, t_eth, d


def plot_data( t_zda, t_eth, d, plot_hours ):
    t_units = 'secs'
    scale = 1.0
    if plot_hours:
        scale = 1.0/(60*60)
        t_units = 'hours'
    fig,ax=subplots(1,1)
    ax.plot(t_eth*scale,d[:,2]*1e3)
    ax.plot(t_zda*scale,d[:,1]*1e3)
    ax.set_ylabel('latency [ms]')
    i = where(isnan(d[:,2]))[0]
    if len(i) > 0:
        ax.plot(t_eth[i]*scale,d[0,1]*1e3*ones(len(i)),'ro')
        print("%d missing ETH epochs"%len(i))
    i=argmax(d[:,1])
    print('max ZDA latency %.3f ms @ %.3f [%s]'%(d[i,1]*1e3,d[i,0]*scale,t_units))
    i=nanargmax(d[:,2])
    print('max ETH latency %.3f ms @ %.3f [%s]'%(d[i,2]*1e3,d[i,0]*scale,t_units))

    legend(['ETH','ZDA'])
    ax.set_xlabel('GPS %s'%t_units)
    ax.grid()

    fig2,ax2=subplots(1,1)
    ax2.plot(t_eth[1:]*scale,(d[1:,2]-d[:-1,1])*1e3,'.',markersize=1)
    ax2.set_xlabel('GPS %s'%t_units)
    ax2.set_ylabel('ETH delay after IOTX [ms]')
    ax2.grid()

    show()


if __name__ == '__main__':
    parser = argparse.ArgumentParser(description=usage,
                                     formatter_class=argparse.RawDescriptionHelpFormatter)
    parser.add_argument("filename", help="NMEA GGA/ZDA/DBG filename")
    parser.add_argument("--hours", help="plot using GPS hours as timescale instead of seconds",
                        action="store_true")
    args = parser.parse_args()
    t_zda, t_eth, d = parse_data( args.filename )
    plot_data( t_zda, t_eth, d, args.hours )