#!/usr/bin/env python
# Script to process the input file in StingerNavPC in StingerRTX (aka GVBS) mode
#
# 1. Runs viewdat on the input file and exports the available PVT
#    solution
# 2. Computes the average position from the file, this will be used as
#    the reference position for the GVRS module
# 3. Finds the start/stop times, this will be used to download the
#    appropriate files with GVRS data
# 4. Using HTTP protocol downloads the GVRS files from the server
# 5. Using the average position from step #2 and the data from #4 runs
#    the GVRS module. This creates a DAT file containing synthetic
#    observables for the location in #2
# 6. Convert the DAT file to T02
# 7. Run StingerNavPC with the file provided on the command line and the
#    GVRS data from steps #5 & #6 (the T02 format file) as a source of
#    correction
# 8. Generates a KMZ file from the StingerNavPC Kalman filter output
#
# ToDo:
#  - Limited error checking
#  - May need tweaks on Linux, especially to get Wine to work
#  - As the correction files are from the live beam logged in Sunnyvale
#    it only contains corrections for the US.
#  
#  Copyright 2016-2018 Trimble Inc.
#
# You need the following tools in your path:
#   viewdat, t0x2t0x, gvrs.exe, dat2t01, StingerNavPC, t012kml
# gvrs.exe can be found in:
#   /net/quark/mnt/data_drive/DataDump/tools/GVRS/

import os
import sys
import uuid
import math
import urllib
import platform
import datetime
import shutil
import subprocess
from getGVRSdata import getGVRSdata,weeksecondstotime

def check_run(cmd):
    subprocess.check_call(cmd,shell=True)

# When true instead of running SNPC on windows runs on bash under
# Windows (only relevant if we are running on Windows). As bash under
# windows provides GCC for free while Visual Studio requires a license
# this is a way to compile locally SNPC without needing a VS license
runOnWindowsBash = False

def LLH2XYZ(Lat,Lon,Hgt):
  # Constants
  A_WGS84    = 6378137.0 # Earth Semi-Major Axis (m) 
  E2_WGS84   = 6.69437999013e-3
  ONE_MIN_E2 = 0.99330562000987
  
  # Convert to Radians from degrees
  Lat = Lat * math.pi / 180.0
  Lon = Lon * math.pi / 180.0

  sinlat = math.sin(Lat)
  coslat = math.cos(Lat)

  sinlon = math.sin(Lon)
  coslon = math.cos(Lon)

  Rn  = A_WGS84 / math.sqrt(1.0 - (E2_WGS84 * sinlat * sinlat))
  tmp = (Rn + Hgt) * coslat
  X = tmp * coslon
  Y = tmp * sinlon
  Z = (Rn * ONE_MIN_E2 + Hgt) * sinlat

  return X, Y, Z


