#!/usr/bin/env python

from mutils import *
import os
import argparse

prog_desc="""\
Analyze GLN biases in residuals.txt to generate a new
GLONASS_FDMA_Corr[] in pf_fix.c.

Steps:
 1- Log 24+ hours of data from a known location
 2- IMPORTANT: compile StingerNavPC with GLONASS_FDMA_Corr[] set to
    all 0.
 3- Run StingerNavPC with the following options:
    --static_truth=...
    --no_truth_1sv
 4- ./gln_fdma_bias.py residuals.txt
 5- Make sure the GPS residuals look small.  Then update the
    GLONASS_FDMA_Corr[] table in Stinger."""

parser = argparse.ArgumentParser(description=prog_desc,
                                 formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('filename', help='residuals file name')
parser.add_argument('-p','--plot', help='Plot results',action="store_true")
parser.add_argument('-e','--elev', help='Elevation angle limit')

args = parser.parse_args()

el_mask = 20.
if args.elev:
    el_mask = float(args.elev)

print('WARNING - be sure StingerNavPC was run with GLONASS_FDMA_Corr[] set to all 0')
if not 'd' in globals():
    filename = args.filename
    info = os.stat(filename)
    if info.st_size < 1e9:
        print("Loading: " + filename)
        d=doload(filename)
    else:
        reduced_filename = filename + ".reduced"
        if not os.path.isfile(reduced_filename):
            print("Generating: " + reduced_filename)
            with open(reduced_filename,"w") as fout, open(filename,"r") as fin:
                for line in fin:
                    words = line.split()
                    words = [words[i] for i in (1,2,3,7,16,23)]
                    if float(words[0]).is_integer():
                        fout.write( ' '.join(words) + "\n" )
        print("Loading: " + reduced_filename)
        d = doload(reduced_filename)
print('Done loading')

k=dotdict({})
if d.shape[1] == 6:
    k.TIME=0
    k.SV=1
    k.SAT_TYPE=2
    k.RANGE_RES=3
    k.EL=4
    k.FDMA=5
else:
    k.TIME=1
    k.SV=2
    k.SAT_TYPE=3
    k.RANGE_RES=7
    k.EL=16
    k.FDMA=23

# do GPS first as a sanity check
i = find( d[:,k.SAT_TYPE]==RcvrConst.SAT_TYPE_GPS )
gps_data = []
for sv in unique(d[i,k.SV]):
    i2 = i[find( d[i,k.SV]==sv )]
    gps_data.append( [sv, 0, len(i2), median(d[i2,k.RANGE_RES]), mean(d[i2,k.RANGE_RES])] )
gps_data = array(gps_data)
k2=dotdict({})
k2.SV=0
k2.FDMA=1
k2.LEN=2
k2.MEDIAN=3
k2.MEAN=4

gps_data[:,k2.MEDIAN] -= mean(gps_data[:,k2.MEDIAN])
gps_data[:,k2.MEAN] -= mean(gps_data[:,k2.MEAN])

if args.plot:
    figure()
    plot( gps_data[:,k2.SV], gps_data[:,k2.MEDIAN]-mean(gps_data[:,k2.MEDIAN]), '.-' )
    plot( gps_data[:,k2.SV], gps_data[:,k2.MEAN]-mean(gps_data[:,k2.MEAN]), '.-' )
    xlabel('SV')
    ylabel('bias [m]')
    grid()
    title('GPS')

print('GPS SV, median, mean:')
for x in gps_data:
    print('  %2d %5.2f %5.2f' % (x[k2.SV],x[k2.MEDIAN],x[k2.MEAN]))

# do GLN
i = find( d[:,k.SAT_TYPE]==RcvrConst.SAT_TYPE_GLONASS )
gln_data = []
for fdma in unique(d[i,k.FDMA]):
    i2 = i[find( d[i,k.FDMA] == fdma )]
    for sv in unique(d[i2,k.SV]):
        i3 = i2[find( d[i2,k.SV] == sv )]
        gln_data.append( [sv, fdma, len(i3), median(d[i3,k.RANGE_RES]), mean(d[i3,k.RANGE_RES])] )
        if not all(d[i3,k.FDMA]==fdma): print('error fdma')
        if not all(d[i3,k.SV]==sv): print('error sv')
gln_data = array(gln_data)
gln_data[:,k2.MEDIAN] -= mean(gln_data[:,k2.MEDIAN])
gln_data[:,k2.MEAN] -= mean(gln_data[:,k2.MEAN])

if args.plot:
    fig,(ax0,ax1) = subplots(2,1,sharex=True)
    ax0.set_title('GLN')
    ax0.plot( gln_data[:,k2.FDMA], gln_data[:,k2.MEDIAN]-mean(gln_data[:,k2.MEDIAN]), '.' )
    ax0.plot( gln_data[:,k2.FDMA], gln_data[:,k2.MEAN]-mean(gln_data[:,k2.MEAN]), '.' )
    ax0.grid()
    ax0.set_ylabel('bias [m]')
    ax1.plot( gln_data[:,k2.FDMA], gln_data[:,k2.SV], '.' )
    ax1.grid()
    ax1.set_ylabel('SV ID')
    ax1.set_xlabel('FDMA')

print('GLN SV, FDMA, median, mean:')
for x in gln_data:
    print('  %2d %2d %4.1f %4.1f' % (x[k2.SV],x[k2.FDMA],x[k2.MEDIAN],x[k2.MEAN]))

print('DBL const GLONASS_FDMA_Corr[] = {')
for fdma in range(-7,7):
    i = find( gln_data[:,k2.FDMA] == fdma )
    if len(i) == 0:
        val = nan
    else:
        val = mean(gln_data[i,k2.MEDIAN])
    print('  %4.1f, // %d' % (val,fdma))
print('} ;')

if args.plot:
    show()