#!/usr/bin/env python

# Copyright (c) 2018-2019 Trimble Inc. ###############################
# $Id: sdp4_check.py,v 1.10 2019/03/13 20:22:26 wlentz Exp $
######################################################################

usage="""\
Example:
 ./sdp4_check.py 10.1.150.xxx

Compares the receiver predicted elevation to:
 1- TLE from latest sd_sdp4_alms.h/list.sv in CVS
 2- TLE from latest Celestrak data
Currently only operates on BeiDou satellites.
"""

import requests
from xml.etree import ElementTree
import ephem # pip install pyephem if missing
from mutils import *
from datetime import datetime, timedelta
import os
import sys
import pandas as pd
import re
import socket
from six import print_
import argparse

user_lat = 37.384896686
user_lon = -122.005463678
user_hgt = -6.225
auth_info = ['admin','password']
max_elev_deg_err = 5.0

def read_tle( lines ):
    return ephem.readtle('sv', lines[0], lines[1])

def compute_elev( tle, t, lat=user_lat, lon=user_lon, hgt=user_hgt ):
    observer = ephem.Observer()
    observer.lat = '%.9f' % lat
    observer.lon = '%.9f' % lon
    observer.elevation = hgt
    observer.pressure = 0
    observer.date = t
    tle.compute(observer)
    return degrees(tle.alt)

def get_pred_elev(rcvr,sv_id,sat_type):
    """Get predicted elevation from receiver web page.
       rcvr = IP address (e.g., "10.1.150.123"
       sv_id = satellite ID (e.g., 1)
       sat_type = satellite type (e.g., "SAT_TYPE_GPS" or "SAT_TYPE_BEIDOU")
    """
    sat_str = sat_type.split('_')[-1].lower() # e.g., 'gps' or 'beidou'
    resp = requests.get('http://%s/xml/dynamic/%sTrack%d.xml?Lat=%.9f&Lon=%.9f&Hgt=%.3f' % (rcvr, sat_str, sv_id, user_lat, user_lon, user_hgt), auth=tuple(auth_info), timeout=10)
    if resp.status_code == requests.codes.unauthorized:
        print("Bad login/authorization")
    try:
        xml = ElementTree.fromstring(resp.content)
    except:
        if resp.content == b'':
            return []
        else:
            raise RuntimeError(resp.content)
    d = []
    for sv in xml.findall('sv'):
        src = sv.find('src')
        if src is not None:
            src = src.text
        else:
            src = ''
        d.append( [float(sv.find('week').text), float(sv.find('secs').text), float(sv.find('elev').text), src] )
    return pd.DataFrame.from_records(d, columns=['week','secs','elev','src'] )

def get_alm_eph(rcvr,sv_id,sat_type):
    """Get raw almanac and ephemeris data from receiver web page
       rcvr = IP address (e.g., "10.1.150.123"
       sv_id = satellite ID (e.g., 1)
       sat_type = satellite type (e.g., "SAT_TYPE_GPS" or "SAT_TYPE_BEIDOU")
    """
    num_sat_type = getattr(RcvrConst,sat_type)
    resp1 = requests.get('http://%s/xml/dynamic/satData.xml?sys=%d&type=1&sv=%d' % (rcvr, num_sat_type, sv_id), auth=tuple(auth_info), timeout=10)
    resp2 = requests.get('http://%s/xml/dynamic/satData.xml?sys=%d&type=0&sv=%d' % (rcvr, num_sat_type, sv_id), auth=tuple(auth_info), timeout=10)
    return resp1.text + resp2.text

def get_sd_sdp4():
    print("Getting Stinger data...")
    sd_sdp4 = {}
    sd_tle = {}

    resp = requests.get('http://cvsweb.eng.trimble.com/viewvc.cgi/stinger/sd/sd_sdp4_alms.h?view=co',
                 auth=('cvs','svale'))
    for line in resp.iter_lines():
        line = line.decode('utf-8')
        # Legacy file format using array of chars, look for:
        #   {"line 1"},
        #   {"line 2"}
        m = re.match(r' *{"(.*?)"}', line)
        if m is None or m.group(1) == "":
            # New file format using structure, look for:
            #    "line1",
            #    "line2"
            m = re.match(r'    "(.*?)"', line)
            if m is None or m.group(1) == "":
                continue
        catalog_id = int(m.group(1).split()[1].strip('U'))
        sd_sdp4.setdefault(catalog_id,[])
        sd_sdp4[catalog_id].append( m.group(1) )
        if len(sd_sdp4[catalog_id]) == 2:
            sd_tle[catalog_id] = read_tle( sd_sdp4[catalog_id] )
    return sd_tle

