from datetime import datetime, timedelta
import numpy as np

def to_datetime(dtlist):
    return datetime(int(dtlist[0]), int(dtlist[1]), int(dtlist[2]),int(dtlist[3]),int(dtlist[4]),int(float(dtlist[5])),round((float(dtlist[5])%1)*1E6))

def dt2float(dt,dt0):
    return ((dt - dt0) / timedelta(seconds=1)).astype('float')

def get_sat_freq(sat):
    GPRN = int(sat[1:])
    if sat[0] == 'G': #GPS
        FL1 = 1.57542E9
        FL2 = 1.22760E9
    elif sat[0] == 'R': #GLONASS
        Kfrq = [1, -4, 5, 6, 1, -4, 5, 6, -2, -7, 0, -1, -2, -7, 0, -1, 4, -3, 3, 2, 4, -3, 3, 2]
        baseFL1 = 1.602E9
        baseFL2 = 1.246E9
        DFRQ1_GLO = 5.625E5
        DFRQ2_GLO = 4.375E5
        FL1 = baseFL1 + Kfrq[GPRN - 1] * DFRQ1_GLO
        FL2 = baseFL2 + Kfrq[GPRN - 1] * DFRQ2_GLO
        GPRN = GPRN + 100
    elif sat[0] == 'E': #GALILEO
        FL1 = 1.57542E9
        FL2 = 1.20714E9
        GPRN = GPRN + 200
    elif sat[0] == 'C': # BEIDOU
        FL1 = 1.57542E9
        FL2 = 1.17645E9
        GPRN = GPRN + 400
    else: # ERROR!!
        FL1 = 0
        FL2 = 0
        GPRN = 0
    return FL1, FL2, GPRN

def disp_progress():
    from re import sub
    status = {-2: 'Aborted', -1: 'Queued', 0: 'Processing', 1: 'Completed'}
    print(' #      Last Modified       TAR File Token      Status')
    print('===  ===================  ==================  ==========')
    with np.load('progress_rt.npz',allow_pickle=True) as fid:
        for nn, (tarfile, progress, timetag) in enumerate(zip(fid['tarfiles'], fid['progress'], fid['timetag'])):
            tarseg = sub(r'.*_(\d{8}_\d{4}_.*)\.tar',r'\1',tarfile)
            print(f"{nn:3d}  {timetag:%Y-%m-%d %H:%M:%S}  {tarseg}  {status[progress]}")

def disp_pod(podfile):
    import pickle
    from matplotlib import pyplot as plt
    from re import sub
    dt0 = datetime.strptime(sub(r'.*\.(\d{4}\.\d{3})\..*',r'\1',podfile),'%Y.%j')
    figfile = sub(r'\.pkl',r'.png',podfile)
    colors = np.array([[int(color[ii:ii + 2], 16) / 255 for ii in range(1, 6, 2)] for color in
                       plt.rcParams['axes.prop_cycle'].by_key()['color']])
    with open(podfile, 'rb') as fid:
        pkldata = pickle.load(fid)
        dsat = pkldata['dsat']
        dtime = pkldata['dtime']
        dataall = pkldata['dataall']
    fig, ax = plt.subplots(1, 1, figsize = (12,9))
    for dnum, (sat, time, data) in enumerate(zip(dsat, dtime, dataall)):
        idx = np.all(np.isfinite(data), axis = 1)
        ax.plot(dt2float(time[~idx], dt0) / 3600, np.full(time[~idx].shape, dnum), '.',
                color=(colors[dnum % 10] + 2) / 3)
        ax.plot(dt2float(time[~idx], dt0) / 3600, np.full(time[~idx].shape, dnum), '.', ms=0.5,
                color=colors[dnum%10])
        ax.plot(dt2float(time[idx],dt0)/3600, np.full(time[idx].shape, dnum),'.',color = colors[dnum%10])
    ax.grid(True)
    ax.set_xlabel(f'Hours of {dt0:%Y-%m-%d}')
    ax.set_xticks(range(-3,30,3))
    ax.set_ylim((-1,len(dsat)))
    ax.set_yticks(range(len(dsat)))
    ax.set_yticklabels(dsat)
    ax.set_title(sub(r'.*/(.*)\.pkl',r'\1',podfile))
    fig.savefig(figfile, dpi = 300)

def load_gnss(file):
    with np.load(file, allow_pickle=True) as fid:
        Gdata = fid['Gdata']
        Gdt = fid['Gdt']
        Gsats = fid['Gsats']
    return Gdata, Gdt, Gsats

