#!@PYSHEBANG@
# -*- coding: utf-8 -*-
# @GENERATED@
# This file is Copyright 2020 by the GPSD project
# SPDX-License-Identifier: BSD-2-clause

# This code runs compatibly under Python 2 and 3.x for x >= 2.
# Preserve this property!
#
"""gpssubframe -- read JSON subframe messages from gpsd and decode them."""

from __future__ import print_function

import argparse
import math
import socket
import sys
import time            # for time.time()

# pylint wants local modules last
try:
    import gps
except ImportError as e:
    sys.stderr.write(
        "%s: can't load Python gps libraries -- check PYTHONPATH.\n" %
        (sys.argv[0]))
    sys.stderr.write("%s\n" % e)
    sys.exit(1)

gps_version = '@VERSION@'
if gps.__version__ != gps_version:
    sys.stderr.write("%s: ERROR: need gps module version %s, got %s\n" %
                     (sys.argv[0], gps_version, gps.__version__))
    sys.exit(1)


ura2ure = {0: "0.00 to 2.40",
           1: "2.40 to 3.40",
           2: "3.40 to 4.85",
           3: "4.85 to 6.85",
           4: "6.85 to 9.65",
           5: "9.65 to 13.65",
           6: "13.65 to 24.00",
           7: "24.00 to 48.00",
           8: "48.00 to 96.00",
           9: "96.00 to 192.00",
           10: "192.00 to 384.00",
           11: "384.00 to 768.00",
           12: "768.00 to 1536.00",
           13: "1536.00 to 3072.00",
           14: "3072.00 to 6144.00",
           15: "6144.00 or worse",
           }

health_str = {0: "All Signals OK",
              1: "All Signals Weak",
              2: "All Signals Dead",
              3: "All Signals Have No Data Modulation",
              4: "L1 P Signal Weak",
              5: "L1 P Signal Dead",
              6: "L1 P Signal Has No Data Modulation",
              7: "L2 P Signal Weak",
              8: "L2 P Signal Dead",
              9: "L2 P Signal Has No Data Modulation",
              10: "L1C Signal Weak",
              11: "L1C Signal Dead",
              12: "L1C Signal Has No Data Modulation",
              13: "L2C Signal Weak",
              14: "L2C Signal Dead",
              15: "L2C Signal Has No Data Modulation",
              16: "L1 & L2 P Signal Weak",
              18: "L1 & L2 P Signal Dead",
              19: "L1 & L2 P Signal Has No Data Modulation",
              20: "L1 & L2C Signal Weak",
              21: "L1 & L2C Signal Dead",
              22: "L1 & L2C Signal Has No Data Modulation",
              23: "L1 Signal Weak",
              24: "L1 Signal Dead",
              25: "L1 Signal Has No Data Modulation",
              26: "L2 Signal Weak",
              27: "L2 Signal Dead",
              28: "L2 Signal Has No Data Modulation",
              29: "SV Is Temporarily Out",
              30: "SV Will Be Temporarily Out",
              31: "Multiple Anomalies",
              }

# subframe 4 page 25 sv config
sv_conf = {
    0: "Reserved",
    1: "Block II/Block IIA/IIR SV",
    2: "M-code, L2C, Block IIR-M SV",
    3: "M-code, L2C, L5, Block IIF SV",
    4: "M-code, L2C, L5, No SA,  Block III SV",
    5: "Reserved",
    6: "Reserved",
    7: "Reserved",
    }


def kepler(e, M):
    """Keplers iteration to solve his equation."""

    # https://en.wikipedia.org/wiki/Kepler%27s_equation
    E = M             # initial guess
    for iter in range(1, 20):
        temp = E
        E = M + e * math.sin(E)
        if 1e-12 > math.fabs(E - temp):
            break
    return E

# another kepler algorithm here:
# http://www.alpheratz.net/dynamics/twobody/KeplerIterations_summary.pdf


