#!/usr/bin/env python

#######################################################
# Copyright Trimble Inc 2021
# Purpose: For plotting out useful plots for record 35:2 from multiple T0x files
#          Usually used for reset / reboot tests that results in multiple T0x files
#
# Plots:
# - Average satellites tracked over time (total) since first position
# - Average satellites tracked per satellite constellation since first position + 120s (zoom)
# - Gaps between each T04 file based on valid GPS timestamps only
#
# Usage examples:
# - plot_rec35-2_Pos_multi.py "*.T04"
# - plot_rec35-2_Pos_multi.py "*.T04" --label "Sapphire 1.06.16"
#
#
# Contributor(s): Joshua_McLean@Trimble.com, 
#######################################################

import argparse
import os
from re import A
import sys

import glob
import matplotlib
import mutils as m
import numpy as np

from matplotlib.ticker import (MultipleLocator, FormatStrFormatter,
                               AutoMinorLocator)

#from mutils import *

parser = argparse.ArgumentParser(description='Generate useful .png plots for record 35:2 for multiple T0x files')
parser.add_argument('pattern', help='T0x file search pattern, i.e. "*.T04"')
parser.add_argument('--label', help='Label to include in plot(s)', default=None, type=str)
parser.add_argument('--fig', help='Output plots in Matlab fig instead of PNG', action='store_true')
args = parser.parse_args()

# Allow running headless from the command line
if (args.fig == False):
    matplotlib.use("agg")

t0x_file_pat = args.pattern

deltaTimes = []
gapDurations = []               # to calculate mean gap duration between each files based on 35:2 (from last to first)
svTrackedCombined = []
svUsedCombined = []
gpsTrkCombined = []
sbaTrkCombined = []
gloTrkCombined = []
galTrkCombined = []
qzsTrkCombined = []
bdsTrkCombined = []
irnTrkCombined = []
xpsTrkCombined = []
gpsUsedCombined = []
sbaUsedCombined = []
gloUsedCombined = []
galUsedCombined = []
qzsUsedCombined = []
bdsUsedCombined = []
irnUsedCombined = []
xpsUsedCombined = []

fileNum = 0                     # file counter
maxGapTime = 150                # max gap time between T04 files (to detect missed reboot)
lastFileEndGPSTime = -1.0       # to record last GPS time for previous file (to calc gap duration)

gapDurations.append(np.nan)        #we don't have the gap for first file

