#!/usr/bin/env python
usage = """
Usage:
 python plot_rtk_carr.py 903
where "903" is the anti-jam regression run number.
Creates gps/gln_rtk_run_903.png and gps/gln_carr_run_903.png.

This is a temporary script to analyze RTK performance and
cycle slips. It will eventually go away.
"""
import argparse

parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter,
                                 description=usage)
parser.add_argument('run_num', help='anti-jam run number on fermion', type=int)
parser.add_argument('-i','--interactive', help='Interactive plots? (otherwise just store PNGs)', action="store_true")
parser.add_argument('-r','--ref', help='Different ref (e.g., RX0-901)')
args = parser.parse_args()

# Allow running headless from the command line
if not args.interactive:
    import matplotlib
    matplotlib.use("agg")

from mutils import *
import sys
import os

base_dir="/net/fermion/mnt/data_drive/rxu/AntiJam/Web2/GPSTools/Antijamtest/server_main/DataDir/"
t_start = 432060
t_rtk_start = t_start+60

def plot_err(ax,d,ref_llh):
    err_3d = comp_3d_dist( d[:,d.k.LAT:d.k.LAT+3], ref_llh )
    sigma_3d = sqrt( d.SigU**2 + d.SigE**2 + d.SigN**2 )
    ax.plot( d.TIME - t_start, err_3d, '.-', label='err' )
    if len(find(sigma_3d>1e-3)) > 0:
        ax.plot( d.TIME - t_start, 3*sigma_3d, label='3σ', alpha=.5 )
        ax.legend()

close('all')

######################################################################
# Plot RTK results
if args.ref:
    ref_filename = base_dir+"%s/*.T04"%args.ref
    ref_txt = args.ref
else:
    ref_filename = base_dir+"RX0-%d/*.T04"%args.run_num
    ref_txt = 'Sept'
d1=vd2cls(ref_filename,'-d35:2')
d1=d1[d1.TIME>=t_rtk_start]
d2=vd2cls(base_dir+"RX1-%d/*.T04"%args.run_num,'-d35:2')
d2=d2[d2.TIME>=t_rtk_start]
ref_llh = median(d2[:,[d2.k.LAT,d2.k.LON,d2.k.HGT]],axis=0)
expected_ref_llh = r_[37 + 23/60., -(122), 0.]
if sqrt(sum((ref_llh-expected_ref_llh)**2)) > .05:
    print("WARNING: Unexpected reference pos!")
    print(" expected",expected_ref_llh)
    print("      got",ref_llh)

fig,ax=subplots(2,1,sharex=True,sharey=True)
plot_err(ax[0],d1,ref_llh)
ax[0].set_ylabel('%s 3D err [m]'%ref_txt)
plot_err(ax[1],d2,ref_llh)
ax[1].set_ylabel('Auger 3D err [m]')
ax[1].set_xlabel('secs since %d'%t_start)
ax[0].set_title("run %d. Ref LLH %.5f %.5f %.3f"%(args.run_num,*ref_llh))
for a in ax: a.grid()
savefig('rtk_run_%d.png'%args.run_num)

######################################################################
# Plot ant1-ant0 GPS L1CA carrier diff

d2=vd2cls(base_dir+"RX1-%d/*.T04"%args.run_num,'-d35:19 -t -s%d'%t_start)
d20=d2[(d2.ANTENNA==0)&(d2.CSTATE>=16)]
d21=d2[(d2.ANTENNA==1)&(d2.CSTATE>=16)]
d1=vd2cls(ref_filename,'-d35:19 -t -s%d'%t_start)
d10=d1[(d1.ANTENNA==0)&(d1.CSTATE>=16)]
d11=d1[(d1.ANTENNA==1)&(d1.CSTATE>=16)]

def plot_dph( ax, da, db ):
    dph = db.PHASE - da.PHASE
    i = find(   (diff(da.SLIP_CNT)!=0)
              | (diff(db.SLIP_CNT)!=0)
             )
    i_rng = concatenate(([-1], i, [len(dph)]))
    for i0,i1 in zip(i_rng[:-1],roll(i_rng,-1)[:-1]):
        dph[i0+1:i1+1] -= around(median(dph[i0+1:i1+1]))
    dph -= median(dph)
    ax.plot( da.TIME - t_start, dph )
    if len(i) > 0:
        ax.plot( da.TIME[i] - t_start, dph[i], 'rx' )
    return len(i)

def do_carr_run( target_sat_type, target_txt ):
    fig,ax=subplots(4,1,sharex=True,figsize=(8,6))
    n_slips = r_[0,0]
    n_total = r_[0,0]
    for sv,sat_type in d2.get_sv_list():
        if sat_type!=target_sat_type:
            continue
        d1a,d1b = sv_intersect( d10, d11, sv=(sv,sat_type,0,0) )
        d2a,d2b = sv_intersect( d20, d21, sv=(sv,sat_type,0,0) )
        if len(d2a) < 10:
            continue
        n_total[0] += len(d1b)
        n_slips[0] += plot_dph( ax[0], d1a, d1b )
        ax[0].set_ylabel('%s Δph[cyc]'%ref_txt)
        ax[0].set_ylim([-1,1])

        n_total[1] += len(d1b)
        n_slips[1] += plot_dph( ax[1], d2a, d2b )
        ax[1].set_ylabel('Auger Δph[cyc]')
        ax[1].set_ylim([-1,1])
        ax[0].get_shared_y_axes().join(ax[0], ax[1])

        dcno = d1b.CNO-d1a.CNO
        if len(dcno) > 0 and max(dcno) > max(-dcno):
            ax[2].set_ylabel('%s -ΔSNR[dB]'%ref_txt)
            dcno *= -1
        else:
            ax[2].set_ylabel('%s ΔSNR[dB]'%ref_txt)
        ax[2].plot( d1a.TIME - t_start, dcno )
        dcno = d2b.CNO-d2a.CNO
        ax[3].plot( d2a.TIME - t_start, dcno )
        ax[3].set_ylabel('Auger ΔSNR[dB]')
        ax[3].set_xlabel('secs since %d'%t_start)
        ax[3].get_shared_y_axes().join(ax[2], ax[3])

    ax[0].set_title("%s run %d antenna1-antenna0\n%s total %d slips %d. Auger total %d slips %d"%
                    (target_txt,args.run_num,ref_txt,
                     n_total[0],n_slips[0],
                     n_total[1],n_slips[1]))

    for a in ax: a.grid()
    fig.tight_layout()
    fig.savefig(target_txt+'carr_run_%d.png'%args.run_num)

do_carr_run( 0, 'gps' )
do_carr_run( 2, 'gln' )

if args.interactive:
    show()