# Online data check:
# real time sat positions: https://in-the-sky.org/satmap_worldmap.php
def SatPos(ephm, t):
    """Calculate GPS Satellite Position from Ephemeris/Almanac."""
    # compute Sat position from IS-GPS-200 table 20-IV
    # See also:
    # RTKLIB src/ephemeris.c
    # https://www.telesens.co/2017/07/17/calculating-position-from-raw-gps-data/#1b_Code_for_Calculating_Satellite_Position
    # gnss-sdr: src/core/system_parameters/gps_ephemeris.cc

    # Semi-major axis
    A = ephm['sqrtA'] ** 2      # meters

    # Computed mean motion
    n0 = math.sqrt(gps.WGS84GM / (A ** 3))           # rad/sec

    # t: GPS system time of week at time of transmission
    # tk: time from ephemeris reference epoch
    tk = t - ephm['toe']                 # seconds
    # handle week rollover
    if 302400 < tk:
        tk -= 604800
    elif -302400 > tk:
        tk += 604800

    # corrected mean motion
    n = n0 + ephm['deltan']   # rad/sec

    # mean anomaly, rads
    Mk = ephm['M0'] + (n * tk)

    # solve for eccentric anomaly
    Ek = kepler(ephm['e'], Mk)   # radians
    cosEk = math.cos(Ek)
    sinEk = math.sin(Ek)

    # true anomaly
    if False:
        nuk = math.atan2(
           math.sqrt(1 - (ephm['e'] ** 2)) * sinEk /
           (1 - ephm['e'] * math.cos(Ek)),
           (cosEk - ephm['e']) / (1 - ephm['e'] * cosEk))
    else:
        # alternate true anomaly
        # close enough?
        nuk = math.atan2(math.sqrt(1 - (ephm['e'] ** 2)) * sinEk,
                         (cosEk - ephm['e']))

    # Argument of Latitude
    Phik = nuk + ephm['omega']

    if 'Cus' in ephm:
        # 2nd harmonic corrections
        # Argument of Latitude Correction
        sin2Phik = math.sin(2 * Phik)
        cos2Phik = math.cos(2 * Phik)
        deltauk = (ephm['Cus'] * sin2Phik + ephm['Cuc'] * cos2Phik)
        # Radius Correction
        deltark = (ephm['Crs'] * sin2Phik + ephm['Crc'] * cos2Phik)
        # Inclination Correction
        deltaik = (ephm['Cis'] * sin2Phik + ephm['Cic'] * cos2Phik)
    else:
        # almanac does not have these corrections
        deltauk = 0
        deltark = 0
        deltaik = 0

    # Corrected Argument of Latitude
    uk = Phik + deltauk

    # Corrected Radius
    rk = A * (1 - ephm['e'] * cosEk) + deltark

    # Corrected Inclination Angle
    ik = ephm['i0'] + ephm['IDOT'] * tk + deltaik

    # Positions in orbital plane.
    xkprime = rk * math.cos(uk)
    ykprime = rk * math.sin(uk)

    # diff of sat and earth Omega dot
    delta_Omegad = ephm['Omegad'] - gps.WGS84AV
    # Corrected longitude of ascending node.
    Omegak = (ephm['Omega0'] + delta_Omegad * tk - gps.WGS84AV * ephm['toe'])

    # Earth-fixed coordinates.
    cosik = math.cos(ik)
    sinik = math.sin(ik)
    ykprimecosik = ykprime * cosik
    sinOmegak = math.sin(Omegak)
    cosOmegak = math.cos(Omegak)
    x = xkprime * cosOmegak - ykprimecosik * sinOmegak
    y = xkprime * sinOmegak + ykprimecosik * cosOmegak
    z = ykprime * sinik

    # sat velocity in ECEF coordinates, meters per second
    # not validated yet.
    vx = (-delta_Omegad * (xkprime + ykprime * cosik) +
          x * cosOmegak -
          y * cosik * sinOmegak)
    vy = (delta_Omegad * (xkprime * cosOmegak - ykprime * cosik * sinOmegak) +
          x * sinOmegak +
          y * cosik * cosOmegak)
    vz = y * sinik

    # Almanac does not have toc, or ure
    if 'toc' in ephm:
        # relativistic correction term, seconds
        deltatr = gps.GPS_F * ephm['e'] * ephm['sqrtA'] * sinEk
        # toff == SV PRN code phase offset, seconds
        # we want tgps, but our t (tsv) is close enough.
        toff = t - ephm['toc']
        # handle week rollover
        if 302400 < toff:
            toff -= 604800
        elif -302400 > toff:
            toff += 604800
        # SV Clock Correction
        deltatsv = (ephm['af0'] + ephm['af1'] * toff +
                    ephm['af2'] * toff ** 2 + deltatr)
        print("deltatr %e deltatsv %e Tgd %.e\n"
              "URE %s Health %s" %
              (deltatr, deltatsv, ephm['Tgd'], ura2ure[ephm['ura']],
               health_str[ephm['hlth'] & 0x1f]))

    if 1 < options.debug:
        if 2 < options.debug:
            print(ephm)
        print("t %.3f tk %.3f" % (t, tk))
        print("A %s n0 %s n %s\nMk %s Ek %s" % (A, n0, n, Mk, Ek))
        print("nuk %.10g Phik %.10g" % (nuk, Phik))
        print("deltauk %.10g deltark %.10g deltaik %.10g" %
              (deltauk, deltark, deltaik))
        print("uk %.10g rk %.10g ik %.10g" % (uk, rk, ik))
        print("xkprime %.10g ykprime %.10g Omegak %.10g" %
              (xkprime, ykprime, Omegak))
        # compute distance from earth's center, and orbital speed, as
        # simple cross-check
        # GPS orbit: speed about 3.9 km/sec, radius about 26,600 km
        dist = math.sqrt((x * x) + (y * y) + (z * z))
        speed = math.sqrt((vx * vx) + (vy * vy) + (vz * vz))
        print("dist %13.3f speed %13.3f" %
              (dist, speed))

    # Finally, lat/lon/alt
    (lat, lon, altHAE) = gps.ecef2lla(x, y, z)
    print(" x %13.3f  y %13.3f  z %13.3f\n"
          "vx %13.3f yv %13.3f zv %13.3f\n"
          "lat %.6f lon %.6f altHAE %.3f" %
          (x, y, z, vx, vy, vz, lat, lon, altHAE))

    # ax and el to sat
    if 'lat' in tpv and 'lon' in tpv and 'altHAE' in tpv:
        (E, N, U) = gps.ecef2enu(x, y, z,
                                 tpv['lat'], tpv['lon'], tpv['altHAE'])
        print("E %.3f N %.3f U %.3f" % (E, N, U))
        (az, el, rng) = gps.enu2aer(E, N, U)
        print("az %.3f el %.3f rng %.3f" % (az, el, rng))


