import matplotlib
# Allow running headless from the command line
matplotlib.use("agg")
from pylab import *

import numpy as np
from scipy.signal import butter, lfilter, freqz
import mutils as m

#
# See this document
# https://docs.google.com/document/d/1XKkHff0R_ODxA_qRNYNbZPsKJtLGgND4Xk6Y80D_cFo/edit#
# 
# The output UCD file is based on this simulation (file located on the GSS9000 simulator PC):
# D:\posapp\Scenarios\IonoGradient\IonoGradient-1.scn
# For the UCD to have any effect, the simulated PRNs need to be included in the Spirent
# .scn scenario!
#
# Loads several T04 datasets. From this we compute the L1-L2 phase
# for a single satellite over an interesting time window where there was 
# a large iono gradient. 
#
# We then low pass filter the data to remove small non-physical spikes/jumps.
#
# We manipulate the data to compute the ionospheric phase impact at L1 and L2
# frequencies (using the L1 C/A and L2C phase data). We assume the 1/F^2 relationship
# and compute the impact to L5.
#
# We plot a few things
#
# We then create a series of SimRemote MOD commands (see page 5-78 of the SimRemote manual)
# an updated command is created every 0.1s for L1, L2, and L5.
#
# Copyright Trimble Inc 2022.
#

# True  == adjust the C/No
# False == no C/No adjustment 
ApplyCnoScint = True
# True == add a 10dB C/No fade when we don't have iono scint C/No data
# False == no C/No fade
ApplySineCnoFade = False

# This is time from the start of the scenario, add 300s before we start
# adjusting the iono to allow the receiver tracking to stabilize.
sim_time_offset = 300

# How long is the simulation?
sim_length = 45*60 - sim_time_offset

# When did the simulation start? Needed for the "As Run" truth
sim_time_start = 432000 # from the .scn file

# Filename for the Spirent MOD command file
filenameIonoMOD   = 'IONO-MOD.ucd'
# Filename where we'll store the iono truth - used for analytics
filenameIonoTruth = 'IONO-Truth.txt'

def butter_lowpass(cutoff, fs, order=5):
  nyq = 0.5 * fs
  normal_cutoff = cutoff / nyq
  b, a = butter(order, normal_cutoff, btype='low', analog=False)
  return b, a

def butter_lowpass_filter(data, cutoff, fs, order=5):
  b, a = butter_lowpass(cutoff, fs, order=order)
  y = lfilter(b, a, data)
  return y

