# Copyright Trimble Inc 2021

#
# You'll need something like this to setup the python path:
# export PYTHONPATH=$HOME/GPSTools/pythonTools/:$HOME/GPSTools/RFPlaybackRegression/:$HOME/GPSTools/pythonTools/obstructions/
#

import numpy as np
import os
import json
import sys
import plotOverpass as obs
import datetime
import mutils as m
import mutils.PosTypeConst as PosTypeConst
from mutils.PosPacConst import *
from ProcessResults import parse_sampleConfig, cdf_vals, get_rounded_cdf_percents,find_overpasses,bin_measurements,plot_meas_bins

import matplotlib.pyplot as plt
from matplotlib import use

# Allow running headless from the command line
use("agg")

# Override pylab find() to avoid deprecation warning
# pylint: disable=function-redefined
def find(x):
  return np.where(x)[0]


def findInDict(lst, key, value):
  for i, dic in enumerate(lst):
   if dic[key] == value:
     return i
  return -1


if __name__ == "__main__":
  # data format:
  # pass_num time range_to_overpass fixtype lat lon hgt pdop dE dN dU sigmaE sigmaN sigmaU true_heading true_roll true_pitch

  now = datetime.datetime.now()
  YYYYMMDD = str(now.year) + '-' + str(now.month).zfill(2) + '-' + str(now.day).zfill(2)
  if not os.path.exists(YYYYMMDD):
    os.makedirs(YYYYMMDD)

  if not os.path.exists("Trend"):
    os.makedirs("Trend")

  if(len(sys.argv) == 2):
    with open(sys.argv[1],'r') as f: 
      config = json.load(f)
  else:
    print("Usage:\n  python processOverpass.py config.json")
    sys.exit()

  firstRX = None
  processedDataSet = []
  
  ObsStore = []
  dataBase = '/net/fermion/mnt/data_drive/SpirentTest/GPSTools/RFPlaybackRegression/DataDir/'
  ppFile = None

  for run in config['dataSets']: # The json file can include multiple runs - read and concat.
    for RXRec in config['RX']:
      RX = RXRec['ID']
      if not os.path.exists(YYYYMMDD + '/RX' + str(RX)):
        os.makedirs(YYYYMMDD + '/RX' + str(RX))

      longLabel = RXRec['label'] # Too long for the title in many graphs
      
      # Get the index into the ObsStore array of dictionaries - create
      # an entry if we don't have one
      index = findInDict(ObsStore,'RX', RX)
      if(index == -1):
        ObsStore.append({'RX':RX,'Label':longLabel,'numOverpasses':0,'Obs':[]})
        index = findInDict(ObsStore,'RX', RX)

      if(firstRX is None):
        firstRX = RX

      # The data directory has a symlink to the latest data set. By expanding the symlink we can determine
      # the run number, we use this in the path to the results.
      latest = dataBase + 'latest-RX' + str(RX) + '-' + run + '/'

      # Now break out the run ID from the full path (expand the symlink)
      fullPath= os.path.realpath(latest)
      RunID = (fullPath.split('/')[-1]).split('-')[1]
      
      # Save this so we can provide a summary at the end
      if(firstRX == RX):
        processedDataSet.append(RunID + ' ' + run + '.xml')

      XMLConfig = fullPath +  '/' + run + '.xml'
      # Parse the XML config file
      RunConfig = parse_sampleConfig(XMLConfig)

      # Get the truth - it will be the same for all receivers, so we only
      # need to read it once per dataset
      if(ppFile != RunConfig.truth_file):
        pp=m.doload(RunConfig.truth_file)
        ppFile = RunConfig.truth_file

      try:
        print("Load Data Run",fullPath)
        # Get the measurements
        meas = m.vd2cls(fullPath + '/*.T04', '-d35:19')
        
        # Some of the truth files cover multiple RF sample sets. Therefore, filter the
        # data to only include the current test to avoid over counting the number of overpasses
        idx = find( (pp[:,dPP_TIME] >= meas.TIME[0]) & (pp[:,dPP_TIME] <= meas.TIME[-1]) ) 
        overpass_locs, events = find_overpasses( pp[idx,:], False )
        ObsStore[index]['numOverpasses'] += len(events)
        print("Num Overpasses Data Run",fullPath,len(events))

        # Get the epochs as a function of distance to/from the obstruction
        results = bin_measurements( meas, events, 10 )

        if(len(results) > 0):

          print("RX ",RX,"Run ",RunID,"run",run,"GPS Bin 0",results['all'][0,0,0][0])

          if(len(ObsStore[index]['Obs']) == 0):
            ObsStore[index]['Obs'] = results
          else:
            # Combine the data 
            all_types = np.array(list(results['all'].keys()))
            for i in range(len(all_types)):
              sat,freq,track = all_types[i]
              for j in range(len(ObsStore[index]['Obs']['all'][sat,freq,track])):
                ObsStore[index]['Obs']['all'][sat,freq,track][j] += results['all'][sat,freq,track][j]
                ObsStore[index]['Obs']['pll'][sat,freq,track][j] += results['pll'][sat,freq,track][j]
                ObsStore[index]['Obs']['lock'][sat,freq,track][j] += results['lock'][sat,freq,track][j]
         
        print("index = ",index," len(ObsStore[]['Obs']) ",len(ObsStore[index]['Obs']), "NumOP ", ObsStore[index]['numOverpasses'])
      except:
        print("Problem with Data Run",fullPath)