# current GPS Ephemeris here, url split: https://cddis.nasa.gov/
#   Data_and_Derived_Products/GNSS/broadcast_ephemeris_data.html
ephemeris1 = {}
ephemeris2 = {}
ephemeris3 = {}
# current almanac here: https://www.navcen.uscg.gov/?pageName=gpsAlmanacs
almanac = {}
# ionosphere, subframe 4, page 18, data id 56
ionosphere = None
# subframe 4/5 page 25
health = None
health2 = None
tpv = None               # for current time and position


ephem1_fields = {
    1: ('ura', 'URA Index'),
    2: ('WN', 'Data Sequence Propagation Week Number'),
    3: ('L2P', 'L2 P data flag'),
    4: ('hlth', 'SV health'),
    5: ('Tgd', '(s) Group Delay Differential'),
    6: ('IODC', 'Issue of Data, Clock'),
    7: ('toc', '(s) Time of Clock'),
    8: ('L2', 'Code on L2'),
    9: ('af0', '(sc) SV Clock Bias Correction Coefficient'),
    10: ('af1', '(s) SV Clock Drift Correction Coefficient'),
    11: ('af2', '(s/s) Drift Rate Correction Coefficient'),
    }

ephem2_fields = {
    1: ('IODE', 'Issue of Data, Ephemeris'),
    2: ('M0', '(sc) Mean Anomaly at Reference Time'),
    3: ('deltan', '(sc/s) Mean Motion Difference from Computed Value'),
    4: ('e', 'Eccentricity '),
    5: ('sqrtA', '(sqrt(m))Square Root of the Semi-Major Axis'),
    6: ('FIT', 'Fit Interval Flag'),
    7: ('AODO', '(s) Age of Data Offset'),
    8: ('Crs', '(m) Sine Correction Amplitude Term to Orbit Radius'),
    9: ('Cus', '(rad) Cosine Correction Amplitude Term to Orbit Radius'),
    10: ('Cuc', '(rad) Sine Harmonic Correction Term to Arg of Lat'),
    11: ('toe', '(s) Reference Time Ephemeris')
    }