def get_svs_latest():
    print("Getting Celestrak data...")
    all_sv = {}
    mapping = {}
    resp = requests.get('http://cvsweb.eng.trimble.com/viewvc.cgi/stinger/sd/list.sv?view=co',
                 auth=('cvs','svale'))
    # The column format, using w[.] below, of list.sv is:
    # w[0] SAT_TYPE_x [or GPS_ALL or GAL_ALL to get SV info. from Celestrak]
    # w[1] SV Id
    # w[2] NORAD ID / cat. number
    # w[3] allow fallback [only in revisions on/after 9th Jan. 2019 but it's not used but this script]
    for line in resp.iter_lines():
        line = line.decode('utf-8')
        if line.startswith('SAT_TYPE'):
            w = line.split()
            mapping[int(w[2])] = (w[0], int(w[1]))
    resp = requests.get('https://www.celestrak.com/NORAD/elements/active.txt')
    resp = resp.text.split('\r\n')
    for n in range(0,len(resp)-1,3):
        tle = read_tle( resp[n+1:n+3] )
        if tle.catalog_number in mapping:
            prn = mapping[tle.catalog_number]
            all_sv[ prn ] = tle
    return all_sv

if __name__ == "__main__":
    parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter,description=usage)
    parser.add_argument('rcvr', help='Receiver IP address or DNS name')
    parser.add_argument('--user', help='Login username')
    parser.add_argument('--passwd', help='Login password')
    parser.add_argument('--diag_out', help='text filename for extended diagnostic output')
    args = parser.parse_args()
    rcvr = args.rcvr
    if not '.' in rcvr:
        # If the user hasn't given a domain, do an IP address lookup
        # so the proxy doesn't get confused.
        rcvr = socket.gethostbyname(rcvr)
    if args.diag_out:
        out_alms = open(args.diag_out,'w')
    else:
        out_alms = None
    if args.user:
        auth_info[0] = args.user
    if args.passwd:
        auth_info[1] = args.passwd

    close('all')
    sd_sdp4 = get_sd_sdp4()
    svs_latest = get_svs_latest()
    invalid_elevs = []
    for svs_key in sorted(svs_latest.keys()):
        sat_type, sv_id = svs_key
        d = get_pred_elev( rcvr, sv_id, sat_type )
        catalog_num = svs_latest[svs_key].catalog_number
        cmp_rcvr_sd = True
        if not catalog_num in sd_sdp4:
            cmp_rcvr_sd = False
        if len(d) == 0:
            print("No receiver data %s %d / %d" % (sat_type, sv_id,catalog_num))
            continue

        tle_elev = []
        tle_elev_latest = []
        for _,row in d.iterrows():
            gps_offset = row.week*60*60*24*7 + row.secs
            t = datetime(1980,1,6) + timedelta(seconds=gps_offset)
            if ( cmp_rcvr_sd ) :
                elev = compute_elev( sd_sdp4[ catalog_num ], t )
            else :
                elev = None
            tle_elev.append( elev )
            elev = compute_elev( svs_latest[ svs_key ], t )
            tle_elev_latest.append( elev )
        tle_elev = array(tle_elev)
        tle_elev_latest = array(tle_elev_latest)
        tle_diff = max(abs(d.elev - tle_elev))
        tle_diff_latest = max(abs(d.elev - tle_elev_latest))
        src_list = unique(d.src)
        info = 'check %s %d. ' % (sat_type.split('_')[-1],sv_id)
        if len(src_list[0]) > 0:
            info += 'Rcvr src %s. ' % src_list[0]
        info += 'TLE elev err: '
        if ( cmp_rcvr_sd ) :
            info += 'rcvr-SD=%.1f, ' % tle_diff
        else :
            info += 'rcvr-SD=N/A, '
        info += 'rcvr-latest=%.1f [deg]' % tle_diff_latest
        print(info)

        if len(src_list) > 1:
            print('  WARNING - multiple receiver elev sources')
            print(src_list)

        if tle_diff > max_elev_deg_err or tle_diff_latest > max_elev_deg_err:
            warn  = '  invalid elev.  TLE epochs: '
            if ( cmp_rcvr_sd ) :
                warn += 'SD=%.1f ' % sd_sdp4[ catalog_num ]._epoch
            else :
                warn += 'SD=N/A '
            warn += 'latest=%.1f' % svs_latest[svs_key]._epoch
            print( warn )
            alm_eph = get_alm_eph( rcvr, sv_id, sat_type )
            if out_alms is not None:
                print_('',file=out_alms)
                print_('rcvr<->new_tle %.1f' % (tle_diff_latest),file=out_alms)
                if cmp_rcvr_sd:
                    print_('rcvr<->old_tle %.1f' % (tle_diff),file=out_alms)
                    print_('old_tle<->new_tle %.1f' % max(abs(tle_elev - tle_elev_latest)),file=out_alms)
                print_(alm_eph,file=out_alms)
            figure()
            title('%s %d' % (sat_type,sv_id))
            sp=6 # put a little space between markers for clarity
            src = src_list[0]
            i = find( d.src == src )
            plot( d.secs[i], d.elev[i], label='%s (st_get_elev%s)'%(rcvr,'='+src) )
            plot( d.secs[::sp], tle_elev[::sp], '1', markersize=8, label='Top CVS sd_sdp4_alms.h' )
            plot( d.secs[::sp], tle_elev_latest[::sp], '2', markersize=8, label='Celestrak SDP4' )
            xlabel('GPS seconds')
            ylabel('Elev [deg]')
            grid()
            figlegend()
show()