#!/usr/bin/env python

#######################################################
# Copyright Trimble Inc 2021
# Purpose: For plotting out useful plots for record 35:19 
#          from a T0x file with minimalist user input
#
# Plots:
# - Number of tracked signals for each satellite system over time
# - Log10 weighed average of SNR values for each satellite system 
#
# Usage example:
# - plot_rec35-19_Meas.py "Path_To_Your_T0x_File"
#
#
# Contributor(s): Joshua_McLean@Trimble.com, 
#######################################################

import argparse
from math import nan
import os
import sys

import glob
import matplotlib
from numpy.lib.function_base import append
import mutils as m
import numpy as np

import mutils.RTConst as RTConst

from matplotlib.font_manager import (FontProperties)

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

parser = argparse.ArgumentParser(description='Generate useful .png plots for record 35:2 for multiple T0x files')
parser.add_argument('filename', help='T0x filename')
parser.add_argument('--fig', help='Output plots in Matlab fig instead of PNG', required=False, action='store_true')
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 prefix names to plot out. Others will be skipped. example --filter=snr_avg_qzss;snr_avg_sbas', 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)
parser.add_argument('--png', help='Optional Prefix for PNG file names, e.g. --png TestPlot',required=False)

args = parser.parse_args()

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

if (args.png):
  pngPrefix = args.png
else:
  pngPrefix = args.filename

plotSubLabel = args.label

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

svTypeList = (
    (RTConst.RT_SatType_GPS, "GPS"),
    (RTConst.RT_SatType_GLONASS, "GLONASS"),
    (RTConst.RT_SatType_GALILEO, "Galileo"),
    (RTConst.RT_SatType_BEIDOU_B1GEOPhs, "BeiDou"),
    (RTConst.RT_SatType_QZSS, "QZSS"),
    (RTConst.RT_SatType_SBAS, "SBAS"),
    (RTConst.RT_SatType_IRNSS, "IRNSS"))


if (args.opt != None):
    (ref,k)=m.vd2arr(args.filename,rec='-d27 --translate_rec35_sub19_to_rec27 --dec=1000 '+ args.opt)
else:
    (ref,k)=m.vd2arr(args.filename,rec='-d27 --translate_rec35_sub19_to_rec27 --dec=1000')

#(ref,k)=m.vd2arr(filename,rec='-d35:19 --dec=1000')


seconds       = ref[:,k.TIME]
svIDs         = ref[:,k.SV]
svTypes       = ref[:,k.SAT_TYPE]   # https://wiki.eng.trimble.com/index.php/SV_System
svFreqBands   = ref[:,k.FREQ]       # https://wiki.eng.trimble.com/index.php/SV_Frequency_Band
svTrackTypes  = ref[:,k.TRACK]      # https://wiki.eng.trimble.com/index.php/SV_Signal
svCNOs        = ref[:,k.CNO]
svFlags       = ref[:,k.SV_FLAGS]   # https://wiki.eng.trimble.com/index.php/Dat_file_format#Measurement_header
svMeasFlags   = ref[:,k.MEAS1]
svElevs       = ref[:,k.EL]

uniqueSvTypeTrackCombos = []

#sv type + track + band id > array of SNRs 
svTypeTrackCombined = {}

#plot the number of unique signals tracked over time for each constellation
# i.e. GPS L1 + GPS L2 counts as two

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

totalSignals = {} #total number of tracked signals for each gps second

#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(pngSuffix):
    
    if (args.fig == True):
        m.show()
        return

    pngName = '%s_rec35-19_%s.png' % (pngPrefix, pngSuffix)

    # 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 == pngPrefix):
                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)


for (svTypeId, svTypeName) in svTypeList:

    x = [] # delta second
    y = [] # number of tracked signals for given time

    numSignalsTracked = 0
    prevSecond = None
    signals = []
    start = seconds[0]

    for i in np.arange(0, len(seconds) - 1):
        
        second = int(seconds[i])
        svType = int(svTypes[i])
        svFlag = int(svFlags[i])
        svMeasFlag = int(svMeasFlags[i])        
        band   = int(svFreqBands[i])
        track  = int(svTrackTypes[i])
        svID   = int(svIDs[i])
        svNO   = float(svCNOs[i])
        svElev = int(svElevs[i])
 
        #check for rollover
        if second < start:
          second += (60 * 60 * 24 * 7) #604800

        if (prevSecond is None):
            prevSecond = second

        #append data for previous seconds
        #then reset
        if (second != prevSecond):
            
            if (numSignalsTracked == 0):
                print("0 tracked?")

            x.append(prevSecond)
            y.append(numSignalsTracked)

            if (prevSecond not in totalSignals):
                totalSignals[prevSecond] = 0

            #increment number of signals tracked
            totalSignals[prevSecond] += numSignalsTracked
            
            numSignalsTracked = 0
            signals = []
            prevSecond = second

        #not the type we want
        if (svType != svTypeId):
            continue

        # b4(0x10): set = satellite unhealthy / reset = satellite healthy
        if ((svFlag & 0x10) != 0):
            continue #unhealthy

        # b5 (0x20): set = Data sync/Lock Point Known
        if ((svMeasFlag & 0x20) == 0):
           continue #not known

        svTypeAndTrackComboId = (svType << 16) + (band << 8) + track
        uniqueSVAndTrackComboId = (svID << 16) + (band << 8) + track

        if (svTypeAndTrackComboId not in uniqueSvTypeTrackCombos):
            uniqueSvTypeTrackCombos.append(svTypeAndTrackComboId)

        if (second not in svTypeTrackCombined):
            svTypeTrackCombined[second] = {}

        if (svTypeAndTrackComboId not in svTypeTrackCombined[second]):
            svTypeTrackCombined[second][svTypeAndTrackComboId] = []

        # only for > 20 elev and > 20 for more stable SV CNos
        if (svElev > 20 and svNO > 20):            
            svTypeTrackCombined[second][svTypeAndTrackComboId].append(pow(10, svNO / 10.0 ))  # for Log10 based averaging

        if (uniqueSVAndTrackComboId not in signals):
            signals.append(uniqueSVAndTrackComboId)
            numSignalsTracked += 1
        else:
            print("Duplicate data (%d, %d, %d) found for same gps second" % (svID, band, track))

    m.plot( x, y, "-", markersize=2, label=svTypeName)