ephem3_fields = {
    1: ('IODE', 'Issue of Data, Ephemeris'),
    2: ('Crc', '(m) Cosine Harmonic Correction Amplitude Orbit Radius'),
    3: ('Cic',
        '(rad) Cosine Harmonic Correction Amplitude Angle of Inclination'),
    4: ('Cis',
        '(rad) Sine Harmonic Correction Amplitude Angle of Inclination'),
    5: ('Omega0',
        '(sc) Longitude of Ascending Node of Orbit Plane Week beginning'),
    6: ('i0', '(sc) Inclination Angle at Reference Time'),
    7: ('omega', '(sc) Argument of Perigee'),
    8: ('Omegad', '(sc/s) Rate of Right Ascension'),
    9: ('IDOT', '(sc/s) Rate of Inclination Angle'),
    }

almanac_fields = {
    # 1: ('ID' same as SV
    1: ('Health', 'SV health'),
    2: ('e', 'Eccentricity '),
    3: ('toa', '(s) Almanac Rference Time'),
    4: ('deltai', '(sc) Inclination offset from 0.3 semicircles (= 54 deg)'),
    5: ('Omegad', '(sc/s) Rate of Right Ascension'),
    6: ('sqrtA', '(m-2)Square Root of the Semi-Major Axis'),
    7: ('Omega0',
        '(sc) Longitude of Ascending Node of Orbit Plane Week beginning'),
    8: ('omega', '(sc) Argument of Perigee'),
    9: ('M0', '(sc) Mean Anomaly at Reference Time'),
    10: ('af0', '(s) SV Clock Bias Correction Coefficient'),
    11: ('af1', '(s/s) SV Clock Drift Correction Coefficient'),
    }

# see IS-GPS-200 Section 20.3.3.5.2.4 Coordinated Universal Time (UTC).
ionosphere_fields = {
    1: ('A0', '(s) constant term of polynomial'),
    2: ('A1', '(s) first order term of polynomial'),
    3: ('ls', '(s) delta time due to leap seconds'),
    4: ('tot', '(s) reference time for UTC data'),
    5: ('WNt', '(wk) UTC reference week number'),
    6: ('WNlsf', '(wk) Week Number of future Leap Second'),
    7: ('DN', '(dy) Day Number of future Leap Second'),
    8: ('lsf', '(s) future Leap Second'),
    9: ('a0', '(s) constant term amplitude of vertical delay'),
    10: ('a1', '(s/sc) first order term amplitude of vertical delay'),
    11: ('a2', '(s/sc^2) second order term amplitude of vertical delay'),
    12: ('a3', '(s/sc^3) third order term amplitude of vertical delay'),
    13: ('b0', '(s) constant term period of the model'),
    14: ('b1', '(s/sc) first order term period of the model'),
    15: ('b2', '(s/sc^2) second order term period of the model'),
    16: ('b3', '(s/sc^3) third order term period of the model'),
    }


def _print_msg(svid, ephem, fields):
    """Print Subframe Data."""
    for index in sorted(fields.keys()):
        fld = fields[index]
        print("%10s %s" % (fld[0], ephem[fld[0]]))
        if options.desc:
            print("           %-48s " % (fld[1]))


description = 'Convert one gpsd JSON message class to csv format.'
usage = '%(prog)s [OPTIONS] [host[:port[:device]]]'
epilog = ('BSD terms apply: see the file COPYING in the distribution root'
          ' for details.')

