from glob import glob
from matplotlib import pyplot as plt
from re import sub
import numpy as np
from datetime import datetime, timedelta
from tools import dt2float

gfiles = glob('gnssLrg/2025.310/*.npz')
gfile = gfiles[4]
tstart = datetime.strptime(sub(r'.*_s(\d+)_.*',r'\1',gfile),'%Y%j%H%M')
nfiles = [f'./gnssOrb/GNSS_{tstart+timedelta(days=tt):%Y.%j}_NRT.npz' for tt in range(3)]

with np.load(gfile, allow_pickle=True) as fid:
    GdataL = fid['Gdata'][120:-120]
    GdtL = fid['Gdt'][120:-120]
    GsatsL = fid['Gsats']

GdataN = np.empty((0,0,8))
GdtN = np.empty((0,))
GsatsN = np.empty((0,))
for nfile in nfiles:
    with np.load(nfile, allow_pickle=True) as fid:
        Gdata0 = fid['Gdata']
        Gdt0 = fid['Gdt']
        Gsats0 = fid['Gsats']
    tidx = np.logical_and(Gdt0 >= GdtL[0], Gdt0 <= GdtL[-1])
    sidx = np.isin(Gsats0, GsatsL)
    Gdt0 = Gdt0[tidx]
    Gsats0 = Gsats0[sidx]
    Gdata0 = Gdata0[np.ix_(tidx,sidx)]
    tidx = np.argsort(Gdt0)
    sidx = np.argsort(Gsats0)
    Gdt0 = Gdt0[tidx]
    Gsats0 = Gsats0[sidx]
    Gdata0 = Gdata0[np.ix_(tidx,sidx)]
    sidx = ~np.isin(Gsats0, GsatsN)
    GsatsN = np.hstack((GsatsN, Gsats0[sidx]))
    GdataN = np.hstack((GdataN, np.full((GdataN.shape[0],np.sum(sidx),GdataN.shape[2]),np.nan)))
    tidx = ~np.isin(Gdt0, GdtN)
    GdtN = np.hstack((GdtN, Gdt0[tidx]))
    GdataN = np.vstack((GdataN, np.full((np.sum(tidx),GdataN.shape[1],GdataN.shape[2]),np.nan)))
    tidx = np.argsort(GdtN)
    sidx = np.argsort(GsatsN)
    GdtN = GdtN[tidx]
    GsatsN = GsatsN[sidx]
    GdataN = GdataN[np.ix_(tidx,sidx)]
    GdataN[np.ix_(np.isin(GdtN, Gdt0),np.isin(GsatsN, Gsats0))] = Gdata0

nlidx = np.isin(GsatsN,GsatsL)
lnidx = np.isin(GsatsL,GsatsN)

xc = np.where(nlidx)[0]
yc = GdataL[:, lnidx, :] - GdataN[:, nlidx, :]
ey = GdataN[:, nlidx, :3]
ey /= np.sum(ey**2, axis=2)[:,:,None]**0.5
ep = np.cross(ey, GdataN[:,nlidx,4:7])
ep /= np.sum(ep**2, axis=2)[:,:,None]**0.5
er = np.cross(ep, ey)
yc = np.concatenate((np.sum((yc[:,:,:3]*er,yc[:,:,:3]*ep,yc[:,:,:3]*ey),axis=3),
                     yc[None,:,:,3],
                     np.sum((yc[:,:,4:7]*er,yc[:,:,4:7]*ep,yc[:,:,4:7]*ey),axis=3)),axis=0)

plt.close('all')
xyz = 'rpy'
clrs = np.array([[int(y['color'][x:x+2],16)/255 for x in range(1,7,2)] for y in plt.rcParams['axes.prop_cycle']])
fig, axs = plt.subplots(3, 1, figsize=(12, 9))

