#!/usr/bin/env python

usage="""\
Convert POSPac data from Sunnyvale van runs to Titan truth reference file.
This is helpful when sending data to the RTK group.

Example:
  ./pospac_to_titan_ref.py pospac.txt pospac.ref
"""

from mutils import llh2enu, array
from mutils.PosPacConst import *
import argparse
import sys
import os

# This file header is copied from an example RTK group truth file.
# The contents should be ignored...
file_header="""\**************************************************************************************************************************
 POS Export Utility                                    
 Copyright (C) 1997-2017 by Applanix Corporation      [Jun 28 2017] 
 All rights reserved.                                        
**************************************************************************************************************************
 
 Parameter setup:
 POSPROC SBET file: some_file.out 
 Camera mid-exposure event file: some_file.dat 
   Event time shift: 0.000000 sec 
 Photo ID file:  
   Photo ID file format: 2 Fields (Time, Photo ID) Format 
   Offset between PHOTO ID and EVENT file times: 0.000000 sec 
   PHOTO ID time tolerance: 0.300000 sec 
 Mission Start Time:
   Date of Mission: 2018-07-12
   Start Time: 09:34:20
 Mapping frame epoch: 2018.526027
 Mapping frame datum: ITRF2014  ; a = 6378137.000000; 1/f = 298.257224;
 Coordinate transformation from ITRF00 to mapping frame datum 
    dX = 0.001553; dY = 0.002053; dZ = -0.042299; f = 1.000000003058; 
    R1 = -0.000000000000; R2 = -0.000000000000; R3 = -0.000000000000;
 sequence of the rotations: x,y,z,1 
 Mapping frame projection : TM;
 central meridian = 9.000000 deg;
 latitude of the grid origin = 0.000000 deg; grid scale factor = 0.999600: 
 false easting = 500000.000000 m; false northing = 0.000000 m; 
 Boresight values: tx =        0.0000 arc min, ty =        0.0000 arc min, tz =        0.0000 arc min. 
 Lever arm values: lx =        0.0000 m, ly =        0.0000 m, lz =        0.0000 m. 
 TIME, DISTANCE, EASTING, NORTHING, ELLIPSOID HEIGHT, LATITUDE, LONGITUDE, ELLIPSOID HEIGHT, ROLL, PITCH, HEADING, EAST VELOCITY, NORTH VELOCITY, UP VELOCITY, EAST SD, NORTH SD, HEIGHT SD, ROLL SD, PITCH SD, HEADING SD

  (time in Sec, distance in Meters, position in Meters, lat, long in Degrees, orientation angles and SD in Degrees, velocity in Meter/Sec, position SD in Meters)  

"""

""

def main():
    parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter,description=usage)
    parser.add_argument('input',
                        help='Input POSPac filename')
    parser.add_argument('output',
                        help='Output RTK reference')
    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)
    args = parser.parse_args()

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

    out = open(args.output,'w')
    out.write(file_header)
    first_llh = None
    for line in open(args.input,'r'):
        w=line.split()
        if len(w) < 5:
            continue
        t = float(w[dPP_TIME])
        if t < args.min_t or t > args.max_t:
            continue
        llh = array([float(w[dPP_LAT]),float(w[dPP_LON]),float(w[dPP_HGT])])
        if first_llh is None:
            first_llh = llh
        enu = llh2enu( llh, first_llh )
        out.write(" %.5f         0.000 %11.3f %11.3f %8.3f %.8f %.8f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f\n"%
                  (t,
                   enu[0],
                   enu[1],
                   llh[2],
                   llh[0],
                   llh[1],
                   llh[2],
                   float(w[dPP_roll]),
                   float(w[dPP_pitch]),
                   float(w[dPP_heading]),
                   float(w[dPP_velE]),
                   float(w[dPP_velN]),
                   float(w[dPP_velU]),
                   float(w[dPP_sigE]),
                   float(w[dPP_sigN]),
                   float(w[dPP_sigU]),
                   float(w[dPP_sigRoll]),
                   float(w[dPP_sigPitch]),
                   float(w[dPP_sigHeading])
                  ))

if __name__ == '__main__':
    main()