parser = argparse.ArgumentParser(
             description=description,
             epilog=epilog,
             formatter_class=argparse.RawDescriptionHelpFormatter,
             usage=usage)
parser.add_argument(
    '-?',
    action="help",
    help='show this help message and exit'
)
parser.add_argument(
    '--desc',
    dest='desc',
    default=False,
    action="store_true",
    help='Print long descriptions [Default %(default)s)]'
)
parser.add_argument(
    '-D',
    '--debug',
    dest='debug',
    default=0,
    type=int,
    help='Set level of debug. Must be integer. [Default %(default)s)]'
)
parser.add_argument(
    '--device',
    dest='device',
    default='',
    help='The device to connect. [Default %(default)s)]'
)
parser.add_argument(
    '--host',
    dest='host',
    default='localhost',
    help='The host to connect. [Default %(default)s)]'
)
parser.add_argument(
    '-n',
    '--count',
    dest='count',
    default=0,
    type=int,
    help='Count of messages to parse. 0 to disable. [Default %(default)s)]'
)
parser.add_argument(
    '--port',
    dest='port',
    default=gps.GPSD_PORT,
    help='The port to connect. [Default %(default)s)]'
)
parser.add_argument(
    '--progress',
    dest='progress',
    default=False,
    action="store_true",
    help='Print progress reports [Default %(default)s)]'
)
parser.add_argument(
    '--satpos',
    dest='satpos',
    default=False,
    action="store_true",
    help='Compute Satellite Positions [Default %(default)s)]'
)
parser.add_argument(
    '--test',
    dest='test',
    default=False,
    action="store_true",
    help='Run tests of the algorithms [Default %(default)s)]'
)
parser.add_argument(
    '-V', '--version',
    action='version',
    version="%(prog)s: Version " + gps_version + "\n",
    help='Output version to stderr, then exit'
)
parser.add_argument(
    '-x',
    '--seconds',
    dest='seconds',
    default=16,
    type=int,
    help='Seconds of messages to parse. 0 to disable. [Default %(default)s)]'
)
parser.add_argument(
    'target',
    nargs='?',
    help='[host[:port[:device]]]'
)
options = parser.parse_args()

# the options host, port, device are set by the defaults
if options.target:
    # override host, port and device with target
    arg = options.target.split(':')
    len_arg = len(arg)
    if len_arg == 1:
        (options.host,) = arg
    elif len_arg == 2:
        (options.host, options.port) = arg
    elif len_arg == 3:
        (options.host, options.port, options.device) = arg
    else:
        parser.print_help()
        sys.exit(0)

if not options.port:
    options.port = 2947

try:
    session = gps.gps(host=options.host, port=options.port,
                      verbose=options.debug)
except socket.error:
    sys.stderr.write("gpscsv: Could not connect to gpsd daemon\n")
    sys.exit(1)

session.stream(gps.WATCH_ENABLE | gps.WATCH_SCALED, devpath=options.device)

# save subframe 1 WN
subframe1_WN = -1

count = 0
if 0 < options.seconds:
    end_seconds = time.time() + options.seconds
else:
    end_seconds = 0