def read_gnss(dt,prev=False):
    import os
    Gdata1 = np.array([])
    if prev:
        for x in ['NRT','RT']:
            gnssfile = f'../gnss/gnssOrb/GNSS_{dt - timedelta(days=1):%Y.%j}_{x}.npz'
            if os.path.isfile(gnssfile):
                if len(Gdata1) == 0:
                    Gdata1, Gdt1, Gsats1 = load_gnss(gnssfile)
                else:
                    Gdata0, Gdt0, Gsats0 = load_gnss(gnssfile)
                    if Gdt1.shape == Gdt0.shape and np.all(Gdt1 == Gdt0):
                        idx = ~np.isin(Gsats0, Gsats1)
                        Gsats1 = np.hstack([Gsats1, Gsats0[idx]])
                        Gdata1 = np.hstack([Gdata1, Gdata0[:, idx, :]])
                    del Gdata0, Gdt0, Gsats0
    Gdata2 = np.array([])
    for x in ['NRT','RT']:
        gnssfile = f'../gnss/gnssOrb/GNSS_{dt:%Y.%j}_{x}.npz'
        if os.path.isfile(gnssfile):
            if len(Gdata2) == 0:
                Gdata2, Gdt2, Gsats2 = load_gnss(gnssfile)
            else:
                Gdata0, Gdt0, Gsats0 = load_gnss(gnssfile)
                if Gdt2.shape == Gdt0.shape and np.all(Gdt2 == Gdt0):
                    idx = ~np.isin(Gsats0, Gsats2)
                    Gsats2 = np.hstack([Gsats2, Gsats0[idx]])
                    Gdata2 = np.hstack([Gdata2, Gdata0[:, idx, :]])
                del Gdata0, Gdt0, Gsats0
    Gsats = Gsats2
    if prev:
        idx = Gdt1<Gdt2[0]
        Gdt1 = Gdt1[idx]
        Gdata1 = Gdata1[idx]
        Gdt = np.hstack([Gdt1, Gdt2])
        Gdata = np.vstack([np.full((len(Gdt1),len(Gsats),8), np.nan),Gdata2])
        for Gsat in Gsats:
            if Gsat in Gsats1:
                Gdata[:len(Gdt1),Gsat==Gsats,:] = Gdata1[:,Gsat==Gsats1,:]
    else:
        Gdata = Gdata2
        Gdt = Gdt2
    return Gdata, Gdt, Gsats

def read_gnss_v1(dt_rng):
    from glob import glob
    from re import sub
    src_types = ['GRG','GFZ','COD']
    gfiles = np.hstack([glob(f'../gnss/gnssLrg/{dt_rng[0]-timedelta(days=x):%Y.%j}/*.npz') for x in range(3)])
    tstart = np.array([datetime.strptime(sub(r'.*_s(\d+)_.*', r'\1', gfile), '%Y%j%H%M') for gfile in gfiles])
    tend = np.array([datetime.strptime(sub(r'.*_e(\d+)_.*', r'\1', gfile), '%Y%j%H%M') for gfile in gfiles])
    tidx = np.logical_and(tstart+timedelta(hours=1)<dt_rng[0],tend-timedelta(hours=1)>dt_rng[1])
    gfiles = gfiles[tidx]
    tstart = tstart[tidx]
    tidx = tstart == max(tstart)
    gfiles = gfiles[tidx]
    tstart = tstart[tidx]
    # tend = tend[tidx]
    Gdt = None
    Gsats = []
    Gdata = []
    for ss in src_types:
        sidx = np.where([ss in gf for gf in gfiles])[0]
        if sidx.size == 0:
            continue
        gfile = gfiles[sidx[0]]
        Gdata0, Gdt0, Gsats0 = load_gnss(gfile)
        tidx = np.logical_and(Gdt0>=dt_rng[0]-timedelta(minutes=10),Gdt0<dt_rng[1]+timedelta(minutes=10))
        sidx = ~np.isin(Gsats0,Gsats)
        Gdata0 = Gdata0[tidx][:,sidx]
        Gsats0 = Gsats0[sidx]
        Gdt0 = Gdt0[tidx]
        if Gdt is None:
            Gdata = Gdata0
            Gsats = Gsats0
            Gdt = Gdt0
        else:
            if ~np.all(Gdt == Gdt0):
                continue
            Gdata = np.hstack([Gdata, Gdata0])
            Gsats = np.hstack([Gsats, Gsats0])
    return Gdata, Gdt, Gsats

def read_gnss_v2(dt_rng, gsats):
    from glob import glob
    from re import sub
    src_types = ['GRG','GFZ','COD']
    gfiles = np.hstack([glob(f'../gnss/gnssLrg/{dt_rng[0]-timedelta(days=x):%Y.%j}/*.npz') for x in range(3)])
    tstart = np.array([datetime.strptime(sub(r'.*_s(\d+)_.*', r'\1', gfile), '%Y%j%H%M') for gfile in gfiles])
    tend = np.array([datetime.strptime(sub(r'.*_e(\d+)_.*', r'\1', gfile), '%Y%j%H%M') for gfile in gfiles])
    tidx = np.logical_and(tstart+timedelta(hours=1)<dt_rng[0],tend-timedelta(hours=1)>dt_rng[1])
    gfiles = gfiles[tidx]
    tstart = tstart[tidx]
    # tend = tend[tidx]
    Gdt = None
    for gsat in gsats:
        gfile = None
        for ss in src_types:
            sidx = np.where([ss in gf for gf in gfiles])[0]
            if sidx.size==0:
                continue
            sfile = gfiles[sidx[np.argmin(np.abs(dt_rng[0] - tstart[sidx] - timedelta(hours=6)))]]
            with np.load(sfile, allow_pickle=True) as fid:
                if gsat in fid['Gsats']:
                    gfile = sfile
                    break
        if gfile is None:
            raise FileNotFoundError(f'Could not find a GNSS Orb file containing {gsat}.')
        Gdata0, Gdt0, Gsats0 = load_gnss(gfile)
        tidx = np.logical_and(Gdt0>=dt_rng[0]-timedelta(minutes=10),Gdt0<dt_rng[1]+timedelta(minutes=10))
        sidx = np.searchsorted(Gsats0,gsat)
        Gdata0 = Gdata0[tidx,sidx]
        Gdt0 = Gdt0[tidx]
        if Gdt is None:
            Gdata = Gdata0[:,None,:]
            Gdt = Gdt0
        else:
            tidx = np.isin(Gdt, Gdt0)
            Gdt = Gdt[tidx]
            Gdata = Gdata[tidx]
            tidx = np.isin(Gdt0, Gdt)
            Gdata0 = Gdata0[tidx]
            Gdata = np.hstack([Gdata, Gdata0[:,None,:]])
    return Gdata, Gdt