for filename in sorted(glob.glob(t0x_file_pat)):
    (ref,k)=m.vd2arr(filename,rec='-d35:2 --dec=1000')

    firstTime = 0
    gapDurationSec = -1.0
    fileNum += 1

    weeks      = ref[:,k.WEEK]
    seconds    = ref[:,k.TIME]
    svTracked  = ref[:,k.NTRK]
    pdop       = ref[:,k.PDOP]
    hdop       = ref[:,k.HDOP]
    vdop       = ref[:,k.VDOP]
    svUsed     = ref[:,k.NUSED] 
    corrAge    = ref[:,k.corrAge]  
    fixModes   = ref[:,k.FIXMODE]    
    fixType    = ref[:,k.FIXTYPE]
    gpsUsed    = ref[:,k.GPSUsd]      
    sbaUsed    = ref[:,k.SBASUsd]     
    gloUsed    = ref[:,k.GLOUsd]     
    galUsed    = ref[:,k.GALUsd]      
    qzsUsed    = ref[:,k.QZSSUsd]     
    bdsUsed    = ref[:,k.BDSUsd]      
    irnUsed    = ref[:,k.IRNSSUsd]    
    xpsUsed    = ref[:,k.XPSUsd]      
    gpsTrk     = ref[:,k.GPSTrk]      
    sbaTrk     = ref[:,k.SBASTrk]     
    gloTrk     = ref[:,k.GLOTrk]      
    galTrk     = ref[:,k.GALTrk]      
    qzsTrk     = ref[:,k.QZSSTrk]     
    bdsTrk     = ref[:,k.BDSTrk]      
    irnTrk     = ref[:,k.IRNSSTrk]    
    xpsTrk     = ref[:,k.XPSTrk]      

   

    for index in range(0, len(seconds), 1):
        

        if (index >= len(deltaTimes)):
            deltaTimes.append(index)
            svTrackedCombined.append([])
            svUsedCombined.append([])
            gpsTrkCombined.append([])
            sbaTrkCombined.append([])
            gloTrkCombined.append([])
            galTrkCombined.append([])
            qzsTrkCombined.append([])
            bdsTrkCombined.append([])
            irnTrkCombined.append([])
            gpsUsedCombined.append([])
            sbaUsedCombined.append([])
            gloUsedCombined.append([])
            galUsedCombined.append([])
            qzsUsedCombined.append([])
            bdsUsedCombined.append([])
            irnUsedCombined.append([])

        medianWeek = np.median(weeks)

        week = weeks[index]
        second = seconds[index]
        fixMode = fixModes[index]

        #ignore positions that doesn't have a fix yet
        if (fixMode < 5):
            continue

        #ignore bad week numbers.
        #Due to a Sapphire bug from earlier versions..
        if (np.abs(week - medianWeek) > 10 ):
            continue

        time = week * 60 * 60 * 24 * 7 + second

        if (firstTime == 0):
            firstTime = time
        
        deltaTime = time - firstTime        #delta time since first position time
           
        if ((gapDurationSec < 0)              #gap duration not set yet for this file?
            and (fixMode >= 5)                  #3D solution or better  
            and (lastFileEndGPSTime > 0.0)      #has gps time for last file?
            and (fileNum > 1)):                 #is not the first file?
                gapDurationSec = time - lastFileEndGPSTime

                if (gapDurationSec > 200):  
                    print('WARN: Ignoring gap time of %d' % gapDurationSec)
                    gapDurations.append(np.nan) #treat as a outlier
                else:
                    gapDurations.append(gapDurationSec) 
            
        #store latest / last gps second for next file
        if (fixMode >= 5):
            lastFileEndGPSTime = time

        svTrackedCombined[index].append(svTracked[index])
        gpsTrkCombined[index].append(gpsTrk[index])
        sbaTrkCombined[index].append(sbaTrk[index])
        gloTrkCombined[index].append(gloTrk[index])
        galTrkCombined[index].append(galTrk[index])
        qzsTrkCombined[index].append(qzsTrk[index])
        bdsTrkCombined[index].append(bdsTrk[index])
        irnTrkCombined[index].append(irnTrk[index])
        gpsUsedCombined[index].append(gpsTrk[index])
        sbaUsedCombined[index].append(sbaTrk[index])
        gloUsedCombined[index].append(gloTrk[index])
        galUsedCombined[index].append(galTrk[index])
        qzsUsedCombined[index].append(qzsTrk[index])
        bdsUsedCombined[index].append(bdsTrk[index])
        irnUsedCombined[index].append(irnTrk[index])

svTrackedCombinedAvg = []
gpsTrkCombinedAvg = []
sbaTrkCombinedAvg = []
gloTrkCombinedAvg = []
galTrkCombinedAvg = []
qzsTrkCombinedAvg = []
bdsTrkCombinedAvg = []
irnTrkCombinedAvg = []

svUsedCombinedAvg = []
gpsUsedCombinedAvg = []
sbaUsedCombinedAvg = []
gloUsedCombinedAvg = []
galUsedCombinedAvg = []
qzsUsedCombinedAvg = []
bdsUsedCombinedAvg = []
irnUsedCombinedAvg = []

for i in range(0, len(deltaTimes), 1):
    svTrackedCombinedAvg.append(np.average(svTrackedCombined[i]))
    gpsTrkCombinedAvg.append(np.average(gpsTrkCombined[i]))
    sbaTrkCombinedAvg.append(np.average(sbaTrkCombined[i]))
    gloTrkCombinedAvg.append(np.average(gloTrkCombined[i]))
    galTrkCombinedAvg.append(np.average(galTrkCombined[i]))
    qzsTrkCombinedAvg.append(np.average(qzsTrkCombined[i]))
    bdsTrkCombinedAvg.append(np.average(bdsTrkCombined[i]))
    irnTrkCombinedAvg.append(np.average(irnTrkCombined[i]))

    svUsedCombinedAvg.append(np.average(svUsedCombined[i]))
    gpsUsedCombinedAvg.append(np.average(gpsUsedCombined[i]))
    sbaUsedCombinedAvg.append(np.average(sbaUsedCombined[i]))
    gloUsedCombinedAvg.append(np.average(gloUsedCombined[i]))
    galUsedCombinedAvg.append(np.average(galUsedCombined[i]))
    qzsUsedCombinedAvg.append(np.average(qzsUsedCombined[i]))
    bdsUsedCombinedAvg.append(np.average(bdsUsedCombined[i]))
    irnUsedCombinedAvg.append(np.average(irnUsedCombined[i]))