try:
    while True:
        msg = session.next()
        if 'SUBFRAME' == msg['class']:
            if 1 < options.debug:
                print("Subframe %d TOW17 %s WN %d" %
                      (msg["frame"], msg['TOW17'], subframe1_WN))
            if 1 == msg['frame']:
                if 'WN' in msg['EPHEM1']:
                    # save WN for later use
                    subframe1_WN = msg['EPHEM1']['WN']

                if ((msg['tSV'] not in ephemeris1 or
                     msg['EPHEM1']['IODC'] != ephemeris1[msg['tSV']]['IODC'])):
                    if options.progress:
                        print("Received Ephemeris1 GPS %2d, IODC %3d" %
                              (msg['tSV'], msg['EPHEM1']['IODC']))
                    ephemeris1[msg['tSV']] = msg['EPHEM1']
            elif 2 == msg['frame']:
                if ((msg['tSV'] not in ephemeris2 or
                     msg['EPHEM2']['IODE'] != ephemeris2[msg['tSV']]['IODE'])):
                    if options.progress:
                        print("Received Ephemeris2 GPS %2d, IODE %3d" %
                              (msg['tSV'], msg['EPHEM2']['IODE']))
                    ephemeris2[msg['tSV']] = msg['EPHEM2']
            elif 3 == msg['frame']:
                if ((msg['tSV'] not in ephemeris3 or
                     msg['EPHEM3']['IODE'] != ephemeris3[msg['tSV']]['IODE'])):
                    if options.progress:
                        print("Received Ephemeris3 GPS %2d, IODE %3d" %
                              (msg['tSV'], msg['EPHEM3']['IODE']))
                    ephemeris3[msg['tSV']] = msg['EPHEM3']
            elif 4 <= msg['frame'] <= 5:
                if 'ALMANAC' in msg:
                    # save it if:
                    #  not dummy
                    #  don't already have it
                    #  newer than what we have
                    if ((0 != msg['dataid'] and
                         (msg['dataid'] not in almanac or
                          almanac[msg['dataid']]['toa'] <
                          msg['ALMANAC']['toa']))):
                        # FIXME: handle week rollover
                        if options.progress:
                            print("Received Almanac dataid %2d  toa %d" %
                                  (msg['dataid'], msg['ALMANAC']['toa']))
                        almanac[msg['dataid']] = msg['ALMANAC']
                elif 'IONO' in msg:
                    if ((not ionosphere or
                         msg['IONO']['tot'] < ionosphere['tot'])):
                        # FIXME handle WNt rollover
                        # new data set
                        if options.progress:
                            print("Received Subframe 4 dataid 56 "
                                  "(Ionosphere and UTC) tot %d WNt %d" %
                                  (msg['IONO']['tot'], msg['IONO']['WNt']))
                        ionosphere = msg['IONO']
                elif 'HEALTH' in msg:
                    if options.progress:
                        print("Received Subframe 4 dataid 63 "
                              "(A-S, Configurations)")
                    health = msg['HEALTH']
                elif 'HEALTH2' in msg:
                    if ((not health2 or
                         msg['HEALTH2']['toa'] < health2['toa'])):
                        # FIXME handle WNa rollover
                        # new data set
                        if options.progress:
                            print("Received Subframe 5 dataid 51 "
                                  "(SV Health) toa %s WNa %s" %
                                  (msg['HEALTH2']['toa'],
                                   msg['HEALTH2']['WNa']))
                        health2 = msg['HEALTH2']
                # else not Almanac page
            # else, bad packet...
        elif 'TPV' == msg['class']:
            tpv = msg

        if 0 < options.count:
            count += 1
            if count >= options.count:
                break

        if 0 < options.seconds:
            if time.time() > end_seconds:
                break

except KeyboardInterrupt:
    # caught control-C
    print()
    sys.exit(1)

all_sv = (list(ephemeris1.keys()) + list(ephemeris2.keys()) +
          list(ephemeris3.keys()) + list(almanac.keys()))
for sv in sorted(set(all_sv)):
    print("\nSV %u" % sv)
    print("  Subframe 1, Clock Data")
    if sv in ephemeris1:
        _print_msg(sv, ephemeris1[sv], ephem1_fields)
    else:
        print("    Missing")
    print("  Subframe 2, Orbit Data")
    if sv in ephemeris2:
        _print_msg(sv, ephemeris2[sv], ephem2_fields)
    else:
        print("    Missing")
    print("  Subframe 3, Orbit Data")
    if sv in ephemeris3:
        _print_msg(sv, ephemeris3[sv], ephem3_fields)
    else:
        print("    Missing")
    print("  Subframe 4 page 25")
    svid = "SV%d" % sv
    if health and svid in health:
        if 0x3f == health[svid]:
            print("       Flags  %2d (n/a)" % health[svid])
        else:
            print("       Flags  %2d (A/S %3s Config %s)" %
                  (health[svid], 'Yes' if (health[svid] & 8) else 'No',
                   sv_conf[health[svid] & 0x7]))
    else:
        print("    Missing")
    print("  Almanac")
    if sv in almanac:
        _print_msg(sv, almanac[sv], almanac_fields)
    else:
        print("    Missing")