# def get_sun_manual(jd):
#     deg2rad = np.pi / 180
#     tut1 = (jd - 2451545) / 36525
#     meanlong = (280.460 + 36000.77 * tut1) % 360
#     meananomaly = ((357.5277233 + 35999.05034 * tut1) % 360) * deg2rad
#     eclplong = ((meanlong + 1.914666471 * np.sin(meananomaly) +
#                             0.019994643 * np.sin(2.0 * meananomaly)) % 360) * deg2rad
#     obliquity = (23.439291 - 0.0130042 * tut1) * deg2rad
#     magr = 1.000140612 - 0.016708617 * np.cos(meananomaly) - 0.000139589 * np.cos(2.0 * meananomaly)
#     rsun = np.array([np.cos(eclplong), np.sin(eclplong)*np.cos(obliquity), np.sin(eclplong)*np.sin(obliquity)]).transpose() * np.expand_dims(magr, axis=1)
#     return rsun

def conv_gnss_ant(pos, ant, dts):
    from astropy.time import Time, TimeDelta
    from astropy.coordinates import get_sun
    from astropy import units as u
    dts = Time(dts) - TimeDelta(18, format='sec')  # leap seconds
    ez = -pos * [1, 1, 1.00669437999014]
    ex = pos - get_sun(dts).cartesian.xyz.to(u.km).value.transpose()
    # ex = pos - get_sun_manual(dts.jd) * u.au.to(u.km) # original MATLAB function
    ez /= np.sum(ez ** 2, axis=1)[:,None] ** 0.5
    ey = np.cross(ez, ex)
    ey /= np.sum(ey ** 2, axis=1)[:,None] ** 0.5
    ex = np.cross(ey, ez)
    return ant[0]*ex + ant[1]*ey + ant[2]*ez

def lagrange(X, Y, x, order):
    X = np.ravel(X)
    nX = len(X)
    nx = len(x)
    if Y.ndim == 1:
        y = np.full((nx, ), np.nan)
    else:
        y = np.full((nx, Y.shape[1]), np.nan)
    T = X[:, None] - X[None, :]
    np.fill_diagonal(T, 1)
    for ii in range(nx):
        idx = np.searchsorted(X, x[ii])
        if idx < X.size and X[idx] == x[ii]:
            y[ii] = Y[idx]
        else:
            idxL = max(min(idx - order, nx - 2 * order), 0)
            idxR = min(max(idx + order, 2 * order), nX)
            J = np.arange(idxL, idxR)
            pX = x[ii] - X[J] # method 1, quick as it multiplies all numerators and all denominators before division
            pX = np.prod(pX) / pX
            R = np.prod(T[J][:,J], axis=1)
            y[ii] = (pX / R) @ Y[J]
            # pX = np.tile(x[ii] - X[J], (len(J), 1)) # method 2, reduce error as it pairs numerators and denominators
            # np.fill_diagonal(pX, 1)
            # R = T[J][:,J]
            # y[ii] = np.prod(pX / R, axis=1) @ Y[J]
    return y

def inv_quat_rotate(q, v):
    t = np.array([q[:,3]*v[0]+q[:,1]*v[2]-q[:,2]*v[1],
                  q[:,3]*v[1]+q[:,2]*v[0]-q[:,0]*v[2],
                  q[:,3]*v[2]+q[:,0]*v[1]-q[:,1]*v[0],
                 -q[:,0]*v[0]-q[:,1]*v[1]-q[:,2]*v[2]])
    v = np.array([-t[3]*q[:,0]+t[0]*q[:,3]-t[1]*q[:,2]+t[2]*q[:,1],
                  -t[3]*q[:,1]+t[1]*q[:,3]-t[2]*q[:,0]+t[0]*q[:,2],
                  -t[3]*q[:,2]+t[2]*q[:,3]-t[0]*q[:,1]+t[1]*q[:,0]]).transpose()
    return v