##########################################
## Number satellite used / tracked    
##########################################
fig = m.figure()
ax = fig.add_subplot(1, 1, 1)
m.plot( deltaTimes, svTrackedCombinedAvg, ".", markersize=2, label="Tracked (Avg)")
m.plot( deltaTimes, svUsedCombinedAvg, ".", markersize=2, label="Used (Avg)")

title = "Rec 35:2 Average Satellites Tracked (Combined)"
if (args.label is not None):
    title += "\n[%s]" % args.label

m.title(title)
m.xlabel('Time since first position (sec)')
m.ylabel('Number of satellites')
m.grid(True)
m.legend(loc='best')
m.tight_layout()

ax.yaxis.grid(True, which='minor', linestyle=':', color='lightgrey')
ax.yaxis.set_major_locator(MultipleLocator(5))
ax.yaxis.set_major_formatter(FormatStrFormatter("%d"))
ax.yaxis.set_minor_locator(MultipleLocator(1))
fig.gca().set_ylim(10, 60) 

if (args.fig == True):
    m.show()
else:

    ax.set_xlim(0)
    pngName = 'rec35-2_sv_total_avg.png'
    if (args.label is not None):
        pngName = "%s_%s" % (args.label, pngName)

    m.savefig(pngName)
    print('saved %s' % pngName)
    

fig = m.figure()
ax = fig.add_subplot(1, 1, 1)
m.plot( deltaTimes, gpsTrkCombinedAvg, "-", markersize=2, label="GPS")
m.plot( deltaTimes, gloTrkCombinedAvg, "-", markersize=2, label="GLO")
m.plot( deltaTimes, galTrkCombinedAvg, "-", markersize=2, label="GAL")
m.plot( deltaTimes, bdsTrkCombinedAvg, "-", markersize=2, label="BDS")
m.plot( deltaTimes, qzsTrkCombinedAvg, "-", markersize=2, label="QZS")
m.plot( deltaTimes, sbaTrkCombinedAvg, "-", markersize=2, label="SBA")
m.plot( deltaTimes, irnTrkCombinedAvg, "-", markersize=2, label="IRN")

title = "Rec 35:2 Average Satellites Tracked"
if (args.label is not None):
    title += "\n[%s]" % args.label

m.title(title)
m.xlabel('Time since first position (sec)')
m.ylabel('Number of satellites')
m.grid(True)
m.legend(loc='lower right')
m.tight_layout()

ax.set_ylim(-1, 20)

ax.yaxis.grid(True, which='minor', linestyle=':', color='lightgrey')
ax.yaxis.set_major_locator(MultipleLocator(5))
ax.yaxis.set_major_formatter(FormatStrFormatter("%d"))
ax.yaxis.set_minor_locator(MultipleLocator(1))

if (args.fig == True):
    m.show()
else:

    ax.set_xlim(0)
    pngName = 'rec35-2_sv_avg.png'
    if (args.label is not None):
        pngName = "%s_%s" % (args.label, pngName)

    m.savefig(pngName)
    print('saved %s' % pngName)

    ax.set_xlim(0, 120)
    pngName = 'rec35-2_sv_avg_120s.png'
    if (args.label is not None):
        pngName = "%s_%s" % (args.label, pngName)

    m.savefig(pngName)
    print('saved %s' % pngName)


# plot gap times (time of last 35:2 to first valid 35:2 position)

fig = m.figure()
fig.add_subplot(1, 1, 1)

title = "Rec 35:2 Gap Times"

if (args.label is not None):
    title += "\n[%s]" % args.label

m.title(title)
m.xlabel('Reset interval')
m.ylabel('Gap time (minutes)')

x = np.arange(1, fileNum + 1, 1)
m.plot( x, gapDurations, "X", markersize=4, linestyle='dotted', linewidth=1, label=None)
m.grid(True)
m.tight_layout()

pngName = 'rec35-2_gap_times.png'
if (args.label is not None):
        pngName = "%s_%s" % (args.label, pngName)

m.savefig(pngName)
m.close(fig)

print("Min gap time: %.1fs" % np.nanmin(gapDurations))
print("Max gap time: %.1fs" % np.nanmax(gapDurations))
print("Median gap time: %.1fs" % np.nanmedian(gapDurations))
print("Mean gap time: %.1fs" % np.nanmean(gapDurations))
print("Std. dev. gap time: %.1fs" % np.nanstd(gapDurations))
