#!/usr/bin/env python
import os, sys

def print_usage():
    print("""**********************************************************************
Usage: ./cmp_nopi_titan_MP.py nopi_path plot_mode
 where:
  nopi_path = is the path to NoPi results directory
  plot_mode = 0 -> display interactively on screen, 1 -> save to file
NOTE: this loads Titan HUD files from the current working directory.

Description:
 Compare Titan estimated multipath to NoPi double-diffs.  It's always
 good to run Titan in -fixedRovPos mode first.

 Currently only looks at GPS L1 code and carrier.

 The Titan code multipath(MP) states should contain all MP so they
 are easy to compare to NoPi results.

 Unfortunately the carrier MP gets shared between the Titan carrier MP
 and Titan AMB states.  Thus, it is helpful to show how much carrier
 MP was picked up by the Titan MP state vs the Titan AMB state.  Doing
 this is a bit tricky, though, because all AMB states also include a
 slowly-drifting clock term.
**********************************************************************""")

if len(sys.argv) != 3:
    print_usage()
    sys.exit(0)

from pylab import *
from mutils import *

nopi_path = sys.argv[1]
if int(sys.argv[2]) > 0:
    print('saving plots')
    do_save = True
else:
    print('displaying plots to screen')
    do_save = False

close('all')

if not 'rdr' in globals():
    rdr = TitanHudPKFEE('.')
    savename = os.path.basename(os.getcwd())
    d = doload( nopi_path + '/diffs_combo_0.mtb')

d_t = 0
d_sv = 1
d_code = 6
d_carr = 7

sv_list1 = unique(d[:,d_sv])
sv_list2 = rdr.get_amb_svs()
sv_list = intersect( sv_list1, sv_list2 )[0]

figure()
for sv in sv_list:
    a, b, _ = rdr.get_L1_amb(sv)
    plot( a, b )
title('All PKFEE reduced L1 phase ambiguities')
xlabel('GPS time [s]')
ylabel('AMB [cyc]')
if do_save:
    savefig('all_amb_%s.png' % (savename))
else:
    show()

for sv in sv_list:
    cde_t, cde_val, cde_std, _ = rdr.get_L1_cde_MP(sv)
    phs_t, phs_val, phs_std = rdr.get_L1_phs_MP(sv)
    phs_amb_t, phs_amb, _ = rdr.get_L1_phs_MP_plus_amb(sv)
    phs_res_t, phs_res, _ = rdr.get_L1_phs_res(sv)
    cde_res_t, cde_res, _ = rdr.get_L1_cde_res(sv)

    i = find(d[:,d_sv]==sv)

    figure(figsize=(10,8))
    ax=subplot(411)
    plot(d[i,d_t]*.001, d[i,d_code], label='NoPi')
    plot(cde_t, cde_val, label='Titan')
    title('sv %d: code MP' % sv)
    legend(loc='best',prop={'size':8})
    ylabel('code err [m]')
    grid('on')

    subplot(412,sharex=ax)
    plot(d[i,d_t]*.001, d[i,d_code], label='NoPi')
    plot(cde_res_t, cde_res, label='Titan')
    title('code res')
    legend(loc='best',prop={'size':8})
    ylabel('code err [m]')
    grid('on')

    subplot(413,sharex=ax)
    plot(d[i,d_t]*.001, d[i,d_carr]*-L1_WAVELENGTH, label='NoPi')
    plot(phs_t, phs_val, label='Titan MP')
    # Also show what happens when we add AMB state offset, because
    # the AMB states absorb some of the slowly-changing multipath.
    plot(phs_amb_t, phs_amb, label='Titan MP+Amb')
    title('phase MP')
    legend(loc='best',prop={'size':8})
    ylabel('phase err [m]')
    grid('on')

    subplot(414,sharex=ax)
    plot(d[i,d_t]*.001, d[i,d_carr]*-L1_WAVELENGTH, label='NoPi')
    plot(phs_res_t, phs_res, label='Titan')
    title('phase res')
    legend(loc='best',prop={'size':8})
    ylabel('phase err [m]')
    grid('on')
    xlabel('GPS time [s]')
    tight_layout()
    if do_save:
        savefig('sv%d_%s.png' % (sv,savename))
    else:
        show()
        raw_input('Enter to continue')