###########################################################
# POSPac2UMT.py
# Convert LLH from a POSPac output file into a format that can be used by the Spirent
# simulators to model the antenna motion, a UMT file.
#
# See also parseSpirent.pl to convert a Spirent truth file to POSPac format for
# processing with the error analysis scripts.
#
# Spirent Instructions
# --------------------
# Pipe the output from this script to a file with .umt extension.
# Create a new scenario on the Spirent with a 'simple motion' type vehicle.
# Copy the .umt file to the new scenario directory.
# Load the scenario into SimGen. Under the Vehicle -> Motion -> User motion file,
# select the .umt file.
# Run the simulation as normal.
#
# Useful Spirent Notes
# --------------------
# Always reboot the receiver when restarting the simulator. Stinger doesn't like time
# to go backwards.
# Always clear the 'satellite data' when switching between live sky and the simulator.
# Run first for ~15min without dynamics so the receiver can collect all the satellite
# almanacs etc.. This is one-shot operation.
# Disable iono/tropo (also in Stinger) for the best position accuracy.


import sys
import os
import math


###########################################################
# Script configuration class
# Decode command line input to configuration values

class cl_config :
  def __init__( self ) :
    # Dwell time at the start location [optional from command line]
    self.dwell_time =  0.0
    # Don't use data before this time [optional from command line]
    self.start_time = -1.0
    # Don't use data after this time [optional from command line]
    self.end_time   = -1.0
    # Output data rate [optional from command line]
    self.data_rate  = -1.0
    # Time tag adjust time to convert from POSPac time to simulation time [calc'ed]
    self.delta_time =  0.0
    # Input POSPac file name [from command line]
    self.fname      = ''

  def show_help( self ) :
    print('Expected usage:')
    print('  python POSPac2UMT.py <POSPac File> [-d<Init. Dwell] [-s<Start Time>] [-e<End Time>] [-r<Output rate>]')
    print('  Where:')
    print('    -d Optional initial dwell time [s] (Default = 0.0 sec)')
    print('       Simulates static position at the first valid location for this dwell time')
    print('    -s Optional start time for POSPac file (Default = start of data)')
    print('    -e Optional end time for POSPac file (Default = end of data)')
    print('    -r Optional output data rate [s] (Default = same rate as input)')

  def parse_cmd_line( self, sys_args ) :
    if ( len( sys_args ) < 2 ) :
      self.show_help()
      return( 0 )

    self.fname = sys_args[1]
    for x in range( 2, len( sys_args ) ) :
      cmdline = sys_args[x]

      if ( cmdline[0:2] == '-d' ) :
        config.dwell_time = float( cmdline[2:] )
      elif ( cmdline[0:2] == '-s' ) :
        config.start_time = float( cmdline[2:] )
      elif ( cmdline[0:2] == '-e' ) :
        config.end_time = float( cmdline[2:] )
      elif ( cmdline[0:2] == '-r' ) :
        config.data_rate = float( cmdline[2:] )
      else :
        self.show_help()
        return( 0 )

    # Successfully decoded all information on the command line
    return( 1 )


###########################################################
# Class to process a line from the POSPac ouptut file

class cl_line :
  def __init__( self ) :
    # POSPac data
    self.ttag = 0
    self.lat  = 0.0
    self.lon  = 0.0
    self.hei  = 0.0
    # Is the data valid for use? Within the start/end times etc..
    self.vld  = 0

  # Process a line of POSPac data
  def process( self, line, config ) :
    # Assume that the line is invalid by default
    self.vld  = 0

    # Check for empty and incomplete lines
    tmp = line.split( ' ' )
    if ( len(tmp) >= 3 ) :
      self.ttag = float( tmp[0] )
      self.lat  = float( tmp[1] )
      self.lon  = float( tmp[2] )
      self.hei  = float( tmp[3] )

      # Check the start/end times for valid data
      if ( ( config.start_time < 0.0 or self.ttag >= config.start_time )
       and ( config.end_time   < 0.0 or self.ttag <  config.end_time )
         ) :
        self.vld  = 1
    return

  # Try to output a line of data
  def output( self, delta_time ) :
    # Only print valid epochs
    # Include check of the user output rate control
    if ( self.vld
      and ( config.data_rate < 0.0
        or ( abs( (self.ttag % config.data_rate) - config.data_rate ) < 1e-6 )
          )
       ) :
      # Spirent UMT file format
      print "%.3f,motb,%.13f,%.13f,%.7f"    \
            % ( self.ttag + delta_time,     \
                self.lat * math.pi / 180.0, \
                self.lon * math.pi / 180.0, \
                self.hei                    \
              )


###########################################################
# Main scipt proceedure

# Configure the script from the command line inputs
config = cl_config()
if ( config.parse_cmd_line( sys.argv ) == 0 ) :
  sys.exit()

# Open the POSPac file
if ( not( os.access( config.fname, os.F_OK ) ) ) :
  print 'Error opening POSPac file ' + config.fname
  sys.exit()
fptr = open( config.fname, 'rt' )

# Spirent UMT file header
print 'INTERP_VEL, INTERP_ACC, INTERP_JERK, INTERP_HEAD, INTERP_ANG_RATE, INTERP_ANG_ACC'

# Storage objects for POSPac data
first  = cl_line()
latest = cl_line()

# Read line from POSPac file. Ignore for empty lines.
line = fptr.readline()
while( len( line ) > 0 ) :
  if ( len( line ) > 1 ) :
    # Process the line of POSPac data.
    latest.process( line, config )

    # Check for the first valid epoch of data.
    if ( latest.vld != 0 and first.vld == 0 ) :
      # Make copy of the first valid epoch of POSPac data and compute offset between
      # POSPac and simulation time
      first = latest
      first.output( 0.0 - first.ttag )
      config.delta_time = config.dwell_time - first.ttag

    # Output data in Spirent UMT format, if valid.
    latest.output( config.delta_time )

  # Read the next line from POSPac file.
  line = fptr.readline()

# Close the POSPac file
fptr.close()