#!/usr/bin/env python

usage="""\
Convert POSPac data from Sunnyvale van runs to Titan truth data.
The Titan command output can be used with the TitanPlayer option:
 -command titan_ref_pos.txt

Example:
  ./pospac_to_titan_cmd.py -w 2100 pospac.txt titan_ref_pos.txt
"""

from mutils import conv_lla_ecef
import argparse
import sys
import os

parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter,description=usage)
parser.add_argument('-w','--week',
                    help='GPS week # for data',
                    type=int, required=True)
parser.add_argument('-m','--msecs',
                    help='Output truth data every X msecs',
                    # Should this be 100msecs or 200msecs?
                    default=100,
                    type=int)
parser.add_argument('--min_t',
                    help='minimum GPS time tag [s]',
                    default=-1.,
                    type=float)
parser.add_argument('--max_t',
                    help='maximum GPS time tag [s]',
                    default=1e9,
                    type=float)
parser.add_argument('input',
                    help='Input POSPac filename')
parser.add_argument('output',
                    help='Output Titan command filename')
args = parser.parse_args()

if os.path.isfile(args.output):
    print("Remove output file first")
    sys.exit(1)

out = open(args.output,'w')
for line in open(args.input,'r'):
    w=line.split()
    if len(w) < 5:
        continue
    t = float(w[0])
    if round((t*1000)%args.msecs) != 0:
        continue
    if t < args.min_t or t > args.max_t:
        continue

    X,Y,Z = conv_lla_ecef( float(w[1]), float(w[2]), float(w[3]) )
    out.write('GPS %4d     %6.5f $PTNL,TITAN,COO,ROV,GPS, %4d ,     %6.5f ,C,   %.4f ,      %.4f ,     %.4f ,V,     %.5f ,     %.5f ,     %.4f\n' %
              (args.week, # GPS week
               t-1, # GPS sec-1 by convention?
               args.week, # GPS week
               t, # GPS sec
               X, # X
               Y, # Y
               Z, # Z
               0.0004, # X variance?
               0.0004, # Y variance?
               0.0004  # Z variance?
              ) )