#!/usr/bin/env python

#######################################################
# Copyright Trimble Inc 2021
# Purpose: For plotting out useful plots for record 35:2
# 
# Plots:
# - Correction age
# - DOP values
# - Position fix types
# - Horizontal precision
# - Vertical precision
# - Total satellites used and tracked
# - Satellite tracked for each constellation
# - Satellites used in positioning solution
#
# Contributor(s): Joshua_McLean@Trimble.com, 
#######################################################

import argparse
import os
import sys

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 (Position and Attitude)')
parser.add_argument('filename', help='T0x filename')
parser.add_argument('--fig', help='Output plots in Matlab fig instead of PNG', action='store_true')
parser.add_argument('--prec_max_y', help='Max y value for precision related plots', required=False, default=None, type=float)
parser.add_argument('--age_max_y', help='Max y value for the correction age plot', required=False, default=None, type=int)
parser.add_argument('--sv_max_y', help='Max y value for SV related plots', required=False, default=None, type=int)
parser.add_argument('--sv_total_max_y', help='Max y value for Total SV related plots', required=False, default=None, type=int)
parser.add_argument('--pos_filter', help='Position types to filter in', required=False, default=None, nargs='+')
parser.add_argument('--opt', help='Extra args to pass on to Viewdat.exe. example --opt=\"-s34000 and -e35000\"', required=False, default=None, type=str)
parser.add_argument('--filter', help='Inclusive list of plot perfix names to plot out. Others will be skipped. example --filter=prec_horz;prec_vert', required=False, default=None, type=str)
parser.add_argument('--label', help='Plot sub label override. If none is specified, the file name is used', required=False, default=None, type=str)
args = parser.parse_args()

# Allow running headless from the command line
if (args.fig == False):
  matplotlib.use("agg")
else:
  matplotlib.use('Qt5Agg') #pip3 install PyQt5

if (args.opt != None):
    (ref,k)=m.vd2arr(args.filename,rec='-d35:2',opt=[args.opt])
else:
    (ref,k)=m.vd2arr(args.filename,rec='-d35:2')

pos_filter = []
if (args.pos_filter != None):
    print('Filtering in these position types: ' + ' '.join(args.pos_filter))
    for pos in args.pos_filter:
        pos_filter.append(int(pos));


plotSubLabel = args.label

if (plotSubLabel == None):
    plotSubLabel = os.path.basename(args.filename)

svPlotYMax = args.sv_max_y
svTotalPlotYMax = args.sv_total_max_y
precYMax = args.prec_max_y
corrAgeYMax = args.age_max_y

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]    
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]      


horzPrec = np.sqrt(ref[:,k.SigE]**2 + ref[:,k.SigN]**2)
vertPrec = np.abs(ref[:,k.SigU])


firstGpsSecond = -1
secondsAdj = []
i = 0
while i < len(seconds): 
  
  second = seconds[i]
  if (second > 0 and firstGpsSecond == -1):
      firstGpsSecond = second

  if (second < firstGpsSecond):
      second += 604800 #(60 * 60 * 24 * 7)

  secondsAdj.append(second)
  i += 1


#Helper fuction that does three things:
# - if args.fig is True, show the plots using Matlab UI
# - if args.inc contains a list, check if this plot included in the filter list
# - output the current fig as png
def outputOrVisiualizeCurrentFig(prefix):
    
    if (args.fig == True):
        fig.tight_layout()
        m.show(block=False)
        return

    pngName = '%s_rec35-2_%s.png' % (args.filename, prefix)

    # only filter out plots if needed
    if (args.filter != None):

        found = False
        filterList = args.filter.split(';')

        for name in filterList:

            # Does this png plot perfix contains the filtered string? 
            # If so plot it out. 
            if (name == prefix):
                found = True
                break

        #this plot not found in the filter list. skip this plot
        if (found == False):
            print('skipped %s' % pngName)
            return

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