def phase_correction(l1, pos_gnss, vel_gnss, sec_gnss, pos_leo, vel_leo, att_leo, sec_leo, sec_ro, antoffset_leo, calc_exph):
    C = 299792458
    G = 6.67408E-11
    Mearth = 5.9722E24
    ocd = 0
    const = -6.9693E-10
    GMoCC = 2*G*Mearth/C/C # Schwarzschild radius
    pos_gnss_intp = lagrange(sec_gnss - pos_gnss[:, 3] / 1E6, pos_gnss[:, :3], sec_ro, 11)
    # vel_gnss_intp = lagrange(sec_gnss - pos_gnss[:, 3] / 1E6, vel_gnss[:, :3], sec_ro, 11)
    pos_leo_intp = lagrange(sec_leo, pos_leo[:, :3], sec_ro, 11)
    vel_leo_intp = lagrange(sec_leo, vel_leo[:, :3], sec_ro, 11)
    att_leo_intp = lagrange(sec_leo, att_leo, sec_ro, 4)
    att_leo_intp /= np.sum(att_leo_intp ** 2, axis=1)[:,None] ** 0.5
    antoffset_leo_intp = inv_quat_rotate(att_leo_intp, antoffset_leo)
    pos_leo_intp += antoffset_leo_intp
    antvel_leo_intp = np.full(antoffset_leo_intp.shape, np.nan)
    antvel_leo_intp[1:-1] = (antoffset_leo_intp[2:] - antoffset_leo_intp[:-2]) / (sec_ro[2:,None] - sec_ro[:-2,None])
    antvel_leo_intp[0] = antvel_leo_intp[1]
    antvel_leo_intp[-1] = antvel_leo_intp[-2]
    vel_leo_intp += antvel_leo_intp
    obj_leo = dict()
    obj_leo['pos'] = pos_leo_intp
    obj_leo['vel'] = vel_leo_intp
    obj_leo['clk_s'] = lagrange(sec_leo, pos_leo[:, 3], sec_ro, 6) / 1E6
    obj_leo['clk_m'] = obj_leo['clk_s'] * C
    obj_gnss = dict()
    obj_gnss['clk_s'] = lagrange(sec_gnss, pos_gnss[:, 3], sec_ro, 6) / 1E6
    obj_gnss['clk_m'] = obj_gnss['clk_s'] * C
    tao_new = 1E5
    for _ in range(50):
        range_lg = np.sum((pos_leo_intp - pos_gnss_intp) ** 2, axis=1) ** 0.5
        if calc_exph:
            exph = l1 - range_lg
            vec_ol = pos_leo_intp[[0,-1]]
            vec_og = pos_gnss_intp[[0,-1]]
            vec_lg = vec_og - vec_ol
            proj_lg2 = np.sum(np.cross(vec_ol, vec_og) ** 2, axis=1) / np.sum(vec_lg ** 2, axis=1)
            ocd = proj_lg2[0] > proj_lg2[1]
            exph = exph - exph[0] if ocd else exph - exph[-1]
        else:
            exph = np.zeros_like(l1)
        range_leo = np.sum(pos_leo_intp ** 2, axis=1) ** 0.5
        range_gnss = np.sum(pos_gnss_intp ** 2, axis=1) ** 0.5
        delta_rho = GMoCC * np.log((range_gnss + range_leo + range_lg) / (range_gnss + range_leo - range_lg))
        tao_old = tao_new
        tao_new = (range_lg + exph + delta_rho) / C
        pos_gnss_intp = lagrange(sec_gnss - pos_gnss[:, 3] / 1E6, pos_gnss[:, :3], sec_ro - tao_new, 9)
        if not np.any(np.abs(tao_new - tao_old) > 1E-12):
            break
    vel_gnss_intp = lagrange(sec_gnss - pos_gnss[:, 3] / 1E6, vel_gnss[:, :3], sec_ro - tao_new, 9)
    range_dT = np.sum((pos_leo_intp - pos_gnss_intp) ** 2, axis=1) ** 0.5
    # from scipy.interpolate import make_interp_spline
    # speed_dT = make_interp_spline(sec_ro, range_dT, k=2).derivative()(sec_ro)
    obj_gnss['pos'] = pos_gnss_intp
    obj_gnss['vel'] = vel_gnss_intp
    coef_leo = 1 - (GMoCC / range_leo - np.sum(vel_leo_intp ** 2, axis=1) / C / C) / 2 - const
    propertimedelay_gnss = (coef_leo - 1) * obj_gnss['clk_s'] * C
    delt_t_rel_gnss = -2 * np.sum(pos_gnss_intp * vel_gnss_intp, axis=1) / C / C
    delt_t_rel_leo = -2 * np.sum(pos_leo_intp * vel_leo_intp, axis=1) / C / C
    obj_gnss['range_diff_T'] = range_dT
    obj_gnss['range'] = range_lg
    obj_gnss['GL_term'] = -delta_rho + C * delt_t_rel_gnss + propertimedelay_gnss + obj_gnss['clk_s'] * C
    obj_leo['delt_t_rel_r'] = delt_t_rel_leo
    obj_gnss['delt_t_rel_s'] = delt_t_rel_gnss
    obj_leo['ocd'] = ocd
    return obj_gnss, obj_leo

def remove_cycleslip(ph, ph_m, freq, sat_l, ocd):
    c0 = 299792458
    if not ocd:
        ph = ph[::-1]
        ph_m = ph_m[::-1]
    ph_new = (ph - ph_m) * (2 * freq / c0)
    if sat_l == 'G1':
        idx = np.unique(np.where(np.round(np.diff(ph_new[:100])) % 2 == 1)[0] % 2)
        if idx.size != 1:
            ph_new -= np.cumsum(np.concatenate(([0], np.round(np.diff(ph_new)))))
        else:
            for ii in range(idx[0] + 1, len(ph_new), 2):
                ph_new[ii:ii + 2] -= np.round(ph_new[ii] - ph_new[ii - 1])
            ph_new -= np.cumsum(np.concatenate(([0], np.round(np.diff(ph_new) / 2) * 2)))
    elif sat_l[0] == 'R':
        ph_new -= np.cumsum(np.concatenate(([0], np.round(np.diff(ph_new)))))
    else:
        ph_new -= np.cumsum(np.concatenate(([0], np.round(np.diff(ph_new) / 2) * 2)))
    ph_new = ph_new * (c0 / (2 * freq)) + ph_m
    if not ocd:
        ph_new = ph_new[::-1]
    return ph_new

