#!/usr/bin/env python

#######################################################
# Copyright Trimble Inc 2021
# Purpose: For plotting out various plots for record 98 from mutliple T04 files
#          Usually used for reset / reboot tests that results in multiple T0x files
# Plots:
# - All time to first record 98 data from first position timestamp
#
# Contributor(s): Joshua_McLean@Trimble.com, 
#######################################################

import argparse
import os
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 .png plots for record 98 (delta times per type)')
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

# parse record 98 and build the x and y values where
# x values is GPS time and y is delta time
# compared to previous record of same time
def build_xy_values_from_rec98(d, recType):
    xValues = [] # gps time
    yValues = [] 

    i = 0 # skip first

    prevValue = 0
    while i < len(d): 

        timeMs = d[i][2]
        time = timeMs / 1000.0

        if d[i][0] == recType:
            if prevValue != 0:            
                diff = d[i][2] - prevValue   
                if (diff >= 0):         
                    xValues.append(diff)             
                    yValues.append(time) 
            prevValue = d[i][2]

        i += 1

    return xValues, yValues

# plot out times to first record 98 since first fix
def plot_time_to_first_rec98( times, recType, label, plot_label ):

    if len(times) == 0:
        return

    indices = []

    for i in range(0, len(times), 1):
        indices.append(i)

    print("size = %d" % len(indices))

    fig = m.figure()    
    ax = fig.add_subplot(1, 1, 1)
    m.plot( indices, times, marker='x', linestyle='dashed', linewidth=0.5, label=label )


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

    #ax.yaxis.set_minor_locator(MultipleLocator(1))
    #ax.xaxis.set_major_locator(MultipleLocator(1))
    ax.xaxis.set_major_formatter(FormatStrFormatter("%d"))
    
    yMin, yMax = fig.gca().get_ylim()
    fig.gca().set_ylim(0, yMax + 5 )


    m.annotate('Count: %d\nMean: %ds\nMax: %ds\nMin: %ds' % (len(times), np.average(times), np.max(times), np.min(times)), xy=(0.95, 0.05), 
        xycoords='axes fraction', horizontalalignment='right', 
        verticalalignment='bottom')

    if plot_label == None:
        m.title("Rec 98 Time to first " + label + " since first pos fix")
    else:
        m.title("Rec 98 Time to first " + label + " since first pos fix\n[" + plot_label + "]")
    
    m.xlabel('Index')
    m.ylabel('Time')

    m.grid(True)
    m.legend(loc='best')
    m.tight_layout()

    if (args.fig == True):
        m.show()
    else:
        pngName = 'rec98_multi_plot_TTF_' + str(label) + '.png'
        m.savefig(pngName)
        print('saved %s' % pngName)

    return

times128 = []
times129 = []
times130 = []
times131 = []

for filename in sorted(glob.glob(t0x_file_pat)):
    #print("Processing %s" % filename)
    (ref,k)=m.vd2arr(filename,rec='-d35:2')
    d=m.cmd2arr("VIEWDAT.exe -d98 -mb \"" + filename + "\"")

    firstTime = ref[:,k.TIME][0] #time of first position fix

    xValues, yValues = build_xy_values_from_rec98(d, 128) #cmrx
    if len(yValues)  > 0:
        times128.append(yValues[0] - firstTime)

    xValues, yValues = build_xy_values_from_rec98(d, 129) #rtcm
    if len(yValues)  > 0:
        times129.append(yValues[0] - firstTime)

    xValues, yValues = build_xy_values_from_rec98(d, 130) #rtcm32 
    if len(yValues)  > 0:
        times130.append(yValues[0] - firstTime)

    xValues, yValues = build_xy_values_from_rec98(d, 131) #rtx / xfill
    if len(yValues) > 0:
        times131.append(yValues[0] - firstTime)


plot_time_to_first_rec98(times128, 128, "Cmr (128)", args.label)
plot_time_to_first_rec98(times129, 129, "Rtcm (129)", args.label)
plot_time_to_first_rec98(times130, 130, "Rtcm3 (130)", args.label)
plot_time_to_first_rec98(times131, 131, "CmrxRaw (131)", args.label)