# subroutine to plot out fix type for 35:2 fix type plot
# ONLY if values exist for it
def plot_xy_values_for_fix_type(seconds, yValues, fixType, label):
   
    if ((len(pos_filter) < 0) and (fixType not in pos_filter)): 
        return    

    if len(yValues) != len(seconds):
        print("ERROR: len(values) != len(seconds)")
        return
   
    secondsFiltered = []
    yValuesFiltered = [] 

    i = 0

    #filter out data by fix type
    while i < len(yValues): 
        if yValues[i] == fixType:
            secondsFiltered.append(seconds[i])
            yValuesFiltered.append(yValues[i])
        i += 1

    percentage = (float(len(secondsFiltered)) / float(len(yValues))) * 100.0

    #only plot out if we have any values for this type
    if len(yValuesFiltered) > 0:
        m.plot( secondsFiltered, yValuesFiltered, ".", markersize=2, label=str(fixType) + (" (%s) (%0.2f%%)" % (label, percentage)))

    return


# subroutine to plot y values but have its coloring / labels based on fix type
# ONLY if values exist for it
def plot_xy_values_using_fix_type_labels(seconds, yValues, fixTypes, fixType, label):
   
    if ((len(pos_filter) < 0) and (fixType not in pos_filter)): 
        return    

    if len(yValues) != len(seconds):
        print("ERROR: len(values) != len(seconds)")
        return

    if len(fixTypes) != len(seconds):
        print("ERROR: len(fixTypes) != len(seconds)")
        return
   
    secondsFiltered = []
    yValuesFiltered = [] 

    i = 0

    #filter out data by fix type
    while i < len(fixTypes): 
        if fixTypes[i] == fixType:
            secondsFiltered.append(seconds[i])
            yValuesFiltered.append(yValues[i])
        i += 1

    #only plot out if we have any values for this type
    if len(yValuesFiltered) > 0:
        m.plot( secondsFiltered, yValuesFiltered, ".", markersize=2, label=str(fixType) + " (" + label + ")")

    return