ys = yc.shape[1]//2
for ii in range(3):
    axs[0].plot(np.tile(xc, (2, 1)), [np.min(yc[ii,ys:], axis=0), np.max(yc[ii,ys:], axis=0)],
                color=(clrs[ii] + 2) / 3, linewidth=3)
    axs[2].plot(np.tile(xc, (2, 1)), [np.min(yc[ii + 4,ys:], axis=0), np.max(yc[ii + 4,ys:], axis=0)],
                color=(clrs[ii] + 2) / 3, linewidth=3)
axs[1].plot(np.tile(xc, (2, 1)), [np.nanmin(yc[3,ys:], axis=0), np.nanmax(yc[3,ys:], axis=0)],
                color=(clrs[0] + 2) / 3, linewidth=3)
for ii in range(3):
    axs[0].plot(np.tile(xc, (2, 1)), [np.min(yc[ii,:ys], axis=0), np.max(yc[ii,:ys], axis=0)],
                color=(clrs[ii+3] + 2) / 3, linewidth=3)
    axs[2].plot(np.tile(xc, (2, 1)), [np.min(yc[ii + 4,:ys], axis=0), np.max(yc[ii + 4,:ys], axis=0)],
                color=(clrs[ii+3] + 2) / 3, linewidth=3)
axs[1].plot(np.tile(xc, (2, 1)), [np.nanmin(yc[3,:ys], axis=0), np.nanmax(yc[3,:ys], axis=0)],
                color=(clrs[3] + 2) / 3, linewidth=3)
for ii in range(3):
    axs[0].plot(np.tile(xc, (2, 1)).T, np.transpose([np.min(yc[ii,ys:], axis=0), np.max(yc[ii,ys:], axis=0)]),
                color=(clrs[ii] + 1) / 2, linewidth=0.5)
    axs[2].plot(np.tile(xc, (2, 1)).T, np.transpose([np.min(yc[ii + 4,ys:], axis=0), np.max(yc[ii + 4,ys:], axis=0)]),
                color=(clrs[ii] + 1) / 2, linewidth=0.5)
axs[1].plot(np.tile(xc, (2, 1)).T, np.transpose([np.nanmin(yc[3,ys:], axis=0), np.nanmax(yc[3,ys:], axis=0)]),
                color=(clrs[0] + 1) / 2, linewidth=0.5)
for ii in range(3):
    axs[0].plot(np.tile(xc, (2, 1)).T, np.transpose([np.min(yc[ii,:ys], axis=0), np.max(yc[ii,:ys], axis=0)]),
                color=(clrs[ii+3] + 1) / 2, linewidth=0.5)
    axs[2].plot(np.tile(xc, (2, 1)).T, np.transpose([np.min(yc[ii + 4,:ys], axis=0), np.max(yc[ii + 4,:ys], axis=0)]),
                color=(clrs[ii+3] + 1) / 2, linewidth=0.5)
axs[1].plot(np.tile(xc, (2, 1)).T, np.transpose([np.nanmin(yc[3,:ys], axis=0), np.nanmax(yc[3,:ys], axis=0)]),
                color=(clrs[3] + 1) / 2, linewidth=0.5)
for ii in range(3):
    axs[0].plot(xc, np.mean(yc[ii,ys:], axis=0), color=clrs[ii], label=f'Predict {xyz[ii]}')
    axs[2].plot(xc, np.mean(yc[ii + 4,ys:], axis=0), color=clrs[ii])
axs[1].plot(xc, np.nanmean(yc[3,ys:], axis=0), '.-', color=clrs[0])
for ii in range(3):
    axs[0].plot(xc, np.mean(yc[ii,:ys], axis=0), color=clrs[ii+3], label=f'Past {xyz[ii]}')
    axs[2].plot(xc, np.mean(yc[ii + 4,:ys], axis=0), color=clrs[ii+3])
axs[1].plot(xc, np.nanmean(yc[3,:ys], axis=0), '.-', color=clrs[3])