# We require phase data for each satellite, but C/No data is optional
# simPRN_xxx matches the satellites in the simulation scenario, see also this document
# https://docs.google.com/document/d/1XKkHff0R_ODxA_qRNYNbZPsKJtLGgND4Xk6Y80D_cFo/edit#
config = [{# Which PRN to apply this to in the simulation scenario
           'simPRN_GPS':11,
           'simPRN_Galileo':3,
           # Where we will extract the phase data
           'phaseData':'/net/capacitor2/volume1/main/GNSSData/Site3/BX992/*2022033103*.T04',
           'phaseStart':357870,
           'phaseEnd':359782,
           'phasePRN':15,
           'phaseScale':1.0,
           # Where we will extract the C/No fading
           'cnoData':'/net/capacitor2/volume1/main/GNSSData/Site1/Alloy/6037C01546202202230000.T04',
           'cnoStart':260880,
           'cnoEnd':261960,
           'cnoPRN':17,
           'cnoScale':2.0},
          {'simPRN_GPS':1,
           'simPRN_Galileo':4,
           'phaseData':'/net/capacitor2/volume1/main/GNSSData/Site3/BX992/BX992_____202204020300.T04',
           'phaseStart':530642,
           'phaseEnd':531687,
           'phasePRN':15,
           'phaseScale':1.0,
           'cnoData':'/net/capacitor2/volume1/main/GNSSData/Site1/Alloy/6037C01546202202280200.T04',
           'cnoStart':93610,
           'cnoEnd':95030,
           'cnoPRN':17,
           'cnoScale':1.5},
          {'simPRN_GPS':28,
           'simPRN_Galileo':9,
           'phaseData':'/net/capacitor2/volume1/main/GNSSData/Site3/BX992/BX992_____202204020300.T04',
           'phaseStart':530642,
           'phaseEnd':531687,
           'phasePRN':29,
           'phaseScale':1.0,
           'cnoData':'/net/capacitor2/volume1/main/GNSSData/Site1/Alloy/6037C01546202202280300.T04',
           'cnoStart':97310,
           'cnoEnd':98805,
           'cnoPRN':6,
           'cnoScale':1.5},
          {'simPRN_GPS':8,
           'simPRN_Galileo':10,
           'phaseData':'/net/capacitor2/volume1/main/GNSSData/Site3/BX992/BX992_____202204100400.T04',
           'phaseStart':15835,
           'phaseEnd':17500,
           'phasePRN':18,
           'phaseScale':1.0,
           'cnoData':'/net/capacitor2/volume1/main/GNSSData/Site1/Alloy/6037C01546202203010000.T04',
           'cnoStart':173150,
           'cnoEnd':175800,
           'cnoPRN':14,
           'cnoScale':1.5},
          {'simPRN_GPS':6,
           'simPRN_Galileo':20,
           'phaseData':'/net/capacitor2/volume1/main/GNSSData/Site3/BX992/BX992_____202204100400.T04',
           'phaseStart':16945,
           'phaseEnd':17512,
           'phasePRN':24,
           'phaseScale':1.0,
           'cnoData':'/net/capacitor2/volume1/main/GNSSData/Site1/Alloy/6037C01546202203010100.T04',
           'cnoStart':176640,
           'cnoEnd':178610,
           'cnoPRN':17,
           'cnoScale':1.5},
          {'simPRN_GPS':22,
           'simPRN_Galileo':2,
           'phaseData':'/net/capacitor2/volume1/main/GNSSData/Site3/BX992/BX992_____202204100600.T04',
           'phaseStart':22807,
           'phaseEnd':25160,
           'phasePRN':23,
           'phaseScale':1.0},
          {'simPRN_GPS':27,
           'simPRN_Galileo':19,
           'phaseData':'/net/capacitor2/volume1/main/GNSSData/Site3/BX992/BX992_____202204101000.T04',
           'phaseStart':36155,
           'phaseEnd':37714,
           'phasePRN':1,
           'phaseScale':1.0},
          {'simPRN_GPS':19,
           'phaseData':'/net/fermion/mnt/data_drive/rxu/Web3/GPSTools/RFPlaybackRegression/DataDir/RX1-358/*2022040207*.T04',
           'phaseStart':544800,
           'phaseEnd':545660,
           'phasePRN':8,
           'phaseScale':1.0},
          # ToDo - add file caching - we are using the same data, but scaling the phase. The code will currently
          # read the file multiple times!
          {'simPRN_GPS':7,
           'phaseData':'/net/fermion/mnt/data_drive/rxu/Web3/GPSTools/RFPlaybackRegression/DataDir/RX1-358/*2022040207*.T04',
           'phaseStart':544800,
           'phaseEnd':545660,
           'phasePRN':8,
           'phaseScale':2.0},
          {'simPRN_GPS':3,
           'phaseData':'/net/fermion/mnt/data_drive/rxu/Web3/GPSTools/RFPlaybackRegression/DataDir/RX1-358/*2022040207*.T04',
           'phaseStart':544800,
           'phaseEnd':545660,
           'phasePRN':8,
           'phaseScale':3.0},
          {'simPRN_GPS':25,
           'phaseData':'/net/fermion/mnt/data_drive/rxu/Web3/GPSTools/RFPlaybackRegression/DataDir/RX1-358/*2022040207*.T04',
           'phaseStart':544800,
           'phaseEnd':545660,
           'phasePRN':8,
           'phaseScale':4.0}]

