#!/usr/bin/env python
#  Copyright 2017 Trimble Inc.
#
# Note - need following executables:
#  - TitanPlayer(+DebugOut.json & System.json) & GVRS
#    - see /mnt/data_drive/DataDump/tools/Titan/2016_09_13/
#  - viewdat
#  - dat2t01 (only when running StingerNavPC in StingerRTX mode)
#  - StingerNavPC
#
# Calculate StingerNavPC residuals:
#  1- copy/download GVRS data
#  2- run Titan
#  3- generate RefClock.txt from Titan
#  4- Run StingerNavPC to generate residuals
#

from __future__ import print_function
import subprocess
import shutil
import datetime
import os
import platform
import argparse
from getGVRSdata import getGVRSdata, weeksecondstotime
from mutils import conv_lla_ecef, gen_titan_ref_clock, vd2arr, get_kv_info, doload, find, median

######################################################################
# Parse arguments
parser = argparse.ArgumentParser(description='Run Titan to get a clock estimate, then run SNPC to get residuals.')
parser.add_argument('filename', help='T04 filename - helpful if "Log Diagnostics" is enabled')
parser.add_argument('-a','--antenna', help='Antenna ID, e.g. -a 265')
parser.add_argument('-t','--truth_lla', help='Truth lat,lon,hgt, e.g. -t 37.3848990528,-122.0054260639,-6.260')
parser.add_argument('-p','--pospac',
                    help='use POSPac.txt file as truth position',
                    action="store_true")
parser.add_argument('-o','--overwrite',
                    help='Overwrite all intermediate files?  Otherwise use existing files.',
                    action="store_true")
parser.add_argument('-r','--rtx',
                    help='Run Stinger in RTX mode',
                    action="store_true")
parser.add_argument('-g','--gvrs',
                    help='Use GVRS data logged in "filename" to run StingerNavPC in StingerRTX mode.  Forces --rtx',
                    action="store_true")
args = parser.parse_args()

filename = args.filename
ant_ID = None
lla_str = None
d = None
if args.antenna:
    ant_ID = int(args.antenna)
else:
    ant_ID = int(get_kv_info( filename, 'AntennaType' ))
if args.truth_lla:
    lla_str = args.truth_lla
elif args.pospac:
    print("Will use median of POSPac.txt file for GVRS base position")
    d,k = vd2arr(filename,opt=['--dec=200'])
    pospac = doload('POSPac.txt')
    i = find( (pospac[:,0]>=d[0,k.TIME]) & (pospac[:,0]<=d[-1,k.TIME]) )
    lla_str = '%.9f,%.9f,%.3f' % (median(pospac[i,1]),median(pospac[i,2]),median(pospac[i,3]))
else:
    print("Will use rec16 key-value truth position from T04")
    lla_str = get_kv_info( filename, 'RefStationLLH' )

lla_truth= [float(x) for x in lla_str.split(',')]

print("Antenna ID %d" % ant_ID)
if args.pospac:
    print("Base GVRS LLA %s" % lla_str)
else:
    print("Truth LLA %s" % lla_str)
if not args.overwrite:
    print("Will use existing files in directory")
else:
    print("NOTE: Will overwrite any existing files")

if(platform.system() != "Windows"):
    sim = "wine "
else:
    sim = ""

# Make sure cmd completes successfully
def check_run(cmd):
    print("Running %s" % cmd)
    subprocess.check_call(cmd,shell=True)

# Some programs always seems to return an error code, so ignore
# result..
def nocheck_run(cmd):
    print("Running %s" % cmd)
    subprocess.call(cmd,shell=True)

x,y,z = conv_lla_ecef( lla_truth[0], lla_truth[1], lla_truth[2] )
xyz_str = "%.3f %.3f %.3f" % (x,y,z)

######################################################################
# Get GVRS data

if os.path.isfile("RTXObs-GVRS.DAT") and not args.overwrite:
    print("Use existing RTXObs-GVRS.DAT")
else:
    if args.gvrs:
        print("Generate RTXObs-GVRS.DAT from local file")
        dest_file = open("RTXObs.T02",'wb')
        shutil.copyfileobj(open(filename,'rb'), dest_file)
        dest_file.close()
    else:
        print("Generate RTXObs-GVRS.DAT from remotely logged data")
        if d is None:
            d,k = vd2arr(filename,opt=['--dec=200'])
        tstart = weeksecondstotime( d[0,k.WEEK], d[0,k.TIME] )
        tend = weeksecondstotime( d[-1,k.WEEK], d[-1,k.TIME] )
        getGVRSdata( tstart, tend, lla_truth[0], lla_truth[1] )

        dest_file = open("RTXObs.T02",'wb')
        shutil.copyfileobj(open(filename,'rb'), dest_file)
        shutil.copyfileobj(open("CMRx.T04",'rb'), dest_file)
        dest_file.close()

    nocheck_run(sim + "GVRS -gvrs %s %s > log_gvrs.txt" % ("RTXObs.T02",xyz_str) )
    if os.stat("RTXObs-GVRS.DAT").st_size < 100:
        print("GVRS didn't generate a good file")
        raise SystemExit(1)

if os.path.isfile("RTXObs-GVRS.T02") and not args.overwrite:
    print("Use existing RTXObs-GVRS.T02")
else:
    if args.gvrs or args.rtx:
        check_run("dat2t01 RTXObs-GVRS.DAT RTXObs-GVRS.T02")

######################################################################
# Run Titan

if os.path.isfile("PKFEE_States_RcvClkBias.hud") and not args.overwrite:
    print("Using existing PKFEE_States_RcvClkBias.hud")
else:
    fout = open('cfg.txt','w')
    if not args.pospac:
        print("""
        -fixedRovPos
        -rovPosXYZ %s
        """ % xyz_str,file=fout)
    print("""
    -inputRovFiles %s
    -inputRefFiles RTXObs-GVRS.DAT
    -numGNSS 1
    -useGPS
    -useGLO
    -useGAL
    -useBEI

    -refPosXYZ %s
    -rovAntID %d
    -refAntID 255

    -registryFileType Gnss
    -registryFiles System.json DebugOut.json
    """ % (filename, xyz_str, ant_ID),file=fout)
    fout.close()

    nocheck_run(sim + "TitanPlayer -CMDInputFile cfg.txt > log_titan.txt")
    if os.stat("TitanPlayer_NavSoln.hud").st_size < 1000:
        print("TitanPlayer didn't generate a good file")
        raise SystemExit(1)

######################################################################
# Generate RefClock.txt from Titan

if os.path.isfile("RefClock.txt") and not args.overwrite:
    print("Using existing RefClock.txt")
else:
    print("Generating RefClock.txt")
    if d == None:
        d,k = vd2arr(filename,opt=['--dec=200'])
    gen_titan_ref_clock('.', d, k )

######################################################################
# Run StingerNavPC

if os.path.isfile("residuals.txt") and not args.overwrite:
    print("Not overwriting residuals.txt")
else:
    if args.pospac:
        truth_str = "--truth"
    else:
        truth_str = "--static_truth=%s" % lla_str
    if args.gvrs or args.rtx:
        check_run("StingerNavPC %s -d99 -e0 --rcvr_cc --gvrs_enable -tRTXObs-GVRS.T02 "
                  "--base_position_lla=%s %s" % (filename,lla_str,truth_str))
    else:
        check_run("StingerNavPC %s -d99 -e0 -j -w --rcvr_cc %s" % (filename,truth_str))