def processStingerRTX(filename, prefix, runOnWindowsBash, use_existing_cmrx):
  # Set the forward/back slash based on the OS
  if(platform.system() == "Windows"):
    sep = "\\"
    delete = "del"
    move = "ren"
  else:
    # Linux
    sep = "/"
    delete = "rm"
    move = "mv"
    # we're already running under Linux
    runOnWindowsBash = False

  extFilename = os.getcwd() + sep + filename

  if not os.path.isfile(extFilename):
    print("File does not exist\n")
    exit(2)

  check_run("viewdat -d35:2 -mb -h " + filename + " > " + prefix + ".txt")

  LatSum = 0.0
  LonSum = 0.0
  HgtSum = 0.0
  Num = 0

  processedFirst = False
  for line in open(prefix + ".txt",'r').readlines():
    line = line.strip()
    tokens = line.split()

    if(not line.startswith("%") and len(tokens) >= 73 ):
      if(processedFirst == False):
        # By removing 10 minutes we move the time backwards, this allows
        # us to make sure we have corrections available at the beginning
        # file under test
        timeStart = weeksecondstotime(int(tokens[0]),float(tokens[1])-(10.0*60.0))
        processedFirst = True
      LatSum = LatSum + float(tokens[12])
      LonSum = LonSum + float(tokens[13])
      HgtSum = HgtSum + float(tokens[14])
      Num = Num + 1

  # 10 minutes is to make sure we have corrections beyond the end of 
  # processing.
  timeStop = weeksecondstotime(int(tokens[0]),float(tokens[1])+(10.0*60.0))

  # Compute the average Lat/Lon/Hgt - we'll use this as the reference location for the GVRS generator
  LatAv = LatSum / float(Num)
  LonAv = LonSum / float(Num)
  HgtAv = HgtSum / float(Num)

  print("Av LLH: %.10f %.10f %.3f" % (LatAv, LonAv, HgtAv))
  X, Y, Z = LLH2XYZ(LatAv,LonAv,HgtAv)
  print("Av XYZ: %.4f %.4f %.4f" % (X, Y, Z))

  # Delete the viewdat output
  os.system(delete + " " + prefix + ".txt")

  if not use_existing_cmrx:
    # Now download the files we need from the server, concatenate into
    # "CMRx.T04" we'll use this file to feed the GVRS correction generator
    # This files have logged CMRx
    getGVRSdata( timeStart, timeStop, LatAv, LonAv )

    # Add the CMRx data to the original file, this way the GVRS processor
    # will generate observables for the satellites visible at the location
    # we are interested in
    destination = open('RTXObs.T04','wb')
    shutil.copyfileobj(open(filename,'rb'), destination)
    shutil.copyfileobj(open('CMRx.T04','rb'), destination)
    destination.close()
    # Convert from T04 to T02 (needed by GVRS)
    check_run("t0x2t0x RTXObs.T04 RTXObs.T02")
  else:
    # Make sure in T02 format (needed by GVRS)
    check_run("t0x2t0x %s RTXObs.T02"%filename)

  # Run the GVRS correction generator
  os.system("GVRS.exe -gvrs \"RTXObs.T02\" " + str(X) + " " + str(Y) + " " + str(Z) + " > log.txt")

  # GVRS takes Input.T02 -> Input-GVRS.DAT
  # Convert the DAT file to a T02 (gvrs.T02)
  check_run("dat2t01 RTXObs-GVRS.DAT gvrs.T02")
  # Don't need this anymore
  os.system(delete + " CMRx.T04")
  os.system(delete + " RTXObs-GVRS.DAT")

  # Run SNPC
  if(platform.system() == "Windows" and runOnWindowsBash):
    filename = filename.replace("\\", "/")
    check_run("bash -c '~/bin/StingerNavPC "  + filename + " --gvrs_enable -tgvrs.T02 --base_position_lla=" + str(LatAv) + "," + str(LonAv) + "," + str(HgtAv) + "'")
  else:
    check_run("StingerNavPC "  + filename + " --gvrs_enable -tgvrs.T02 --base_position_lla=" + str(LatAv) + "," + str(LonAv) + "," + str(HgtAv) )

  # Covert to KMZ, rename the PVT file and KMZ 
  check_run("t012kml -oStingerRTX navKAL.t04 StingerRTX.kmz")
  os.system(move + " StingerRTX.kmz " + prefix + "-StingerRTX.kmz")
  check_run("viewdat -d35:2 -mb -h navAPP.t04  > " + prefix + ".txt")
  os.system(move + " navAPP.t04 " + prefix + "-PVT.T04")
  os.system(move + " gvrs.T02 " + prefix + "-gvrs.T02")
  # The following also includes WAAS
  os.system(move + " RTXObs.T02 " + prefix + "-CMRxData.T02")
  print("\n\nTo rerun\n")
  print("StingerNavPC "  + filename + " --gvrs_enable -t" + prefix +"-gvrs.T02 --base_position_lla=" + str(LatAv) + "," + str(LonAv) + "," + str(HgtAv) + "\n")
  print("In the US for WAAS\n")
  print("StingerNavPC "  + filename + " -w" + prefix +"-CMRxData.T02\n")
  if(platform.system() == "Windows" and runOnWindowsBash):
    print("Under Bash on windows")
    print("bash -c '~/source/StingerNavPC/NavPC/StingerNavPC "  + filename + " -w" + prefix +"-CMRxData.T02\n")

#######################################################
# Start of script
#######################################################
if __name__ == "__main__":
  import argparse

  parser = argparse.ArgumentParser(description='Run SNPC in GVRS mode on T0x file')
  parser.add_argument('filename', help='T0x filename')
  parser.add_argument('-e', '--existing', help='Use existing logged CMRx data',action="store_true")
  args = parser.parse_args()

  filename = args.filename

  # Add a unique file prefix to all output names. Change
  # this if you want something different prefixed.
  prefix = str(uuid.uuid4())
  # Do all the work
  processStingerRTX(filename, prefix, runOnWindowsBash, args.existing)