def calculate_exph(rofile, reffiles):
    from re import sub
    from scipy.interpolate import CubicSpline
    from frame_conversions import ro_ecef2eci, ro_eci2ecef
    from subprocess import run
    # Constants
    version = '1.0'
    dt0_gps = datetime(1980,1,6)
    antennas = ['POD0', 'POD1', 'OCC2', 'OCC3', 'OCC4', 'OCC5', 'OCC6', 'OCC7']
    # Choose reference file
    for reffile in reffiles:
        with np.load(reffile, allow_pickle=True) as fid:
            data_ref = fid['data']
        yy = data_ref[:,3]-data_ref[:,2]
        if max(yy)-min(yy)<1000:
            break
    if max(yy)-min(yy)>=1000:
        reffile = reffiles[0]
    # Extract info from file names
    leoName = sub(r'.*occTab_(....)\..*',r'\1',rofile)
    ant_leo_ro = int(sub(r'.*\.A(\d\d)\..*',r'\1',rofile))
    ant_leo_ref = int(sub(r'.*\.A(\d\d)\..*',r'\1',reffile))
    dt_ro = [datetime.strptime(x,'%Y%m%d%H%M%S') for x in sub(r'.*(\d{14}\.\d{14}).*',r'\1',rofile).split('.')]
    dt_ref = [datetime.strptime(x,'%Y%m%d%H%M%S') for x in sub(r'.*(\d{14}\.\d{14}).*',r'\1',reffile).split('.')]
    dt_rr = datetime.strptime(sub(r'.*\.(\d{4}\.\d{3})\..*',r'\1',rofile),'%Y.%j')
    sec_rr = (dt_rr - dt0_gps) / timedelta(seconds=1)
    gnss_ro = sub(r'.*\.(...).npz',r'\1',rofile)
    gnss_ref = sub(r'.*\.(...).npz', r'\1', reffile)
    f1_ro, f2_ro, gprn_ro = get_sat_freq(gnss_ro)
    c2_ro = 1 / ((f1_ro / f2_ro) ** 2 - 1)
    c1_ro = c2_ro + 1
    f1_ref, f2_ref, gprn_ref = get_sat_freq(gnss_ref)
    c2_ref = 1 / ((f1_ref / f2_ref) ** 2 - 1)
    c1_ref = c2_ref + 1
    # ant_gps = np.loadtxt("GPS_Antenna_Infos.Final")
    # ant_gps_start_time = [datetime(*map(int, x[1:7])) for x in ant_gps]
    # ant_gps_end_time = [datetime(*map(int, x[7:13])) for x in ant_gps]
    with np.load('../gnss/GPS_Antenna_Infos.npz', allow_pickle=True) as fid:
        antoffset_gnss_ro = fid[gnss_ro][()]
        antoffset_gnss_ref = fid[gnss_ref][()]
    antoffset_gnss_ro = antoffset_gnss_ro['antenna'][np.where(np.logical_and(
                        antoffset_gnss_ro['dt_rng'][:, 0] < dt_rr,
                        antoffset_gnss_ro['dt_rng'][:, 1] > dt_rr))[0][0]][:3] / 1E3
    antoffset_gnss_ref = antoffset_gnss_ref['antenna'][np.where(np.logical_and(
                         antoffset_gnss_ref['dt_rng'][:, 0] < dt_rr,
                         antoffset_gnss_ref['dt_rng'][:, 1] > dt_rr))[0][0]][:3] / 1E3
    # Read in gnss position/velocity file accordingly
    # Gdata, Gdt, Gsats = read_gnss(dt_ref[1] + timedelta(minutes=10),
    #                               (dt_ref[0] - timedelta(minutes=10)).day != (dt_ref[1] + timedelta(minutes=10)).day)
    # if gnss_ro not in Gsats:
    #     raise LookupError(f'RO satellite {gnss_ro} not in Gsats')
    # if gnss_ref not in Gsats:
    #     raise LookupError(f'REF satellite {gnss_ref} not in Gsats')
    # gidx = np.hstack([np.where(Gsats == gnss_ro), np.where(Gsats == gnss_ref)])[0]
    # Gdata = Gdata[:,gidx,:]
    Gdata, Gdt = read_gnss_v2(dt_ref, [gnss_ro, gnss_ref])
    tidx = np.logical_and(Gdt > dt_ref[0] - timedelta(minutes=10), Gdt < dt_ref[1] + timedelta(minutes=10))
    dts_gnss = Gdt[tidx]
    sec_gnss = dt2float(dts_gnss, dt_rr)
    pos_gnss = Gdata[tidx,:,:4]
    vel_gnss = Gdata[tidx,:,4:]
    del Gdata, Gdt
    # Read in leo position/velocity/attitude files accordingly
    with np.load(f"s01_leoOrb/leoOrb_{leoName}.{dt_rr:%Y.%j}.npz", allow_pickle=True) as fid:
        Ldata = fid['data']
        Ldts = fid['dts']
    tidx = np.logical_and(Ldts > dt_ref[0] - timedelta(minutes=1), Ldts < dt_ref[1] + timedelta(minutes=1))
    dts_leoorb = Ldts[tidx]
    pos_leo = Ldata[tidx,:4]
    vel_leo = Ldata[tidx,4:]
    del Ldata, Ldts
    with np.load(f"s01_leoAtt/leoAtt_{leoName}.{dt_rr:%Y.%j}.npz", allow_pickle=True) as fid:
        Ldata = fid['data']
        Ldts = fid['dts']
        Lvar = fid['vars']
    tidx = np.logical_and(Ldts > dt_ref[0] - timedelta(minutes=1), Ldts < dt_ref[1] + timedelta(minutes=1))
    dts_leoatt = Ldts[tidx]
    att_leo = Ldata[tidx,:][:,['att' in x for x in Lvar]]
    del Ldata, Ldts, Lvar
    tidx = np.logical_and(dts_leoorb >= dts_leoatt[0], dts_leoorb <= dts_leoatt[-1])
    dts_leo = dts_leoorb[tidx]
    sec_leo = dt2float(dts_leo, dt_rr)
    att_leo = CubicSpline(dt2float(dts_leoatt,dts_leo[0]), att_leo)(dt2float(dts_leo,dts_leo[0]))
    att_leo /= np.sum(att_leo ** 2, axis=1)[:,None] ** 0.5
    del dts_leoorb, dts_leoatt
    tsmp = 30
    tspc = np.round(np.linspace(0,len(sec_leo)-1,min(max(-((len(sec_leo)-1)//-tsmp)+1,20),len(sec_leo)))).astype(int)
    dts_leo = dts_leo[tspc]
    sec_leo = sec_leo[tspc]
    pos_leo = pos_leo[tspc]
    vel_leo = vel_leo[tspc]
    att_leo = att_leo[tspc]
    # Load ro/ref files
    with np.load(rofile, allow_pickle=True) as fid:
        data_ro = fid['data']
        header_ro = fid['header']
    with np.load(reffile, allow_pickle=True) as fid:
        data_ref = fid['data']
        header_ref = fid['header']
    # data_ro[:, 2] = data_ro[:, 2] + data_ro[:, 3]
    # data_ro[:, 4] = data_ro[:, 4] + data_ro[:, 5]
    l_ro = np.where([x[0]=='L' and '(M)' not in x for x in header_ro])[0]
    l_ref = np.where([x[0]=='L' and '(M)' not in x for x in header_ref])[0]
    m_ro = np.where(['(M)' in x for x in header_ro])[0]
    s_ro = np.where([x[0]=='S' for x in header_ro])[0]
    s_ref = np.where([x[0]=='S' for x in header_ref])[0]
    sec_ro = data_ro[:, 0] - sec_rr + data_ro[:, 1]
    sec_ref = data_ref[:, 0] - sec_rr + data_ref[:, 1]
    tidx = np.logical_and(sec_ref > sec_ro[0] - 30, sec_ref < sec_ro[-1] + 30)
    data_ref = data_ref[tidx]
    sec_ref = sec_ref[tidx]
    if sec_leo[0]>sec_ro[0] or sec_leo[-1]<sec_ro[-1]:
        raise ValueError(f'LEO time ({sec_leo[0]:.1f} >> {sec_leo[-1]:.1f}) could not cover RO event ({sec_ro[0]:.1f} >> {sec_ro[-1]:.1f})!')
    sec_ro -= CubicSpline(sec_leo, pos_leo[:,-1])(sec_ro)/1E6
    if sec_leo[0]>sec_ref[0] or sec_leo[-1]<sec_ref[-1]:
        raise ValueError(f'LEO time ({sec_leo[0]:.1f} >> {sec_leo[-1]:.1f}) could not cover REF event ({sec_ref[0]:.1f} >> {sec_ref[-1]:.1f})!')
    sec_ref -= CubicSpline(sec_leo, pos_leo[:,-1])(sec_ref)/1E6
    tidx = np.logical_and(sec_gnss > sec_ro[0] - 905, sec_gnss < sec_ro[-1] + 905)
    dts_gnss = dts_gnss[tidx]
    sec_gnss = sec_gnss[tidx]
    pos_gnss = pos_gnss[tidx]
    vel_gnss = vel_gnss[tidx]
    if ~np.all(np.isfinite(pos_gnss[:, 0, 3])):
        raise ValueError(f'{np.sum(~np.isfinite(pos_gnss[:, 0, 3]))}/{sec_gnss.size} clock bias value for {gnss_ro} is not finite.')
    tidx = np.logical_and(sec_leo > sec_ro[0] - 905, sec_leo < sec_ro[-1] + 905)
    dts_leo = dts_leo[tidx]
    sec_leo = sec_leo[tidx]
    pos_leo = pos_leo[tidx]
    vel_leo = vel_leo[tidx]
    att_leo = att_leo[tidx]
    ant_ro_ecef = conv_gnss_ant(pos_gnss[:, 0, :3], antoffset_gnss_ro, dts_gnss)
    ant_ref_ecef = conv_gnss_ant(pos_gnss[:, 1, :3], antoffset_gnss_ref, dts_gnss)
    pos_leo_eci, vel_leo_eci = ro_ecef2eci(pos_leo, vel_leo, dts_leo)
    pos_ro_eci, vel_ro_eci = ro_ecef2eci(pos_gnss[:, 0, :3] + ant_ro_ecef, vel_gnss[:, 0, :], dts_gnss)
    pos_ref_eci, vel_ref_eci = ro_ecef2eci(pos_gnss[:, 1, :3] + ant_ref_ecef, vel_gnss[:, 1, :], dts_gnss)
    pos_leo_eci = np.hstack([pos_leo_eci, pos_leo[:,[-1]]])
    vel_leo_eci = np.hstack([vel_leo_eci, pos_leo[:, [-1]]])
    pos_ro_eci = np.hstack([pos_ro_eci, pos_gnss[:, 0, [3]]])
    vel_ro_eci = np.hstack([vel_ro_eci, vel_gnss[:, 0, [3]]])
    pos_ref_eci = np.hstack([pos_ref_eci, pos_gnss[:, 1, [3]]])
    vel_ref_eci = np.hstack([vel_ref_eci, vel_gnss[:, 1, [3]]])
    with np.load('antenna_offset.npz') as fid:
        antoffset_leo_ro = fid[f'ANT_{leoName}_{antennas[ant_leo_ro]}'][12:15]
        antoffset_leo_ref = fid[f'ANT_{leoName}_{antennas[ant_leo_ref]}'][12:15]
    obj_gnss_ro, obj_leo_ro = phase_correction(data_ro[:, l_ro[0]], pos_ro_eci, vel_ro_eci, sec_gnss, pos_leo_eci,
                                         vel_leo_eci, att_leo, sec_leo, sec_ro, antoffset_leo_ro, True)
    phL1_ro = data_ro[:, l_ro[0]] - obj_gnss_ro['range_diff_T'] + obj_gnss_ro['GL_term']
    phL2_ro = data_ro[:, l_ro[1]] - obj_gnss_ro['range_diff_T'] + obj_gnss_ro['GL_term']
    obj_gnss_ref, obj_leo_ref = phase_correction(data_ref[:, l_ref[0]], pos_ref_eci, vel_ref_eci, sec_gnss, pos_leo_eci,
                                         vel_leo_eci, att_leo, sec_leo, sec_ref, antoffset_leo_ref, False)
    phL1_ref = data_ref[:, l_ref[0]] - obj_gnss_ref['range_diff_T'] + obj_gnss_ref['GL_term']
    phL2_ref = data_ref[:, l_ref[1]] - obj_gnss_ref['range_diff_T'] + obj_gnss_ref['GL_term']
    dph_ref = phL1_ref - phL2_ref
    dph_ref_smth = np.hstack(
        [np.cumsum(dph_ref[:49])[::2] / np.arange(1, 51, 2), np.convolve(dph_ref, np.ones((51,)), 'valid') / 51,
         (np.cumsum(dph_ref[:-50:-1])[::2] / np.arange(1, 51, 2))[::-1]])
    termL1_ref = phL1_ref + c2_ref * dph_ref_smth
    termL2_ref = phL2_ref + c1_ref * dph_ref_smth
    termL1_ro = CubicSpline(sec_ref, termL1_ref)(sec_ro)
    termL2_ro = CubicSpline(sec_ref, termL2_ref)(sec_ro)
    # termL1_ro = obj_leo_ro['clk_m'] + obj_leo_ro['delt_t_rel_r'] * c0 # Alternative method
    # termL2_ro = obj_leo_ro['clk_m'] + obj_leo_ro['delt_t_rel_r'] * c0
    exL1 = phL1_ro - termL1_ro
    exL2 = phL2_ro - termL2_ro
    if obj_leo_ro['ocd']:
        exL1 -= exL1[0]
        exL2 -= exL2[0]
    else:
        exL1 -= exL1[-1]
        exL2 -= exL2[-1]
    dt_ros = [dt_rr + timedelta(seconds=x) for x in sec_ro]
    pos_leo_ecef, vel_leo_ecef = ro_eci2ecef(obj_leo_ro['pos'], obj_leo_ro['vel'], dt_ros)
    pos_gnss_ecef, vel_gnss_ecef = ro_eci2ecef(obj_gnss_ro['pos'], obj_gnss_ro['vel'], dt_ros)
    exL1_LM = data_ro[:,m_ro[0]] - (data_ro[:,l_ro[0]] - exL1)
    exL1 = remove_cycleslip(exL1, exL1_LM, f1_ro, gnss_ro[0] + '1', obj_leo_ro['ocd'])
    exL2 = remove_cycleslip(exL2, exL1_LM, f2_ro, gnss_ro[0] + '2', obj_leo_ro['ocd'])
    rofile_tkn = sub(r'.*_(.*).npz',r'\1',rofile)
    fortran_input = f'/dev/shm/tcliu/in_{rofile_tkn}.txt'
    fortran_output = f'/dev/shm/tcliu/out_{rofile_tkn}.txt'
    with open(fortran_input, 'w') as fid:
        fid.write(f'{len(sec_ro)} {1 if obj_leo_ro["ocd"] else -1} {int(f1_ro)} {int(f2_ro)}\n')
        for n in range(len(sec_ro)):
            fid.write(f"{sec_ro[n]:15.8f}{exL1[n]:20.8f}{exL2[n]:20.8f}{data_ro[n,m_ro[0]]:20.8f}"
                      f"{pos_leo_ecef[n,0]:15.5f}{pos_leo_ecef[n,1]:15.5f}{pos_leo_ecef[n,2]:15.5f}"
                      f"{pos_gnss_ecef[n,0]:15.5f}{pos_gnss_ecef[n,1]:15.5f}{pos_gnss_ecef[n,2]:15.5f}3{dt_rr.month:2d}\n")
    run(['PhaseModel/cyclesliperemoval', '-i', fortran_input, '-o', fortran_output], stdout=-3, stderr=-3, timeout=5)
    from os import remove, makedirs
    fortran_data = np.loadtxt(fortran_output)
    if np.max(np.abs(fortran_data[:, 0] - exL2)) < np.max(np.abs(exL1 - exL2)):
        exL1 = fortran_data[:, 0]
        exL1_LM = fortran_data[:, 2]
    remove(fortran_input)
    remove(fortran_output)
    idx = np.where(np.isfinite(exL1))[0]
    if obj_leo_ro['ocd']:
        exL1 -= exL1[idx[0]]
        exL2 -= exL2[idx[0]]
    else:
        exL1 -= exL1[idx[-1]]
        exL2 -= exL2[idx[-1]]
    exLC = exL1 + c2_ro * (exL1 - exL2)
    vec_lg = pos_gnss_ecef - pos_leo_ecef
    proj_lg = pos_leo_ecef - vec_lg * (np.sum(pos_leo_ecef * vec_lg, axis=1) / np.sum(vec_lg * vec_lg, axis=1))[:, None]
    from astropy.coordinates import EarthLocation
    alt_km = EarthLocation.from_geocentric(*proj_lg.T, unit='m').geodetic.height.value / 1E3
    if np.logical_or(np.all(alt_km < 90), np.all(alt_km > 0)):
        raise ValueError(f'Non-RO direct height range: {np.min(alt_km):.3f} >> {np.max(alt_km):.3f} (km)')
    idx = np.where(np.logical_and(alt_km > 90, np.isfinite(exLC)))[0]
    trend_LC = np.polyval(np.polyfit(sec_ro[idx], exLC[idx], 1), sec_ro)
    exL1 -= trend_LC
    exL2 -= trend_LC
    exLC -= trend_LC
    out_dir = f's04_atmPhs/{dt_rr:%Y-%m-%d}/'
    makedirs(out_dir, exist_ok=True)
    filestamp = f'{leoName}.{dt_ro[0]:%Y.%j.%H.%M}.{gnss_ro}'
    out_file = out_dir + f'atmPhs_STAR_{filestamp}_v{version}.nc'
    run(['ncgen','-b','./template.cdl','-o',out_file])
    from netCDF4 import Dataset
    with Dataset(out_file,'r+') as fid:
        fid['time'][...] = data_ro[:, 1]
        fid['caL1Snr'][...] = data_ro[:, s_ro[0]]
        fid['pL1Snr'][...] = data_ro[:, s_ro[0]]
        fid['pL2Snr'][...] = data_ro[:, s_ro[1]]
        fid['xLeo'][...] = obj_leo_ro['pos'][:, 0] / 1E3
        fid['yLeo'][...] = obj_leo_ro['pos'][:, 1] / 1E3
        fid['zLeo'][...] = obj_leo_ro['pos'][:, 2] / 1E3
        fid['xdLeo'][...] = obj_leo_ro['vel'][:, 0] / 1E3
        fid['ydLeo'][...] = obj_leo_ro['vel'][:, 1] / 1E3
        fid['zdLeo'][...] = obj_leo_ro['vel'][:, 2] / 1E3
        fid['xGps'][...] = obj_gnss_ro['pos'][:, 0] / 1E3
        fid['yGps'][...] = obj_gnss_ro['pos'][:, 1] / 1E3
        fid['zGps'][...] = obj_gnss_ro['pos'][:, 2] / 1E3
        fid['xdGps'][...] = obj_gnss_ro['vel'][:, 0] / 1E3
        fid['ydGps'][...] = obj_gnss_ro['vel'][:, 1] / 1E3
        fid['zdGps'][...] = obj_gnss_ro['vel'][:, 2] / 1E3
        fid['exL1'][...] = exL1
        fid['exL2'][...] = exL2
        fid['exLC'][...] = exLC
        fid['xmdl'][...] = data_ro[:, m_ro[0]] - data_ro[0, m_ro[0]]
        fid.setncatts({
            'processingHistory': 'STAR_L1A_L1B_V1.0',
            'timeOfProcessing': datetime.now().strftime('%d-%b-%Y %H:%M:%S'),
            'missionId': 'planetiq',
            'trkStatus': 0,
            'leoID': int(sub(r'[A-Z]',r'',leoName)),
            'occID': 1,
            'conID': 'G',
            'occfreq1': f1_ro,
            'occfreq2': f2_ro,
            'Obs_type_L1': header_ro[l_ro[0]],
            'Obs_type_L2': header_ro[l_ro[1]],
            'setting': int(obj_leo_ro['ocd']),
            'refsatId': gnss_ref,
            'occsatId': gnss_ro,
            'year': dt_ros[0].year,
            'month': dt_ros[0].month,
            'day': dt_ros[0].day,
            'hour': dt_ros[0].hour,
            'minute': dt_ros[0].minute,
            'second': dt_ros[0].second,
            'dayOfYear': int(f'{dt_ros[0]:%j}'),
            'startTime': data_ro[0,0],
            'stopTime': data_ro[-1,0]+data_ro[-1,1],
            'center': 'NOAA/STAR, ESSIC/CISESS/UMD',
            'fileStamp': filestamp,
            'VERSION': f'Version {version}: Single Differencing, Corrected ECI Coordinates'
        })