if __name__ == "__main__":
  I1_Phase_Store = []
  I2_Phase_Store = []
  I5_Phase_Store = []
  I6_Phase_Store = []

  if(ApplyCnoScint == True):
    print('Applying "real" C/No scintillation')
  else:
    print('No C/No scintillation')
    # Force to false
    ApplySineCnoFade = False

  if(ApplySineCnoFade == True):
    print('Applying a 10dB C/No fade PRN dependent when C/No scintillation data runs out')
  else:  
    print('No C/No fade modelled')

  # Extract the iono data
  for loop in range(len(config)):
    dataPath   = config[loop]['phaseData']
    startTime  = config[loop]['phaseStart']
    endTime    = config[loop]['phaseEnd']
    phaseScale = config[loop]['phaseScale']
    prn = config[loop]['phasePRN']

    # Read just the data we want to analyze
    data = m.vd2cls(dataPath,'-d35:19 -pg' + str(prn) + ' -s' + str(startTime) + ' -e' + str(endTime))

    # Break out the L1 C/A and L2C data - shouldn't need to check the system and prn - leave just in case someone messes up the vd2cls/viewdat command
    i_L1CA = m.find( (data[:,data.k.SAT_TYPE] == 0) & (data[:,data.k.SV] == prn) & (data[:,data.k.ANTENNA] == 0) & (data[:,data.k.TRACK] == 0) & (data[:,data.k.FREQ] == 0) )
    i_L2C  = m.find( (data[:,data.k.SAT_TYPE] == 0) & (data[:,data.k.SV] == prn) & (data[:,data.k.ANTENNA] == 0) & (data[:,data.k.TRACK] == 5) & (data[:,data.k.FREQ] == 1) )

    # Make sure the L1 and L2 data is time aligned
    L1CA = data[i_L1CA,:]
    L2C  = data[i_L2C,:]
    _, i1, i2 = m.intersect( L1CA[:,L1CA.k.TIME], L2C[:,L2C.k.TIME] )

    # Get the iono change in metres
    res =  (  L1CA[i1,L1CA.k.PHASE] * m.GnssConst.L1_WAVELENGTH
            -  L2C[i2,L2C.k.PHASE]  * m.GnssConst.L2_WAVELENGTH)
    # Zero the first element so that we don't get a jump when we add this
    # to the simulated phase
    res = res - res[0]

    # This data will have noise, we don't want non-physical noise to 
    # impact the simulation. Therefore, filter the data a little.
    #
    # 6th order butterworth
    # 10Hz data sample rate
    # 3Hz cut off

    order  = 6
    fs     = 10
    cutoff = 3
    iono_smoothed = butter_lowpass_filter(res, cutoff, fs, order=order)
    # Just in case the filter shifted the data, re-zero
    iono_smoothed = iono_smoothed - iono_smoothed[0]

    # Plot the raw and smoothed data
    plot(L1CA[i1,L1CA.k.TIME],res,label='raw')
    plot(L1CA[i1,L1CA.k.TIME],iono_smoothed,label='smoothed')
    m.save_compressed_png('filterPlotPRN' + str(prn) + '.png', dpi=150)
    close()

    # Now compute the L1 and L2 components of the iono from the standard
    # phase equation.
    F1_2 = m.GnssConst.L1_FREQ**2
    F2_2 = m.GnssConst.L2_FREQ**2
    F5_2 = m.GnssConst.L5_FREQ**2
    F6_2 = m.GnssConst.GALILEO_E6_FREQUENCY**2
    F_2_delta = F1_2 - F2_2

    # Get the iono delay at L1 and L2
    I1 = -iono_smoothed * F2_2 / F_2_delta
    I2 = -iono_smoothed * F1_2 / F_2_delta
    I5 = I1 * F1_2 / F5_2
    E6 = I1 * F1_2 / F6_2

    I1 *= phaseScale
    I2 *= phaseScale
    I5 *= phaseScale
    E6 *= phaseScale
    
    I1_Phase_Store.append(I1)
    I2_Phase_Store.append(I2)
    I5_Phase_Store.append(I5)
    I6_Phase_Store.append(E6)

    # Plot it
    figure
    plot(L1CA[i1,L1CA.k.TIME],I1,label='L1 Iono')
    plot(L1CA[i1,L1CA.k.TIME],I2,label='L2 Iono')
    title('Extracted L1 and L2 Ionospheric delay')
    grid()
    legend()
    xlabel('GPS Time [Secs]')
    ylabel('Relative Delay [m]')
    tight_layout()
    m.save_compressed_png('IonoDelay-' + str(prn) + '-Sim' + str(config[loop]['simPRN_GPS']) + '.png', dpi=150)
    close()

  # Scintillation C/No fading

  if(ApplyCnoScint == True):
    L1_CNo_Store = []
    L2_CNo_Store = []

    figure
    for loop in range(len(config)):
      if('cnoData' in config[loop].keys()):
        dataPath  = config[loop]['cnoData']
        startTime = config[loop]['cnoStart']
        endTime   = config[loop]['cnoEnd']
        prn = config[loop]['cnoPRN']
        cnoScale = config[loop]['cnoScale']

        data = m.vd2cls(dataPath,'-d35:19 -pg' + str(prn) + ' -s' + str(startTime) + ' -e' + str(endTime))
        # Break out the L1 C/A and L2C data - shouldn't need to check the system and prn - leave just in case someone messes up the vd2cls/viewdat command
        i_L1CA = m.find( (data[:,data.k.SAT_TYPE] == 0) & (data[:,data.k.SV] == prn) & (data[:,data.k.ANTENNA] == 0) & (data[:,data.k.TRACK] == 0) & (data[:,data.k.FREQ] == 0) )
        i_L2C  = m.find( (data[:,data.k.SAT_TYPE] == 0) & (data[:,data.k.SV] == prn) & (data[:,data.k.ANTENNA] == 0) & (data[:,data.k.TRACK] == 5) & (data[:,data.k.FREQ] == 1) )

        CNo_L1 = data[i_L1CA,data.k.CNO]
        CNo_L1 = CNo_L1 - np.median(CNo_L1)
        # Make the fades deeper
        i = m.find(CNo_L1 < 0.0)
        # Only scale the C/No loss to get deeper fades
        CNo_L1[i] = CNo_L1[i] * cnoScale
        L1_CNo_Store.append(CNo_L1)


        CNo_L2 = data[i_L2C,data.k.CNO]
        CNo_L2 = CNo_L2 - np.median(CNo_L2)
        i = m.find(CNo_L2 < 0.0)
        CNo_L2[i] = CNo_L2[i] * cnoScale
        L2_CNo_Store.append(CNo_L2)
        plot(CNo_L1,label='L1')
        plot(CNo_L2,label='L2')
        grid()
        tight_layout()
        m.save_compressed_png('CNo-PRN' + str(prn) + '-Sim' + str(config[loop]['simPRN_GPS']) + '.png', dpi=150)
        close()
      else:
        L1_CNo_Store.append('')
        L2_CNo_Store.append('')


  #
  # Now form the UCD file for the simulator
  #

  sig_level = 0 # Don't change the power

  fidMOD = open(filenameIonoMOD,'w')
  fidTruth = open(filenameIonoTruth,'w')

  # Load the correct scenario and run it
  fidMOD.write('SC,D:\posapp\Scenarios\IonoGradient\IonoGradient-1.scn\nRU\n')

  #maxData = 0
  #for index in range(len(I1_Phase_Store)):
  #  thisLen = len(I1_Phase_Store[index])
  #  if(thisLen > maxData):
  #    maxData = thisLen

  for index in range(sim_length*10):
    # Format:
    # <timestamp>,MOD,<veh_ant>,<signal_type>,<id>,<multi_index>,
    # <mode>,<all_flag>,<freq>,<all_freq>,<sig_level>,<carr_offset>,<code_offset>
    # Optional - we aren't setting these
    # [,<azimuth_override>, <elev_override>]

    # timestamp = assuming 10Hz data, we need to offset this to tie into the simulation
    # MOD
    # veh_ant = v1_a1 (see example MOD command)
    # signal_type = GPS
    # id = sim_prn (need to match a satellite in the scenario)
    # Multi-index = 0 incident signal
    # Mode = 0 as we are using the PRN
    # all_flag = 0 - just one satellite
    # freq = 0 == L1
    # freq = 1 == L2
    # freq = 2 == L5
    # all_freq = 0 - single freq adjustment
    # sig_level = sig_level 
    # carr_offset = in meters
    # code_offset = in meters
    # 
    # Note the different sign convention for the code and carrier below:

    for ionoIndex in range(len(I1_Phase_Store)):
      thisPRN = config[ionoIndex]['simPRN_GPS']
      sig_level_L1 = 0
      sig_level_L2 = 0
      I1 = 0
      I2 = 0
      I5 = 0
      I6 = 0

      if(ApplyCnoScint == True):
        if(index < len(L1_CNo_Store[ionoIndex])):
          sig_level_L1 = L1_CNo_Store[ionoIndex][index]
        elif(ApplySineCnoFade == False):
          sig_level_L1 = 0
        elif(index < (1200+len(L1_CNo_Store[ionoIndex]))):
          # 2 minutes with no C/No manipulation
          sig_level_L1 = 0
        else:
          # Add 10dB fades, the frequency is based on the PRN
          # Data is 10Hz, observed fades are just a few seconds. Set a nominal 3 seconds, 
          # and adjust +/- 1.5 seconds based on the PRN
          t = index - (1200+len(L1_CNo_Store[ionoIndex]))
          # cos is +/-1, scale by 0.45 so it is +/- 0.45, add 0.55 so that it is between
          # 0.1 and 1.0 (or -10dB and 0dB).
          sig_level_L1 = 10.0*np.log10(np.cos(2 * np.pi * t/(30 + (thisPRN-16)/10))*.45 +0.55)

        if(index < len(L2_CNo_Store[ionoIndex])):
          sig_level_L2 = L2_CNo_Store[ionoIndex][index]
        elif(ApplySineCnoFade == False):
          sig_level_L2 = 0
        elif(index < (1200+len(L2_CNo_Store[ionoIndex]))):
          sig_level_L2 = 0
        else:
          t = index - (1200+len(L2_CNo_Store[ionoIndex]))
          # Make the L2 fade a slightly different frequency to L1
          sig_level_L2 = 10.0*np.log10(np.cos(2 * np.pi * t/(30 + (thisPRN-10)/12))*.45 +0.55)

      if(index < len(I1_Phase_Store[ionoIndex])):
        I1 = I1_Phase_Store[ionoIndex][index]
        I2 = I2_Phase_Store[ionoIndex][index]
        I5 = I5_Phase_Store[ionoIndex][index]
      else:
        # We have no more phase data, check if there's C/No processing, if not we don't
        # need to create a MOD command
        if(    (ApplyCnoScint == False)
            or (     (index >= len(L1_CNo_Store[ionoIndex]))
                 and (ApplySineCnoFade == False))):
          continue
        # When we run out of phase data, hold the last value so that we don't get a jump
        I1 = I1_Phase_Store[ionoIndex][-1]
        I2 = I2_Phase_Store[ionoIndex][-1]
        I5 = I5_Phase_Store[ionoIndex][-1]

      fidMOD.write(   "%.1f,MOD,v1_a1,GPS,%d,0,0,0,0,0,%.1f,%.4f,%.4f\n"
                   % ( sim_time_offset + index / fs, thisPRN, sig_level_L1, -I1, I1) )
      fidMOD.write(   "%.1f,MOD,v1_a1,GPS,%d,0,0,0,1,0,%.1f,%.4f,%.4f\n"
                   % ( sim_time_offset + index / fs, thisPRN, sig_level_L2, -I2, I2) )
      fidMOD.write(   "%.1f,MOD,v1_a1,GPS,%d,0,0,0,2,0,%.1f,%.4f,%.4f\n"
                   % ( sim_time_offset + index / fs, thisPRN, sig_level, -I5, I5) )
      fidTruth.write(   "%.1f,%d,%.4f,%.4f,%.4f\n"
                     % ( sim_time_start + sim_time_offset + index / fs, thisPRN, I1, I2, I5))
                     
      if('simPRN_Galileo' in config[ionoIndex].keys()):
        if(index < len(I1_Phase_Store[ionoIndex])):
          I6 = I6_Phase_Store[ionoIndex][index]
        else:
          I6 = I6_Phase_Store[ionoIndex][-1]

        thisPRN = config[ionoIndex]['simPRN_Galileo']
        fidMOD.write(   "%.1f,MOD,v1_a1,GALILEO,%d,0,0,0,0,0,%.1f,%.4f,%.4f\n"
                     % ( sim_time_offset + index / fs, thisPRN, sig_level_L1, -I1, I1) )
        fidMOD.write(   "%.1f,MOD,v1_a1,GALILEO,%d,0,0,0,1,0,%.1f,%.4f,%.4f\n"
                     % ( sim_time_offset + index / fs, thisPRN, sig_level, -I6, I6) )
        fidMOD.write(   "%.1f,MOD,v1_a1,GALILEO,%d,0,0,0,2,0,%.1f,%.4f,%.4f\n"
                     % ( sim_time_offset + index / fs, thisPRN, sig_level_L2, -I5, I5) )

  # ToDo - does the Spirent require DOS or Linux line endings ... does it care?
  fidTruth.close()
  fidMOD.close()