axs[0].set_xlim((-0.5, len(nlidx) - 0.5))
axs[0].set_xticks(np.arange(len(nlidx)))
axs[0].set_xticklabels(GsatsN)
axs[0].set_ylim((-0.0008,0.0008))
axs[0].tick_params(axis='x', labelrotation=90, labelsize=8)
axs[0].set_ylabel('Position Distance (km)')
axs[0].legend(ncol=2, loc='upper right')
axs[0].set_title(sub(r".*/",r"Difference in ",gfile))
axs[0].grid(True)
axs[1].set_xlim((-0.5, len(nlidx) - 0.5))
axs[1].set_xticks(np.arange(len(nlidx)))
axs[1].set_xticklabels(GsatsN)
axs[1].set_ylim((-0.02,0.02))
axs[1].tick_params(axis='x', labelrotation=90, labelsize=8)
axs[1].set_ylabel('Clock Bias Difference (us)')
axs[1].grid(True)
axs[2].set_xlim((-0.5, len(nlidx) - 0.5))
axs[2].set_xticks(np.arange(len(nlidx)))
axs[2].set_xticklabels(GsatsN)
axs[2].set_ylim((-0.008,0.008))
axs[2].tick_params(axis='x', labelrotation=90, labelsize=8)
axs[2].set_ylabel('Velocity Distance (dm/s)')
axs[2].grid(True)
plt.savefig(sub(r".*/(.*)\.npz",r"\1.png",gfile),dpi=300)

gsat = 'E11'
sidx = np.searchsorted(GsatsN,gsat)
xc = GdtN
yc = yc[:,:,sidx]
zc = GdataN[:, nlidx, :][:,sidx,:7].T

plt.close('all')
xyz = 'rpy'
clrs = np.array([[int(y['color'][x:x+2],16)/255 for x in range(1,7,2)] for y in plt.rcParams['axes.prop_cycle']])
fig, axs = plt.subplots(6, 1, figsize=(12, 9))

axs[0].plot(dt2float(xc,tstart)/3600, zc[:3].T, '.-', markersize=1)
axs[0].set_xlim((0,48))
axs[0].set_xticks(np.arange(0,54,6))
axs[0].grid(True)
axs[0].set_ylabel('Position (km)')
axs[0].legend(['x','y','z'],ncol=3, loc='upper right')
axs[0].set_title(sub(r".*/(.*)",f"{gsat} Difference in \\1",gfile))
axs[1].plot(dt2float(xc,tstart)/3600, yc[:3].T, '.-', markersize=1)
axs[1].set_xlim((0,48))
axs[1].set_ylim((-0.0008,0.0008))
axs[1].grid(True)
axs[1].set_ylabel('Distance (km)')
axs[1].legend(['r','p','y'],ncol=3, loc='upper right')
axs[2].plot(dt2float(xc,tstart)/3600, zc[3], '.-', markersize=1)
axs[2].set_xlim((0,48))
axs[2].grid(True)
axs[2].set_ylabel('Clock Bias (us)')
axs[3].plot(dt2float(xc,tstart)/3600, yc[3], '.-', markersize=1)
axs[3].set_xlim((0,48))
axs[3].set_ylim((-0.02,0.02))
axs[3].grid(True)
axs[3].set_ylabel('Difference (us)')
axs[4].plot(dt2float(xc,tstart)/3600, zc[4:7].T, '.-', markersize=1)
axs[4].set_xlim((0,48))
axs[4].grid(True)
axs[4].set_ylabel('Velocity (dm/s)')
axs[5].plot(dt2float(xc,tstart)/3600, yc[4:7].T, '.', markersize=1)
axs[5].set_xlim((0,48))
axs[5].set_ylim((-0.008,0.008))
axs[5].grid(True)
axs[5].set_ylabel('Difference (dm/s)')
axs[5].set_xlabel(f'Hours since {tstart:%Y-%m-%d %H:%M}')
plt.savefig(sub(r".*/(.*)\.npz",f"\\1_{gsat}.png",gfile),dpi=300)