#!/usr/bin/env python
# Copyright 2014-2015 Trimble
# $Id: gen_ini.py,v 1.4 2018/04/25 15:34:37 wlentz Exp $
# See "Usage" below
from __future__ import print_function
import sys,subprocess
from math import pi,sin,cos,sqrt

def runProcess(exe):
    p = subprocess.Popen(exe, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
    while(True):
        retcode = p.poll() #returns None while subprocess is running
        line = p.stdout.readline()
        yield line
        if(retcode is not None):
            break

def conv_lla_ecef(lat_deg, lon_deg, alt_m):
    A_WGS84 = 6378137.0
    E2_WGS84 = 6.69437999013e-3
    ONE_MIN_E2 = 0.99330562000987
    lat, lon = lat_deg*pi/180.0, lon_deg*pi/180.0
    Rn = A_WGS84 / sqrt(1.0 - (E2_WGS84 * sin(lat) * sin(lat)))
    tmp = (Rn + alt_m) * cos(lat)
    x = tmp * cos(lon)
    y = tmp * sin(lon)
    z = (Rn * ONE_MIN_E2 + alt_m) * sin(lat)
    return x, y, z


if len(sys.argv) != 2:
    print("**********************************************************************")
    print("Usage: %s some_file.T02 > piline.ini" % sys.argv[0])
    print("")
    print("  Takes the first position in 'some_file.T02' and produces")
    print("a piline.ini file for 0-baseline analysis.")
    print("")
    print("NOTE: viewdat must be in your path!")
    print("**********************************************************************")
    sys.exit(1)


got_r29 = 0
llh = None
cmdline = 'viewdat --translate_rec35_sub2_to_rec29 -d29 %s' % sys.argv[1]
for line in runProcess(cmdline.split()):
    if line.startswith(b'TYPE 29: 3D solution [4+'):
        got_r29 = 1
    if got_r29 and line.startswith(b'Lat :'):
        w = line.split()
        llh = (float(w[2]),float(w[6]),float(w[10]))
        break

if llh is None:
    print("Didn't find a good type 29")
    sys.exit(1)

x,y,z = conv_lla_ecef( llh[0], llh[1], llh[2] )
print("base.T01 0")
print("rover.T01 0")
print("results.txt")
print("0.0")
print("0.0")
print("0.0")
print("%.3f %.3f %.3f" % (x,y,z))
print("%.3f %.3f %.3f" % (x,y,z))