##########################################
## Number satellite used / tracked    
##########################################
fig = m.figure()
ax = fig.add_subplot(1, 1, 1)
m.plot( secondsAdj, svTracked, ".", markersize=2, label="Tracked")
m.plot( secondsAdj, svUsed, ".", markersize=2, label="Used")
m.title("Rec 35:2 Total Satellites Used vs Tracked\n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
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(-1, svTotalPlotYMax)

outputOrVisiualizeCurrentFig('sv_total')

##########################################
## Number satellite tracked
##########################################
fig = m.figure()
ax = fig.add_subplot(1, 1, 1)
m.plot( secondsAdj, gpsTrk, label="GPS")
m.plot( secondsAdj, gloTrk, label="GLO")
m.plot( secondsAdj, galTrk, label="GAL")
m.plot( secondsAdj, bdsTrk, label="BDS")
m.plot( secondsAdj, qzsTrk, label="QZS")
m.plot( secondsAdj, sbaTrk, label="SBA")
m.plot( secondsAdj, irnTrk, label="IRN")
m.plot( secondsAdj, xpsTrk, label="XPS")
m.title("Rec 35:2 Satellites Tracked \n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
m.ylabel('Number of satellites tracked')
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(-1, svPlotYMax)
yMinSV, yMaxSV = fig.gca().get_ylim()

outputOrVisiualizeCurrentFig('sv_tracked')


##########################################
## Number satellite used
##########################################
fig = m.figure()
ax = fig.add_subplot(1, 1, 1)
m.plot( secondsAdj, gpsUsed, label="GPS")
m.plot( secondsAdj, gloUsed, label="GLO")
m.plot( secondsAdj, galUsed, label="GAL")
m.plot( secondsAdj, bdsUsed, label="BDS")
m.plot( secondsAdj, qzsUsed, label="QZS")
m.plot( secondsAdj, sbaUsed, label="SBA")
m.plot( secondsAdj, irnUsed, label="IRN")
m.plot( secondsAdj, xpsUsed, label="XPS")
m.title("Rec 35:2 Satellites Used \n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
m.ylabel('Number of satellites used')
m.grid(True)
m.legend(loc='best')
m.tight_layout()
fig.gca().set_ylim(yMinSV, yMaxSV)

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))

outputOrVisiualizeCurrentFig('sv_used')



##########################################
## DOP   
##########################################
fig = m.figure()
m.plot( secondsAdj, pdop, ".", markersize=2, label="PDOP")
m.plot( secondsAdj, hdop, ".", markersize=2, label="HDOP")
m.plot( secondsAdj, vdop, ".", markersize=2, label="VDOP")
m.title("Rec 35:2 DOP\n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
m.ylabel('DOP')
m.grid(True)
m.legend(loc='best')
m.tight_layout()

outputOrVisiualizeCurrentFig('dop')


##########################################
## Horizontal Precision
##########################################
fig = m.figure()
ax = fig.add_subplot(1, 1, 1)
#loop over each fix type
for k in range(0, len(m.PosTypeStrConst)):
    plot_xy_values_using_fix_type_labels(secondsAdj, horzPrec, fixType, k, m.PosTypeStrConst[k] )

m.title("Rec 35:2 Horz Prec\n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
m.ylabel('Horizontal Precision (m)')
m.grid(True)
m.legend(loc='best')
m.tight_layout()

ax.yaxis.grid(True, which='minor', linestyle=':', color='lightgrey')

fig.gca().set_ylim(0, precYMax)

outputOrVisiualizeCurrentFig('prec_horz')


##########################################
## Vertical Precision
##########################################
fig = m.figure()
ax = fig.add_subplot(1, 1, 1)
#loop over each fix type
for k in range(0, len(m.PosTypeStrConst)):
    plot_xy_values_using_fix_type_labels(secondsAdj, vertPrec, fixType, k, m.PosTypeStrConst[k] )

m.title("Rec 35:2 Vert Prec\n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
m.ylabel('Vertical Precision (m)')
m.grid(True)
m.legend(loc='best')
m.tight_layout()

ax.yaxis.grid(True, which='minor', linestyle=':', color='lightgrey')
fig.gca().set_ylim(0, precYMax)

outputOrVisiualizeCurrentFig('prec_vert')


##########################################
## Correction age
##########################################
fig = m.figure()

ax = fig.add_subplot(1, 1, 1)
#loop over each fix type
for k in range(0, len(m.PosTypeStrConst)):
    plot_xy_values_using_fix_type_labels(secondsAdj, corrAge, fixType, k, m.PosTypeStrConst[k] )

m.title("Rec 35:2 Correction age (s)\n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
m.ylabel('Correction age (s)')
m.grid(True)
m.legend(loc='best')
m.tight_layout()

fig.gca().set_ylim(-1, corrAgeYMax)
outputOrVisiualizeCurrentFig('corr_age')

   
##########################################
## Position fix type   
##########################################  
fig = m.figure() 
ax = fig.add_subplot(1, 1, 1)
#loop over each fix type
for k in range(0, len(m.PosTypeStrConst)):
    plot_xy_values_for_fix_type(secondsAdj, fixType, k, m.PosTypeStrConst[k] )

#start, end = ax.get_ylim()
#major_ticks = np.arange(start, end, 1)
#ax.set_yticks(major_ticks)

m.title("Rec 35:2 Position Fix Type\n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
m.ylabel('Fix type')
m.grid(True)
ax.legend(loc='best')
m.tight_layout()


outputOrVisiualizeCurrentFig('fix_type')


if (args.fig == True):
  print("Press Enter in the console, or close all figures, to exit process.")
  try:
      input()
  except KeyboardInterrupt:
      pass