gps_time = None
gps_week = None
gps_tow = None

if 'time' in tpv:
    # convert to UNIX time
    unix_time = gps.isotime(tpv['time'])
    # GPS Epoch starts: Jan 1980 00:00:00 UTC, Unix time: 315964800
    gps_time = unix_time - 315964800
    if 'leapseconds' in tpv:
        gps_time += tpv['leapseconds']
    # 604,800 in a GPS week
    (gps_week, gps_tow) = divmod(gps_time, 604800)

    print("\nUnix time: %s, gps time: %s, week: %s tow: %s" %
          (unix_time, gps_time, int(gps_week), gps_tow))
    if 'lat' in tpv and 'lon' in tpv and 'altHAE' in tpv:
        print("Current position: lat %.8f lon %.8f altHAE %.3f" %
              (tpv['lat'], tpv['lon'], tpv['altHAE']))

if ionosphere:
    print("\nSubframe 4, page 18")
    _print_msg(None, ionosphere, ionosphere_fields)
    if gps_tow:
        # tUTC = tLS + A0 + A1 (tE - tot + 604800 (WN - WNt))
        deltatutc = (ionosphere['ls'] +
                     ionosphere['A0'] +
                     ionosphere['A1'] *
                     (gps_tow - ionosphere['tot'] +
                      604800 * (gps_week - ionosphere['WNt']))
                     )
        print("  deltatutc %.9f" % deltatutc)

if health:
    print("\nSubframe 4, page 25")
    for sv in range(1, 33):
        id = "SV%d" % sv
        if id in health:
            print("  %s %2d (A/S %3s Config %s)" %
                  (id, health[id], 'Yes' if (health[id] & 8) else 'No',
                   sv_conf[health[id] & 0x7]))
    for sv in range(25, 33):
        id = "SVH%d" % sv
        if id in health:
            if 0x3f == health[id]:
                print("  %s %2d (n/a)" % (id, health[id]))
            else:
                print("  %s %2d (Data %s, %s)" %
                      (id, health[id], 'Bad' if (health[id] & 0x20) else 'OK',
                       health_str[health[id] & 0x1f]))

if health2:
    print("\nSubframe 5, page 25\n  WNa %d toa %d" %
          (health2['WNa'], health2['toa']))
    for sv in range(1, 25):
        id = "SVH%d" % sv
        if id in health2:
            if 0x3f == health2[id]:
                print("  %s %2d (n/a)" % (id, health2[id]))
            else:
                print("  %s %2d (Data %s, %s)" %
                      (id, health2[id],
                       'Bad' if (health2[id] & 0x20) else 'OK',
                       health_str[health2[id] & 0x1f]))

if 'time' in tpv and options.satpos:
    for sv in range(1, 33):
        # Ensure we have all 3 parts, and their IODx matches.
        if ((sv in ephemeris1 and
             sv in ephemeris2 and
             sv in ephemeris3 and
             ephemeris1[sv]['IODC'] == ephemeris2[sv]['IODE'] and
             ephemeris2[sv]['IODE'] == ephemeris3[sv]['IODE'])):

            print("\nSV %u Ephemeris position:" % sv)

            # mash into one dict
            eph = ephemeris1[sv]
            eph.update(ephemeris2[sv])
            eph.update(ephemeris3[sv])

            # convert semi-circles to radians
            # use military PI, not real pi.
            cvt = ('deltan', 'i0', 'IDOT', 'M0', 'omega', 'Omega0',
                   'Omegad')
            for i in cvt:
                eph[i] *= gps.GPS_PI

            SatPos(eph, gps_tow)

        if sv in almanac:
            print("\nSV %u Almanac position:" % sv)

            ephm = almanac[sv]
            ephm.update({'i0': ephm['deltai'] + 0.30,
                         'deltan': 0,
                         'IDOT': 0,
                         'toe': ephm['toa']})

            # convert semi-circles to radians
            # use military PI, not real pi.
            cvt = ('deltan', 'i0', 'IDOT', 'M0', 'omega', 'Omega0',
                   'Omegad')
            for i in cvt:
                ephm[i] *= gps.GPS_PI

            SatPos(ephm, gps_tow)