m.title("Rec 35:19 Signals Tracked\n[" + plotSubLabel + "]")
m.xlabel('GPS Second')
m.ylabel('Number of signals tracked')
m.grid(True)
m.legend(loc='upper right')
m.tight_layout()
outputOrVisiualizeCurrentFig('sv_signals_tracked')

def plot_TotalSignalsTracked():

    gpsSeconds = []
    numTracked = []

    gpsSecondFirst = min(totalSignals.keys())
    gpsSecondLast = max(totalSignals.keys())

    for gpsSecond in range(gpsSecondFirst, gpsSecondLast, 1):

        gpsSeconds.append(gpsSecond)    

        #gps second doesn't exist somehow. just add 0
        if (gpsSecond not in totalSignals):
            numTracked.append(0)
            continue

        numTracked.append(totalSignals[gpsSecond])
    
    fig = m.figure()
    ax = fig.add_subplot(1, 1, 1)
    m.plot( gpsSeconds, numTracked, "-", markersize=2, label=svTypeName)
    m.title("Rec 35:19 Total Signals Tracked\n[" + plotSubLabel + "]")
    m.xlabel('GPS Second')
    m.ylabel('Number of signals tracked')
    m.grid(True)
    m.tight_layout()
    
    outputOrVisiualizeCurrentFig('sv_signals_tracked_total')


def plot_Log10SNRAveraging_times(targetSVType):
  

    gpsSeconds = []
    trackData = {}

    gpsSecondFirst = min(svTypeTrackCombined.keys())
    gpsSecondLast = max(svTypeTrackCombined.keys())

    for id in uniqueSvTypeTrackCombos:
        svType = (id >> 16) & 0xFF

        if (svType != targetSVType):
            continue
    
        trackId = id & 0xFFFF

        if (trackId not in trackData):
            trackData[trackId] = []   
   

    #nothing to plot out
    if len(trackData) == 0:
        return

    for gpsSecond in range(gpsSecondFirst, gpsSecondLast, 1):

        gpsSeconds.append(gpsSecond)    

        for trackId in trackData:

            #gps second doesn't exist somehow. just add nan
            if (gpsSecond not in svTypeTrackCombined):
                trackData[trackId].append(nan)
                continue

            
            found = False

            for id in svTypeTrackCombined[gpsSecond]:
                svType = (id >> 16) & 0xFF

                #not the sv type we want? skip
                if (svType != targetSVType):
                    continue

                trackIdMask = id & 0xFFFF

                #not the track id we want? skip
                if (trackId != trackIdMask):
                    continue

                snrs = svTypeTrackCombined[gpsSecond][id]
                avg = np.average(snrs)
                value = np.log10(avg) * 10.0

                trackData[trackId].append(value)
                found = True
                break


            #data was not found. append nan    
            if (found == False):
                trackData[trackId].append(nan)

    sattype = m.get_sat_type(targetSVType)

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

    m.title("Rec 35:19 Avg SNR - %s \n[%s]" % (sattype.sysstr, plotSubLabel))
    m.xlabel('GPS Second')
    m.ylabel('SNR (Log10 Avg, 20 elev mask)') 

    for trackId in trackData:
       
        band = (trackId >> 8) & 0xFF
        track = trackId & 0xFF
        
        subtype = m.get_sub_type(targetSVType,band,track)  

        avg = np.nanmean(trackData[trackId])

        m.plot( gpsSeconds, trackData[trackId], "-", markersize=1, label="%s [%.1f]" % (subtype.fullstr, avg))
    
    fig.gca().set_ylim(30, 60)

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

    m.grid(True)
    m.legend(loc='upper right')
    m.tight_layout()

    outputOrVisiualizeCurrentFig('snr_avg_%s' % sattype.sysstr.lower())
    
        
plot_TotalSignalsTracked()
plot_Log10SNRAveraging_times(RTConst.RT_SatType_GPS)
plot_Log10SNRAveraging_times(RTConst.RT_SatType_GLONASS)
plot_Log10SNRAveraging_times(RTConst.RT_SatType_GALILEO)
plot_Log10SNRAveraging_times(RTConst.RT_SatType_BEIDOU_B1GEOPhs)
plot_Log10SNRAveraging_times(RTConst.RT_SatType_QZSS)
plot_Log10SNRAveraging_times(RTConst.RT_SatType_IRNSS)
plot_Log10SNRAveraging_times(RTConst.RT_SatType_SBAS)

#m.title("Rec 35:19 Signals Tracked\n[" + os.path.basename(args.filename) + "]")
#m.xlabel('GPS Second')
#m.ylabel('Number of signals tracked')
#m.grid(True)
#m.legend(loc='upper right')
#m.tight_layout()
#
#if (args.fig == True):
#    m.show()
#else:
#    pngName = args.filename + '_rec35-19_sv_signals_tracked.png'
#    m.savefig(pngName)
#    print('saved %s' % pngName)