for i in range(len(ObsStore)):
  RX = ObsStore[i]['RX']
  plot_meas_bins( YYYYMMDD + '/RX' + str(RX), ObsStore[i]['numOverpasses'], ObsStore[i]['Obs'])

# We typically order the units from "worst" to "best" in the JSON file, the "worst"
# is also likely to be the oldest firmware and may not have some of the more recent
# signal types. Use the last receiver from the list to get the list of signal types.
all_types = np.array(list(ObsStore[-1]['Obs']['lock'].keys()))
for i in range(len(all_types)):
  sat,freq,track = all_types[i]

  sat_str = m.get_sat_type(sat).sysstr
  sigType = m.get_sub_type(sat,freq,track).sigstr.split()[1]

  plt.figure()
  for k in range(len(ObsStore)):
    x  = np.copy(ObsStore[k]['Obs']['x'][1:])
    dx = x[1]-x[0]
    x -= dx/2 # center bins for plot
    label = 'RX' + str(ObsStore[k]['RX']) + ' - ' + ObsStore[k]['Label']
    plt.plot(x, ObsStore[k]['Obs']['lock'][sat,freq,track], '-1',label=label)

    # Save for further trending
    with open('Trend/RX' + str(ObsStore[k]['RX']) + '.' + str(sat) + '.' + str(freq) + '.' + str(track) + '.csv','a') as fid:
      fid.write('%d,%d,%d,%d,' % (now.year,now.month,now.day,ObsStore[k]['numOverpasses']))
      for j in range(len(x)):
        fid.write('%d' % ObsStore[k]['Obs']['lock'][sat,freq,track][j])
        if(j < (len(x) -1)):
          fid.write(',')
        else:
          fid.write('\n')

  plt.grid(True)

  # Warning - assuming the num overpasses is the same for each receiver.
  # This should be correct, unless one receiver stops processing and the
  # overpass map is updated
  numOverpasses = ObsStore[0]['numOverpasses']
  plt.title(sat_str + ' ' + sigType + ':' + str(numOverpasses) + ' Overpasses')
  plt.xlabel('Distance to/from overpass [m]')
  plt.ylabel('Number of Epochs w/Lock Point')
  plt.legend(fontsize=8)
  obs.savePlot(YYYYMMDD,YYYYMMDD + '_' + sat_str + '_' + sigType + '_CDF.png')
  plt.close()


fid = open(YYYYMMDD + '/' + YYYYMMDD + '-datasets.txt','w')
for dataSet in processedDataSet:
  fid.write("%s\n" % dataSet)
fid.close()