if options.test:
    # SatPos() tests
    # lat 0.000000 lon 0.000000 altHAE 20175272.000
    ephm = {
        'af0': 0,
        'af1': 0,
        'af2': 0,
        'AODO': 0,
        'Cic': 0,
        'Cis': 0,
        'Crc': 0,
        'Crs': 0,
        'Cuc': 0,
        'Cus': 0,
        'deltan': 0,
        'e': 0,         # nominal 0
        'FIT': 0,
        'hlth': 0,
        'i0': 0,        # nominal 55.0 degrees +/- 3
        'IDOT': 0,
        'IODC': 53,
        'IODE': 53,
        'L2': 1,
        'L2P': 0,
        'M0': 0,
        'omega': 0,     # nominal 0, +/1 180 degrees
        'Omega0': 0,
        'Omegad': 0,
        'sqrtA': 5153,  # nominal 5153.542538875565
        'Tgd': 0,
        'toc': 0,
        'toe': 0,
        'ura': 0,
        'WN': 75}

    options.debug = 2
    print("\nTest Ephemeris:")
    SatPos(ephm, 0)

    # move 90 degrees
    print("\nTest omega 90:")
    ephm['omega'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move 180 degrees
    print("\nTest omega 180:")
    ephm['omega'] = gps.GPS_PI

    SatPos(ephm, 0)

    # move 270 degrees
    print("\nTest omega 270:")
    ephm['omega'] = gps.GPS_PI * 1.5

    SatPos(ephm, 0)

    # move omega 0, Omega0 90
    print("\nTest Omega 90:")
    ephm['omega'] = 0
    ephm['Omega0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 0, Omega0 180
    print("\nTest Omega 180:")
    ephm['omega'] = 0
    ephm['Omega0'] = gps.GPS_PI

    SatPos(ephm, 0)

    # move omega 90, Omega0 90
    print("\nTest omega 90 Omega0 90:")
    ephm['omega'] = gps.GPS_PI / 2
    ephm['Omega0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 0, Omega0 0, i0 90
    print("\nTest omega 0 Omega0 0 i0 90:")
    ephm['omega'] = 0
    ephm['Omega0'] = 0
    ephm['i0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 45, Omega0 0, i0 90
    print("\nTest omega 45 Omega0 0 i0 90:")
    ephm['omega'] = gps.GPS_PI / 4
    ephm['Omega0'] = 0
    ephm['i0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 45, Omega0 45, i0 90
    print("\nTest omega 45 Omega0 45 i0 90:")
    ephm['omega'] = gps.GPS_PI / 4
    ephm['Omega0'] = gps.GPS_PI / 4
    ephm['i0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # move omega 45, Omega0 0, i0 45
    print("\nTest omega 45 Omega0 0 i0 45:")
    ephm['omega'] = gps.GPS_PI / 4
    ephm['Omega0'] = 0
    ephm['i0'] = gps.GPS_PI / 4

    SatPos(ephm, 0)

    # move omega 90, Omega0 0, i0 90
    # altitude blows up.
    print("\nTest omega 0 Omega0 0 i0 0:")
    ephm['omega'] = gps.GPS_PI / 2
    ephm['Omega0'] = 0
    ephm['i0'] = gps.GPS_PI / 2

    SatPos(ephm, 0)

    # omega Omega0, i0 seem OK.

    # move omega 0, Omega0 0, i0 0
    # t = one quarter sideral day
    siderial_day = 86164.0905
    print("\nTest omega 0 Omega0 0 i0 0:")
    ephm['omega'] = 0
    ephm['Omega0'] = 0
    ephm['i0'] = 0

    SatPos(ephm, siderial_day / 4)

    # move omega 0, Omega0 0, i0 0
    # t = one half sideral day
    # sat has done one orbit, earth 1/2 orbit
    print("\nTest omega 0 Omega0 0 i0 0:")
    ephm['omega'] = 0
    ephm['Omega0'] = 0
    ephm['i0'] = 0

    SatPos(ephm, siderial_day / 2)
